thomas/Cobots
2
1
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Current pain

Browse source 
Works only in cartesian mode
Something about trajectory time being zero again
This commit is contained in:
Thomas Muller 2022-03-22 20:21:28 -04:00
parent 293f603a8b
commit 25a94b161d
4 changed files with 103 additions and 36 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

9
src/ar3_config/config/ar3_controllers.yaml
View file

@ -1,7 +1,16 @@
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
trajectory_execution:
allowed_execution_duration_scaling: 1.2
allowed_goal_duration_margin: 0.5
allowed_start_tolerance: 0.01
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
controller_names:
- joint_trajectory_controller
joint_trajectory_controller:
action_ns: follow_joint_trajectory
type: FollowJointTrajectory

52
src/ar3_config/launch/dev.launch.py
View file

@ -34,7 +34,7 @@ def generate_launch_description():
executable='rviz2',
output='log',
arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')],
parameters=[robot_description, robot_description_semantic, robot_description_kinematics]
parameters=[robot_description, robot_description_semantic, robot_description_kinematics, get_yaml(join(config_root, 'ompl_planning.yaml'))]
),
# Attatch robot to map
@ -50,21 +50,21 @@ def generate_launch_description():
package='move_group_interface',
executable='move_group_interface_node',
output='screen',
parameters=[robot_description, robot_description_semantic, robot_description_kinematics,
get_yaml(join(config_root, 'ompl_planning.yaml')),
get_yaml(join(config_root, 'trajectory.yaml')),
get_yaml(join(config_root, 'planning_scene.yaml')),
get_yaml(join(config_root, "ar3_controllers.yaml")),
{ "default_planning_pipeline": "ompl" },
{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }]
parameters=[robot_description, robot_description_semantic, robot_description_kinematics]
#get_yaml(join(config_root, 'ompl_planning.yaml')),
#get_yaml(join(config_root, 'trajectory.yaml')),
#get_yaml(join(config_root, 'planning_scene.yaml')),
#get_yaml(join(config_root, "ar3_controllers.yaml")),
#{ "default_planning_pipeline": "ompl" },
#{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }]
),
# Our joystick node
Node(
package='joystickmove',
executable='move',
output='screen',
),
#Node(
# package='joystickmove',
# executable='move',
# output='screen',
#),
# Create joystick controller and ComposableNodes for servo_server
ComposableNodeContainer(
@ -73,18 +73,18 @@ def generate_launch_description():
package="rclcpp_components",
executable="component_container",
composable_node_descriptions=[
ComposableNode(
package="moveit_servo",
plugin="moveit_servo::ServoServer",
name="servo_server",
parameters=[
{'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))},
robot_description,
robot_description_semantic,
robot_description_kinematics,
],
extra_arguments=[{"use_intra_process_comms": True}],
),
#ComposableNode(
# package="moveit_servo",
# plugin="moveit_servo::ServoServer",
# name="servo_server",
# parameters=[
# {'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))},
# robot_description,
# robot_description_semantic,
# robot_description_kinematics,
# ],
# extra_arguments=[{"use_intra_process_comms": True}],
#),
ComposableNode(
package="joy",
plugin="joy::Joy",
@ -134,7 +134,7 @@ def generate_launch_description():
get_yaml(join(config_root, 'trajectory.yaml')),
get_yaml(join(config_root, 'planning_scene.yaml')),
get_yaml(join(config_root, "ar3_controllers.yaml")),
{ "default_planning_pipeline": "move_group" },
{ "default_planning_pipeline": "ompl" },
{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }
],
),

22
src/cobot_platform/cobot_platform/move.py
View file

@ -9,13 +9,14 @@ from math import pi
from serial import Serial
J1_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 10 * 4
J2_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
J3_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
J4_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (13 + 212 / 289) * 2.8
J5_COUNTS_PER_RAD = 1 / (2 * pi) * 800 * 10
J6_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (19 + 38 / 187)
class CobotMove(Node):
J1_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 10 * 4
J2_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
J3_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
J4_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (13 + 212 / 289) * 2.8
J5_COUNTS_PER_RAD = 1 / (2 * pi) * 800 * 10
J6_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (19 + 38 / 187)
def __init__(self):
super().__init__('cobot_move')
@ -30,7 +31,7 @@ class CobotMove(Node):
self._cur = [0.0] * 6
# create a timer to update all the joint angles at once
self._timer = self.create_timer(1 / 20, self._dosomething)
self._timer = self.create_timer(1 / 40, self._dosomething)
# Set joint angles independantly
base_topic = 'move/'
@ -91,6 +92,13 @@ class CobotMove(Node):
e = float(blocks[4][1:]) / J5_COUNTS_PER_RAD
f = float(blocks[5][1:]) / J6_COUNTS_PER_RAD
a = float(self._cur[0] / J1_COUNTS_PER_RAD)
b = float(self._cur[1] / J2_COUNTS_PER_RAD)
c = float(self._cur[2] / J3_COUNTS_PER_RAD)
d = float(self._cur[3] / J4_COUNTS_PER_RAD)
e = float(self._cur[4] / J5_COUNTS_PER_RAD)
f = float(self._cur[5] / J6_COUNTS_PER_RAD)
# Publish those to the update/jX subscribers
self.s_j1.publish(Float32(data=a))
self.s_j2.publish(Float32(data=b))

56
src/move_group_interface/src/move_group_interface_node.cpp
View file

@ -15,6 +15,9 @@
#include <moveit/common_planning_interface_objects/common_objects.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/rdf_loader/rdf_loader.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "geometry_msgs/msg/pose_stamped.hpp"
@ -23,18 +26,27 @@ const std::string PLANNING_GROUP = "ar3_arm";
class ScanNPlan : public rclcpp::Node {
public:
ScanNPlan() : Node("scan_n_plan") {
callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
rclcpp::SubscriptionOptions options;
options.callback_group = callback_group_;
// Create /move_pose subscriber
pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"move_pose",
rclcpp::QoS(1),
std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1)
std::bind(&ScanNPlan::moveCallbackHowItShouldWork, this, std::placeholders::_1),
options
);
/*
relative_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"move_relative_pose",
rclcpp::QoS(1),
std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1)
std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1),
options
);
*/
}
void init() {
@ -46,6 +58,39 @@ private:
void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
RCLCPP_INFO(this->get_logger(), "GOT MSG");
move_group_->setStartStateToCurrentState();
move_group_->setPlanningTime(20);
move_group_->setNumPlanningAttempts(50);
move_group_->setMaxVelocityScalingFactor(1.0);
move_group_->setMaxAccelerationScalingFactor(1.0);
std::vector<geometry_msgs::msg::Pose> waypoints;
waypoints.push_back(p->pose);
moveit_msgs::msg::RobotTrajectory trajectory;
double fraction = move_group_->computeCartesianPath(waypoints, 0.01, 0.0, trajectory, true);
if(fraction >= 1.0) {
RCLCPP_INFO(this->get_logger(), "Did the carts %f%%", fraction * 100.);
robot_trajectory::RobotTrajectory rt(move_group_->getRobotModel(), move_group_->getName());
rt.setRobotTrajectoryMsg(*move_group_->getCurrentState(10), trajectory);
trajectory_processing::IterativeParabolicTimeParameterization iptp;
bool success =
iptp.computeTimeStamps(rt, 0.1, 0.1);
RCLCPP_INFO(this->get_logger(), "Got stamps %s", success ? "YES" : "NO");
moveit::planning_interface::MoveGroupInterface::Plan plan;
rt.getRobotTrajectoryMsg(plan.trajectory_);
move_group_->execute(plan);
}
//move_group_->move();
}
void moveCallbackHowItShouldWork(geometry_msgs::msg::PoseStamped::SharedPtr p) {
RCLCPP_INFO(this->get_logger(), "GOT MSG");
move_group_->setStartStateToCurrentState();
move_group_->setPoseTarget(p->pose);
moveit::planning_interface::MoveGroupInterface::Plan plan;
@ -78,6 +123,8 @@ private:
move_group_->move();
}
private:
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr relative_pose_sub_;
@ -91,8 +138,11 @@ int main(int argc, char* argv[]) {
auto node = std::make_shared<ScanNPlan>();
RCLCPP_INFO(node->get_logger(), "MoveGroup node started.");
rclcpp::executors::MultiThreadedExecutor executor;
node->init();
rclcpp::spin(node);
executor.add_node(node);
executor.spin();
// rclcpp::spin(node);
RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup");
rclcpp::shutdown();