[Story] Rethink/refactor grasping pipeline

[LarsJanssenTUe]

Currently the grasping pipeline is not always as robust as one might hope (although it is quite okay) and it is very slow and inefficient. Any thoughts on whether it is smart to do this/how to do this?

[PetervDooren]

I think that where we lose most robustness is due to the fact that we no longer update the pose of the target entity once we start moving. If this movement includes driving then our accuracy takes a nosedive. Currently we solve this by not driving after doing the final inspect.
Doing some sort of visual servoing could solve this.

I dont think there are many steps we can cut from this process. As I recall picking up an (unknown) object uses the following steps

• drive towards the table to inspect
• inspect the table and determine whether the object is there
• drive towards a position where we can pick up the object
• update the position of the object
• move the gripper to the object
• close gripper
• move the arm back

One think we should look at is our infamous pre-grasp. This thing only works for objects that are table height. Higher or lower can give problems. Either we should make the pre-grasp height adjustable based on the height of the object we are grasping or we throw out the pre-grasp altogether in favor of something like moveit which can do obstacle avoidance.

Finally the way we decide to position for a grasp is lacking. We have a set distance we want to be from the object but again we tuned this for an object on table height. When grasping objects which are higher or lower the robot may position itself in such a way as to place the object outside of its own workspace. This also happened in Sydney on one occasion. I believe issue #934 refers to this. We could improve this positioning using some inverse kinematics

[LoyVanBeek]

To summarize @PetervDooren's reply :-)"

• Visual servoing. This requires
  • track object
  • track hand. For AMIGO, we had an AR-marker on the hand for this purpose.
• Calculate relative grasp position.

Of course, we could use the gripper camera and try to do some depth-from-motion or monocular depth estimation or correlate features seen by the head camera to features seen by the gripper-camera.

[LarsJanssenTUe]

After a discussion with @PetervDooren and @jlunenburg we found a few things that we think will drastically improve the grasping pipeline:

• After the first inspect there is a navigation (navigate_to_grasp). This path is usually relatively simple and short. We want to do this using a ControlToPose sort of thing. This would use the global cost map, but not the local cost map making it more cost effective. On top of this this would allow for the robot to keep a custom pose (in which the arm is sideways and the head is looking directly at the object).

• The prepare_grasp pose is currently not dependent on the height of the object. This should be added to allow for the grasping of high objects. On top of this the pose of the arm should be sideways to allow for doing a second inspect without the arm being in the way. If this is the case the robot can afterwards turn (with relatively high precision) and grasp the object.

TLDR:

• Make a grasp example (a benchmark of sorts) --> Feature/benchmark grasping #984
• Simple navigate using global cost map --> wip: Feature/manipulation control base pose #991
• Custom prepare pose dependent on the object height.
• Make sleeps in inspect robot dependent.
• Renew Rise for inspect

Additional features could be:

• Move the arm in a straight manner in cartesian coordinates.
• Being able to apply a top grasp
• Solve the IK of the robot while knowing the object height to get rid of standard offsets, thereby adding DOF's to the grasping problem and allowing for very low grasps that are currently being blocked.

[jlunenburg]

Head pipeline is not working correctly:

• (measured) joint state topic is not correct (see fix(head)Fixed topics and params for (hero) head ref hero_bringup#42)
• joint names must be configurable (see https://github.com/tue-robotics/head_ref/pull/13/files, fix(head)Fixed topics and params for (hero) head ref hero_bringup#42)
• there's a minus in the manipulation bridge (see https://github.com/tue-robotics/head_ref/pull/13/files, Fix/head neck feedback hero_bridge#13)
• head ref does not take joint limits into account (in progress) (see https://github.com/tue-robotics/head_ref/pull/13/files, Fix/head neck feedback hero_bridge#13)

[jlunenburg]

Concerning low grasping:
With static offset (current situation):

Height: 0.1 --> failed (duration: 12.762)
Height: 0.2 --> failed (duration: 14.745)
Height: 0.3 --> failed (duration: 15.099)
Height: 0.4 --> failed (duration: 19.851)
Height: 0.5 --> failed (duration: 16.674)
Height: 0.6 --> failed (duration: 16.119)
Height: 0.7 --> done (duration: 19.833)
Height: 0.8 --> done (duration: 24.075)
Height: 0.9 --> done (duration: 20.334)
Height: 1.0 --> done (duration: 21.741)
Height: 1.1 --> done (duration: 23.133)
Height: 1.2 --> failed (duration: 17.367)

With 'compute offset' using the manipulation object:

Height: 0.1 --> failed (duration: 16.065)
Height: 0.2 --> failed (duration: 14.1)
Height: 0.3 --> failed (duration: 16.515)
Height: 0.4 --> failed (duration: 15.723)
Height: 0.5 --> failed (duration: 14.34)
Height: 0.6 --> failed (duration: 15.513)
Height: 0.7 --> done (duration: 29.307)
Height: 0.8 --> done (duration: 24.066)
Height: 0.9 --> done (duration: 19.788)
Height: 1.0 --> done (duration: 22.899)
Height: 1.1 --> done (duration: 22.32)
Height: 1.2 --> failed (duration: 18.873)

Reducing the difference between inner and outer radius:

         # Outer radius
-        ro = "(x-%f)^2+(y-%f)^2 < %f^2"%(x, y, radius+0.075)
-        ri = "(x-%f)^2+(y-%f)^2 > %f^2"%(x, y, radius-0.075)
+        ro = "(x-%f)^2+(y-%f)^2 < %f^2"%(x, y, radius+0.025)
+        ri = "(x-%f)^2+(y-%f)^2 > %f^2"%(x, y, radius-0.025)

Height: 0.1 --> done (duration: 25.089)
Height: 0.2 --> done (duration: 24.651)
Height: 0.3 --> done (duration: 18.195)
Height: 0.4 --> done (duration: 24.36)
Height: 0.5 --> done (duration: 20.136)
Height: 0.6 --> done (duration: 22.926)
Height: 0.7 --> done (duration: 18.576)
Height: 0.8 --> done (duration: 19.416)
Height: 0.9 --> done (duration: 19.494)
Height: 1.0 --> done (duration: 21.516)
Height: 1.1 --> done (duration: 21.048)
Height: 1.2 --> failed (duration: 18.24)

[PetervDooren]

Discussions on this topic should be held in #1068

[MatthijsBurgh]

GitHub temporarly disabled to function to convert an issue to a discussion. So keep this issue, until that is possible again.
community/community#2924 (comment)

[PetervDooren]

After our discussion last week this issue still seems to be one of our main challenges. Lets pick this discussion up again here to see what feasible steps we can implement to improve the grasping pipeline.