move_group.move vs cartesian paths

asked 2019-09-11 07:19:36 -0500

thanasis_t gravatar image

Hello,

I am trying to move the end effector of a robotic manipulator through a series of waypoints using MoveIt!. At first I used the move_group.move() command to iterate through each point. The code I used is shown below:

ros::init(argc, argv, "manos_openpose");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
moveit::planning_interface::MoveGroupInterface move_group("arm");

move_group.setPoseReferenceFrame("world");

geometry_msgs::Pose currentPose = move_group.getCurrentPose().pose;

geometry_msgs::Pose target_pose = currentPose;
target_pose.position.z += 0.1;
move_group.setPoseTarget(target_pose);
move_group.move();

target_pose.position.z -= 0.1;
move_group.setPoseTarget(target_pose);
move_group.move();

ros::waitForShutdown();

Everything works fine. Then i tried to replicate the same movement by computing a cartesian path passing through these points. The code i used is shown below:

ros::init(argc, argv, "manos_openpose");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
moveit::planning_interface::MoveGroupInterface move_group("arm");

move_group.setPoseReferenceFrame("world");

geometry_msgs::Pose initState = move_group.getCurrentPose().pose;
std::vector<geometry_msgs::Pose> waypoints;

geometry_msgs::Pose target_pose = initState;
target_pose.position.z += 0.1;
waypoints.push_back(target_pose);

target_pose.position.z -= 0.1;
waypoints.push_back(target_pose);

moveit::planning_interface::MoveGroupInterface::Plan my_plan;
moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.001;
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);

my_plan.trajectory_ = trajectory;
move_group.execute(my_plan);

However the function computeCartesianPath() returns a value between 0.3 and 0.6 (different value each time i run the code). Hence, MoveIt! executes only the portion of the trajectory corresponding to the returned value of the above mentioned function. Any ideas why it won't execute the whole trajectory since the waypoints are valid? Thanks a lot in advance and let me know if there is any other information i could provide.

edit retag flag offensive close merge delete