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?
↧