Limit tool point windup for pick points with symmetry

[shaun-edwards]

In a pick and place application, the pick point orientation and place orientation (at least relative to the pick) are often very important. In some cases, the pick orientation can cause the robot to windup it's wrist joints. This slows down motion and can cause problems with cables or hoses. In cases where the pick point has some symmetry this windup is completely unnecessary. All that is required is that the place orientation is correctly calculated relative to the pick in such a way that the place orientation is correct. Below are some use cases that illustrate the requirements:

• Square end-effector (symmetric about both axes) - In this case the pick can occur at any 90 degree increment from the desired pick point. The pick orientation will have to be accounted for in the place
• Rectangular end-effector (symmetric about one axes) - In this case the pick can occur at any 180 degree increment from the desired pick point. The pick orientation will have to be accounted for in the place
• Single point (i.e. single suction cup) - In this case the pick can occur at any angle from the desired pick point. The pick orientation will have to be accounted for in the place.

[shaun-edwards]

May be related to #77

[henningkayser]

Hi @shaun-edwards,
working with a robot that has cables attached is always a bit tricky. To prevent winding up the cables until they get stretched there should always be appropriate joint limits applied. In simple cases it can be enough to limit the wrist joint to prevent joint flipping. More general scenarios require some evaluation on what joint states are generated for the pick/place poses.

This involves generating and comparing multiple IK solutions. Trac_IK can do this implicitly by using the solve_type Distance together with a suitable seed state. Symmetric grasp poses would need to be sampled depending on the end-effector type. A more flexible (and probably faster) solution would be an optimizing IK solver like for instance BioIK. The solver supports minimizing specific joint value distances in combination with position / orientation target sets and also symmetries around a rotation axes.

Basic pick capabilities are provided by the grasp generator framework moveit_grasps. The framework computes randomized grasp motions and scores the results for certain geometric measures taking into account an ideal pick orientation. Solutions that cause windup can be filtered from the result set or could be disabled by applying mentioned IK solvers.

For a valid pick joint state the place state can be computed using the same IK solver as well. Of course the pick needs to be rejected if no valid place is possible.
An approach to streamline this is to apply the multi-stage planner MoveIt Task Constructor which allows structuring the pick/place movement into separate planning steps that are sampled and connected automatically.

[shaun-edwards]

@henningkayser, I appreciate the pointers to the MoveIt libraries that support pick and place applications.

• I see that the task constructor is relatively new and unstable. Is this true?
• I have used the grasp generator before and I think it is more powerful when applied to articulated grasping. We typically use suction based grippers. Do you think there is functionality in grasp generating/planning that would be useful to leverage with a suction based gripper?

Based on our conversation last week, I think we agreed that we would use the optimizing IK solver to select a pick point that "minimized" the potential wind-up.

The only open question was how this work integrated with #77. The simple, but perhaps wrong, answer is that we use the solution to this issue to generate the joint point for the "pick" grasp. The downside to this approach is that the orientation of the actual pick relative to the picked item is not obvious...please correct me if I am wrong.

[henningkayser]

I see that the task constructor is relatively new and unstable. Is this true?

As it's still in development there can and will probably be some API changes in the future.
Since all of the planning code is encapsulated in separate task components the maintenance effort for MTC changes is limited. I've already worked with it on different occasions and have produced and seen well functioning applications so I'm very positive about the qualities.

I have used the grasp generator before and I think it is more powerful when applied to articulated grasping. We typically use suction based grippers. Do you think there is functionality in grasp generating/planning that would be useful to leverage with a suction based gripper?

We've recently added support for suction grippers and already applied it in other projects.
The upside are more robust grasping success (since multiple grasp poses are sampled and scored) and planning of approach, retreat and lift motions. End effector symmetries could be added as a feature of the grasp generator. We might have to implement support for additional object shapes, though.

Based on our conversation last week, I think we agreed that we would use the optimizing IK solver to select a pick point that "minimized" the potential wind-up.

This is definitely a good place to start! I will prepare a setup using BioIK. Alternatively, we could also try and implement a version that uses rejection sampling and a "normal" IK solver.

The only open question was how this work integrated with #77. The simple, but perhaps wrong, answer is that we use the solution to this issue to generate the joint point for the "pick" grasp. The downside to this approach is that the orientation of the actual pick relative to the picked item is not obvious...please correct me if I am wrong.

I'm not sure I understand this correctly. I would say that we have to compute the pick pose and joint state and then run a (constrained) planner to reach that target. It's also not a problem to determine the relative orientation between object and gripper. The only challenge I see is that we would have to reject some pick solutions depending on what constraints should be used for planning. For instance if we want to implement some "elbow up" constraint then we need to apply the same constraint for grasping.

[davetcoleman]

I see that the task constructor is relatively new and unstable. Is this true?

I don't think this is any less stable than all of MoveIt, in the sense that we are always aiming to make the project better. Considering they published a paper on the MTC, I'd say its pretty usable. I've addressed your concerns with a fix to the README

Henning's other responses make sense to me, here.