Rosanswers logo

I am having a confusing issue when trying to set pose targets wrt a frame other than the chain's end effector link. I can set and get the poses specifying the end_effector_link argument successfully but when I call move(), I get errors to the effect of:

[ INFO] [1467279632.377325997]: Position constraint violated on link 'my_frame'. Desired: -0.868717, -0.024240, 0.347688, current: -0.897177, -0.027324, 0.220500
[ INFO] [1467279632.377386843]: Differences 0.0284601 0.00308382 0.127189
[ INFO] [1467279632.377426372]: Orientation constraint satisfied for link 'my_frame'. Quaternion desired: -0.001033 -0.015521 -0.704873 0.709163, quaternion actual: -0.000985 -0.015375 -0.705119 0.708921, error: x=0.000146, y=0.000269, z=0.000691, tolerance: x=0.001000, y=0.001000, z=0.001000

The confusing issue is that "current: -0.897177, -0.027324, 0.220500" makes no sense in the context of any of my frames that I can figure and furthermore, does NOT match the values read back from a call to getCurrentPose("my_frame").

In an attempt to isolate my issue I have tried following two cases, the first of which works and the latter that fails: Works --

  geometry_msgs::PoseStamped debug = movegroup_.getCurrentPose();
  debug.pose.position.x = debug.pose.position.x + 0.01;

Fails as mentioned above --

  geometry_msgs::PoseStamped debug = movegroup_.getCurrentPose("my_frame");
  debug.pose.position.x = debug.pose.position.x + 0.01;
  movegroup_.setPoseTarget(debug.pose, "my_frame");

Interestingly, trying one more case... if I remove the line

debug.pose.position.x = debug.pose.position.x + 0.01;

and send an unmodified current pose of my_frame as the pose target I get a success in the form of "Goal constraints are already satisfied. No need to plan or execute any motions"

This leads me to believe there is a rotational and/or reference frame discrepancy somewhere... am I missing something?

Originally posted by BrettHemes_ on ROS Answers with karma: 21 on 2016-06-30

Post score: 1


1 Answer 1


Rosanswers logo

Not an answer but I can confirm that moveGroup.setEndEffectorLink() is probably not working right.

Console output from moveGroup says it is using frame 'leap_motion_on_robot':

Attempting to follow 1 waypoints for link 'leap_motion_on_robot'...

moveGroup.getCurrentPose() always returns a pose in the /base_link frame.

I set it to use a different EE frame:


Similar to what you did, I get the current pose. I've then tried converting the current pose to different frames and commanding them as the new EE pose. I expect it to report back that it's already at that pose within specified tolerances. Instead, it moves to a completely new pose. For example,

  // Current pose in base_link
  currentPose = moveGroup.getCurrentPose();  // Always returns in base_link
  ROS_INFO_STREAM( "Current pose from moveGroup:  " << currentPose );

  // Current pose in camera_ee_link
  // Remove the leading "/" for tf2
  if ( currentPose.header.frame_id.at(0) == '/' )

  listener.waitForTransform( "camera_ee_link", currentPose.header.frame_id, ros::Time(0), ros::Duration(10.0) );
    geometry_msgs::TransformStamped tf_to_camera_ee_link_frame = tfBuffer.lookupTransform("camera_ee_link", currentPose.header.frame_id, ros::Time(0), ros::Duration(1.0) );
    tf2::doTransform(currentPose, currentPose, tf_to_camera_ee_link_frame);
  catch(tf2::TransformException ex){
    return false;
  ROS_INFO_STREAM( "Current pose in camera_ee_link:  " << currentPose );

  // Test with currentPose:
  waypoints.push_back(currentPose.pose); // Sending it in base_link. leap_motion_on_robot, and camera_ee_link all fail.
  fraction = move_base_to_manip::cartesian_motion(waypoints, trajectory, moveGroup, nh);
  ROS_INFO("Cartesian path: %.2f%% achieved", fraction * 100.);

The 3 frames I have tried are base_link, leap_motion_on_robot, and camera_ee_link. Basically, any frame that makes any sense whatsoever. It moves to 3 different poses, but none of them is the current pose.

Github issue: https://github.com/ros-planning/moveit/issues/522

Originally posted by AndyZe with karma: 2331 on 2017-06-08

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by BrettHemes on 2017-06-09:
Interesting... this was a long time ago for me but I think I just ended up doing the appropriate transformation on my side so that everything was relative to the default end effector link. At least I am not the only one having issues :/


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.