How to place an object in a pose ignoring orientation

[Jorge Santos Simón]

Hi, I'm playing around with MoveIt! pick and place capability with a tuertblot_arm. Pick works fine, as I can specify the gripper orientation according with the limited dofs of the arm (only 4)

But for place, I can only specify the pose (using Python moveit_commander), so I'm supposed to provide the pose where I'll place the object, including its orientation. This is not a problem if I picked the object just in front of the arm, as it keeps aligned with the x-axis (and the arm) once picked (see picture one). But if picked misaligned (that is. with positive or negative y-axis value relative to the arm base), it appears "rotated" in the gripper (see picture two), and place always fails due to orientation constraints violated (see log below). I have tried to set a high tolerance with set_goal_orientation_tolerance, but nothing changed. In fact, the IK solver use always the same tolerances (highlighted in blue in the log), so must be different parameters.

[ WARN] [1412349544.875206350]: All supplied place locations failed. Retrying last location in verbose mode.

[ INFO] [1412349544.875286078]: Re-added last failed plan for pipeline 'place'. Queue is now of size 1

[ INFO] [1412349544.876071434]: Orientation constraint violated for link 'gripper_link'. Quaternion desired: 0.000013 0.000286 -0.043728 0.999043, quaternion actual: -0.001258 0.003698 -0.324719 0.945802, error: x=0.000661, y=0.007212, z=0.573948, tolerance: x=0.010000, y=0.010000, z=0.010000

[ INFO] [1412349544.876440473]: Orientation constraint violated for link 'gripper_link'. Quaternion desired: 0.000013 0.000286 -0.043728 0.999043, quaternion actual: 0.004062 -0.002497 -0.323064 0.946365, error: x=0.006693, y=0.007365, z=0.570427, tolerance: x=0.010000, y=0.010000, z=0.010000

[ INFO] [1412349544.876782882]: Orientation constraint violated for link 'gripper_link'. Quaternion desired: 0.000013 0.000286 -0.043728 0.999043, quaternion actual: 0.002439 -0.002318 -0.323024 0.946385, error: x=0.003628, y=0.006239, z=0.570353, tolerance: x=0.010000, y=0.010000, z=0.010000

[ INFO] [1412349544.876802548]: Sampler failed to produce a state

[ INFO] [1412349544.876816458]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0

The last comment from Ioan on this thread seems to explain the issue, but I cannot find a solution. It would be fine if I could just ignore the orientation of the place pose, as anyway the turtlebot_arm cannot choose one. Another approach could be to add the other place methods to the Python interface, but A) no idea if I'll have the same problem passing a moveit_msgs::PlaceLocation instead of a Pose  B) I'm not confident enough with MoveIt! code to make changes on it.

Btw, I'm using MoveIt! Hydro from sources.

Thank you very much!

[Martin Günther]

> <https://groups.google.com/forum/#!topic/moveit-users/P2V9eW5BjW8>

> seems to explain the issue, but I cannot find a solution. It would be
> fine if I could just ignore the orientation of the place pose, as
> anyway the turtlebot_arm cannot choose one. Another approach could be
> to add the other place methods to the Python interface, but A) no
> idea if I'll have the same problem passing a
> moveit_msgs::PlaceLocation instead of a Pose B) I'm not confident
> enough with MoveIt! code to make changes on it.
>
> Btw, I'm using MoveIt! Hydro from sources.
>
> Thank you very much!
>

> <https://lh4.googleusercontent.com/-TvavEuV2Sj4/VC7EyEyt1nI/AAAAAAAAASE/vjLPPw1LuA0/s1600/img1.png>
>
> <https://lh3.googleusercontent.com/-kcQpS-LRNO8/VC7E2MXs1jI/AAAAAAAAASM/wpCUrWyJ3hs/s1600/img2.png>
>


Hi Jorge,

you could try generating an IKFast IK (see the tutorial on the MoveIt
website) which ignores the orientation; something like IK type
"Translation3D". Then you could just pass an arbitrary orientation, and
IK would find a feasible solution for you just based on the position
element of the pose.

Cheers,
Martin

[Jorge Santos Simón]

Hi Martin,

Thanks, it's a good advice, but problematic to apply by now, because as explained here, openrave hasn't been upgraded for indigo/14.04, so I cannot regenerate the plugin without reverting my linux/ROS installation. Or can I, and my information is obsolete?

In fact, the ideal would be to ignore roll and yaw, that are constrained by the robot geometry, but not pitch. Like that, I can keep some flexibility in the grasping direction, as some small objects are better grasped from above, but "tall" ones are with an horizontal gripper approach.

Thanks!

[Martin Günther]

Hi Jorge,

On Mon, 6 Oct 2014 10:57:39 +0200
Jorge Santos Simón <jsanto...@gmail.com> wrote:

> Hi Martin,
>
> Thanks, it's a good advice, but problematic to apply by now, because
> as explained here

> <https://github.com/turtlebot/turtlebot_arm/issues/12>, openrave

