Ask Your Question
0

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

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-03-02 04:58:10 -0500

fvd gravatar image

This can happen if you are around singularities, or areas where your IK flips orientation. To check if that is the problem, you could try the same code starting from a different pose, ideally somewhere where the goal state in Rviz looks "stable". Otherwise, your waypoints might not be very good.

This is a bug that should be fixed in the future, but some workarounds for now:

  • Use tighter waypoints and fall back to move_group.move() if the cartesian path planning fails
  • Use named poses that are safe to move to, if you can
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

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

Seen: 352 times

Last updated: Mar 02