Moveit low precision when executing cartesian path

edit

Hi,

I am using MoveIt with Baxter Robot on Gazebo7 to create a circular trajectory of 500 waypoints (running ROS Kinetic on Ubuntu 16.04 and everything was installed according to Rethink Robotics webpage http://sdk.rethinkrobotics.com/wiki/B... )

To make the cartesian path I have followed the tutorial on http://docs.ros.org/kinetic/api/movei... There is no problem when creating the trajectory, however I would expect higher precision on execution. In the image you can see in blue the desired trajectory and in red the actual execution.

Once I have the circle coordinates stored as waypoints I compute and execute the trajectory as follows:

move_group.setMaxVelocityScalingFactor(1.0);
move_group.setGoalTolerance(0.001);

moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 1.0;
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
moveit::planning_interface::MoveGroupInterface::Plan my_plan2;
my_plan2.trajectory_ = trajectory;
move_group.execute(my_plan2);
ROS_INFO("Cartesian path (%.2f%% achieved)", fraction * 100.0);

The computeCartesianPath() function returns a 100% of achievement for the trajectory and also the MoveIt terminal displays the message "Computed Cartesian path with 501 points (followed 100.000000% of requested trajectory)". I have tried decreasing the GoalTolerance parameter of the MoveGroupInterface object but the issue still remains (even setting the GoalTolerance to 0.0 a 100% of achievement is reached but the executed trajectory is not close to being the desired one).

Are there any other parameters that may affect the precision of the trajectory execution? Any ideas on why this is happening and how it could be fixed?

I have tried doing the trajectory with and without PathConstraints, I have modified the eef_step and jump_threshold too just in case that would do it but I can't find a solution. Having a good trajectory is key for our research, any help will be very much appreciated. Please do not hesitate asking for more details if needed.

Thanks

Ignacio