Issue using moveit::planning_interface::MoveGroup::setPoseTarget with non-empty end_effector_link

asked 2016-06-30 10:06:18 -0500

BrettHemes_
21 ●2 ●4 ●4

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;
  movegroup_.setPoseTarget(debug.pose);
  movegroup_.move();

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");
  movegroup_.move();

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?

answered 2017-06-08 12:41:56 -0500

AndyZe

2331 ●38 ●106 ●70

updated 2017-06-08 13:08:55 -0500

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:

moveGroup.setEndEffectorLink("camera_ee_link");

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) == '/' )
    currentPose.header.frame_id.erase(0,1);

  listener.waitForTransform( "camera_ee_link", currentPose.header.frame_id, ros::Time(0), ros::Duration(10.0) );
  try{
    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){
    ROS_ERROR("%s",ex.what());
    return false;
  }
  ROS_INFO_STREAM( "Current pose in camera_ee_link:  " << currentPose );

  // Test with currentPose:
  waypoints.clear();
  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.);
  moveGroup.move();

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/movei...

edit flag offensive delete link

Comments

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 :/

BrettHemes ( 2017-06-09 09:20:25 -0500 )edit

add a comment

Issue using moveit::planning_interface::MoveGroup::setPoseTarget with non-empty end_effector_link

1 Answer

Comments

Question Tools

Stats

Related questions

Issue using moveit::planning_interface::MoveGroup::setPoseTarget with non-empty end_effector_link edit

1 Answer

Comments

Question Tools

Stats

Related questions

Issue using moveit::planning_interface::MoveGroup::setPoseTarget with non-empty end_effector_link