Revision history [back]

Hi Unais,

For starters: http://answers.ros.org/question/74776/cartesian-controller-for-ros/

Constructing a pose message:

#include <tf/transform_broadcaster.h>

// translate roll, pitch and yaw into a Quaternion
double roll     = 0.0;
double pitch    = 0.0;
double yaw      = 1.5707;
tf::Quaternion q;
q.setRPY(roll, pitch, yaw);
geometry_msgs::Quaternion odom_quat;
tf::quaternionTFToMsg(q, odom_quat);

geometry_msgs::Pose pose;
pose.position.x  = 0.0;
pose.position.y  = 0.15;
pose.position.z  = 0.40;
pose.orientation = odom_quat;

You have to push the different poses into a list and pass them to computeCartesianPath(), example:

std::vector< geometry_msgs::Pose > poses;
moveit_msgs::ExecuteKnownTrajectory srv;
srv.request.wait_for_execution = true;
ros::ServiceClient executeKnownTrajectoryServiceClient = node_handle.serviceClient<moveit_msgs::ExecuteKnownTrajectory>("/execute_kinematic_path");

poses.push_back(pose.pose);
poses.push_back(pose2.pose);
poses.push_back(pose3.pose);
poses.push_back(pose4.pose);

group.computeCartesianPath(poses, 0.005, 1000.0, srv.request.trajectory);
executeKnownTrajectoryServiceClient.call(srv);

'eef_step' is the step size used to generate the intermediate points, the 'jump_threshold' is explained here (see last reply): Linear gripper movement. Passing 0.0 for jump_treshold effectively disables the jump threshold test.

It is also possible to specify orientation and position path constraints using the move_group interface. I cannot find the link a.t.m. because ros.org is down for me.

Example of an orientation constraint:

moveit_msgs::Constraints path_constraints = moveit_msgs::Constraints(); 
path_constraints.name = "orientation_constraint";

moveit_msgs::OrientationConstraint orientation_constraint = moveit_msgs::OrientationConstraint();
orientation_constraint.header.frame_id="base_link_1";
orientation_constraint.orientation.x = x;
orientation_constraint.orientation.y = y;
orientation_constraint.orientation.z = z;
orientation_constraint.orientation.w = w;
orientation_constraint.orientation = cur_pose.pose.orientation;
orientation_constraint.absolute_x_axis_tolerance = 0.5;
orientation_constraint.absolute_y_axis_tolerance = 0.5;
orientation_constraint.absolute_z_axis_tolerance = 0.5;
orientation_constraint.weight = 10.0f;
orientation_constraint.link_name = "gripper_link_1";
path_constraints.orientation_constraints.push_back(orientation_constraint);

group.setPathConstraints(path_constraints);

Hi Unais,

For starters: http://answers.ros.org/question/74776/cartesian-controller-for-ros/

Constructing a pose message:

#include <tf/transform_broadcaster.h>

// translate roll, pitch and yaw into a Quaternion
double roll     = 0.0;
double pitch    = 0.0;
double yaw      = 1.5707;
tf::Quaternion q;
q.setRPY(roll, pitch, yaw);
geometry_msgs::Quaternion odom_quat;
tf::quaternionTFToMsg(q, odom_quat);

geometry_msgs::Pose pose;
pose.position.x  = 0.0;
pose.position.y  = 0.15;
pose.position.z  = 0.40;
pose.orientation = odom_quat;

You have to push the different poses into a list and pass them to computeCartesianPath(), example:

std::vector< geometry_msgs::Pose > poses;
moveit_msgs::ExecuteKnownTrajectory srv;
srv.request.wait_for_execution = true;
ros::ServiceClient executeKnownTrajectoryServiceClient = node_handle.serviceClient<moveit_msgs::ExecuteKnownTrajectory>("/execute_kinematic_path");

poses.push_back(pose.pose);
poses.push_back(pose2.pose);
poses.push_back(pose3.pose);
poses.push_back(pose4.pose);

group.computeCartesianPath(poses, 0.005, 1000.0, srv.request.trajectory);
executeKnownTrajectoryServiceClient.call(srv);

'eef_step' is the step size used to generate the intermediate points, the 'jump_threshold' is explained here (see last reply): Linear gripper movement. Passing 0.0 for jump_treshold effectively disables the jump threshold test.

It is also possible to specify orientation and position path constraints using the move_group interface. I cannot find the link a.t.m. because ros.org is down for me.

Example of an orientation constraint:

moveit_msgs::Constraints path_constraints = moveit_msgs::Constraints(); 
path_constraints.name = "orientation_constraint";

moveit_msgs::OrientationConstraint orientation_constraint = moveit_msgs::OrientationConstraint();
orientation_constraint.header.frame_id="base_link_1";
orientation_constraint.orientation.x = x;
orientation_constraint.orientation.y = y;
orientation_constraint.orientation.z = z;
orientation_constraint.orientation.w = w;
orientation_constraint.orientation = cur_pose.pose.orientation;
orientation_constraint.absolute_x_axis_tolerance = 0.5;
orientation_constraint.absolute_y_axis_tolerance = 0.5;
orientation_constraint.absolute_z_axis_tolerance = 0.5;
orientation_constraint.weight = 10.0f;
orientation_constraint.link_name = "gripper_link_1";
path_constraints.orientation_constraints.push_back(orientation_constraint);

group.setPathConstraints(path_constraints);