> hasn't been upgraded for indigo/14.04, so I cannot regenerate the
> plugin without reverting my linux/ROS installation. Or can I, and my
> information is obsolete?

No, you're right, the PPA for OpenRAVE doesn't have 14.04 binaries
(only up to precise). You could either install it from source, or find
a 12.04 system to generate your plugin. You only need OpenRAVE on the
machine where you generate the plugin; once the ROS package is generated,
you can compile it anywhere without having to have OpenRAVE.

> In fact, the ideal would be to ignore roll and yaw, that are
> constrained by the robot geometry, but not pitch. Like that, I can
> keep some flexibility in the grasping direction, as some small
> objects are better grasped from above, but "tall" ones are with an
> horizontal gripper approach.

This is exactly what I think the IKFast IK type "TranslationZAxisAngle4D" does:

"TranslationXAxisAngle4D, TranslationYAxisAngle4D, TranslationZAxisAngle4D
- end effector origin reaches desired 3D translation, manipulator direction
makes a specific angle with x/y/z-axis (defined in the manipulator
base link’s coordinate system)"

(from http://openrave.org/docs/latest_stable/openravepy/ikfast/)

I had to read that 5 times to process, but I think it's exactly what you
want: it lets you specify position and pitch, but ignores roll and yaw.

Hmmm... after writing all this, I just saw that you're a contributor to
the turtlebot_arm github repo, and it already uses an IKFast plugin. So...
what's wrong with that?

I also just found a message by Fergs where he said he's using a
TranslationDirection5D IK type with a fake joint there, so maybe the
TranslationZAxisAngle4D approach didn't work for him.

Cheers,
Martin

[Jorge Santos Simón]

Yes, we use an IKFast plugin, but I just inherited it from Fergs and updated it (without regenerating) to compile on indigo and latest moveit, so I know nearly nothing about the internals. But it seems like the IK restricts the orientation in all three angles. I'll attract Ferg's attention to this thread so maybe he can provide any insight.

Thanks again!

Jorge

[Michael Ferguson]

Jorge,

The TranslationZAxisAngle4D might be the right IK solution, in that it allows you to remove the fake axis I added. However, I think you might be chasing the wrong demon in the code when it comes to pick and place. I think you're finding additional checks that the pick and place is doing that aren't happening in the regular move_group action.

Looking at your output (in the first email in this chain), those messages are not coming from the IK (and I don't see any message from IK, if you tell it to use a place location say 10m away from the robot, do you get different output, I would expect an IK error to be showing up in that case). Thus, I think the IK is succeeding, but you then hit the constraint evaluation afterwards (message is generated here: https://github.com/ros-planning/moveit_core/blob/indigo-devel/kinematic_constraints/src/kinematic_constraint.cpp#L626)

I can't quite find the code for the IK constraint sampler that is being used here https://github.com/ros-planning/moveit_ros/blob/indigo-devel/manipulation/pick_place/src/reachable_valid_pose_filter.cpp#L127 but I suspect that the IK is returning a pose, but it does not meet the very stringent orientation requirements. I'm not entirely sure where these constraints come from (unlike the MoveGroup action, we don't specify them directly, instead pick/place specify just poses for the object).

-Fergs

[Jorge Santos Simón]

Hi Michael, and first of all thanks !

Yes, I think you are right. But those are rather bad news, as I have no idea about how to change the behavior of the kinematic_constraint. Looking at the move_group interface, the orientation tolerance passed with setGoalOrientationTolerance only affects pose goals, but not joint (obviously) nor pickup and place goals. I wonder if this should not be fixed, as I cannot see while pickup and place goals must use the default (rather strict) tolerance of 0.01 radians.

That said, I found a horrible workaround that seems to work, but please!!! don't give this post as solved, as it's a terrible and brittle hack! What I do is to store the yaw orientation I used on pickup,

and subtract it to the place yaw. This works because the picked object "carries" with him the orientation of the arm at pickup time. Admittedly, quite confusing. I post a couple of pictures to better

illustrate what's going on:

Look at the arm orientation at pickup; the object, however, is oriented with the x axis 

At place, I "overturn" the place pose ​the arm orientation at pickup

[Jorge Santos Simón]

I'm really puzzled with place operation behavior....  My trick only works if the target object is very narrow, like 

        target_size = [0.02, 0.005, 0.12]

If I try to place a wider object. as 0.02 x 0.02 x 0.12, it always fails! No idea how this relates to the rotation constrain violated that prevents the place operation to work.

[Jorge Santos Simón]

Coming back over this....

Given nobody implemented the changes required on moveit to ignore the rotation, and that I cannot still remake the IK plugin, I found a tricky but operative solution: revert this line of the place.cpp code. I do this multiplying the place goal pose by the attached collision object pose.

I renamed the moveit_ros issue accordingly.

[Jorge Santos Simón]

Nobody seems to have the same problem I have... but anyway, here is the code that does the trick, well self-explained, I hope