Should provide means to model suction grippers

[EricCousineau-TRI]

@takuya-ikeda-tri is interested in seeing how he could have a simulated vacuum gripper in Drake (e.g. HSR's gripper).

Simplest case is working with manipulating 5-10 boxes, where suction could be modeled as affixing the object(s) to the gripper.
More complex case would be more adversarial shapes, but that is not in the near term.

Taku is fine with any set of workarounds (e.g. pre-described joints with that can be enabled/disabled via bushing stiffness or flags or what not).

I asked about this in the Drake Developer meeting today:
https://docs.google.com/document/d/15iNBw_2O3TGbThHr6wlJNKctHSY5bEvt1rLhl5obZao/edit#bookmark=id.p2793ilwpii8

@SeanCurtis-TRI and @sherm1 mentioned that this should be implement-able in ForceElement, and some geometry queries (e.g. signed distance fields for a prescribed set of gripper-fixed points, normals, etc.) should be available in this interface.

@amcastro-tri suggested that the suction source should be modeled as a point if the shape is simple (e.g. just boxes), just to simplify things at the start.

@IanTheEngineer mentioned that a lab had implemented a suction plugin in Gazebo.
FWIW the closes thing I could find is here: http://docs.ros.org/melodic/api/gazebo_plugins/html/group__GazeboRosVacuumGripper.html (docs aren't exactly the clearest...)

[EricCousineau-TRI]

@takuya-ikeda-tri Would you be up for hashing out the initial prototype for your means? You can ask questions here, and we can field 'em.
Once you find a solution that works for you, then it could get assigned to one of us to upstream to Drake?

[IanTheEngineer]

@EricCousineau-TRI, yes, you have found the plugin that I was referring to. It was originally developed for the Amazon Picking Challenge by the JSK lab. It doesn't appear the documentation is thorough, as you pointed out:

ros-simulation/gazebo_ros_pkgs#389 and original JSK repo https://github.com/start-jsk/jsk_apc.

The winning team for the first year (2015) had a specialized suction gripper. The recap paper which contains a description of the winning team, as well a list of all the items used for picking (page 3) can be found here. There were a few fluffy / mesh / squishy items which made suction a challenge. Page 5 shows a list of teams and their choice of gripper or suction (or both). 7/13 teams chose suction of some sort.

[ike-taku]

@eric Cousineau (TRI) <[email protected]> OK, thank you for all of this information! Once I find it or make our plan, I'll let you know.

…

On Tue, Sep 1, 2020 at 3:03 AM Eric Cousineau ***@***.***> wrote: @takuya-ikeda-tri <https://github.com/takuya-ikeda-tri> Would you be up for hashing out the initial prototype for your means? You can ask questions here, and we can field 'em. Once you find a solution that works for you, then it could get assigned to one of us to upstream to Drake? — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#13977 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ALXEMTCE6Z2XI7VOBA4T35LSDPQVJANCNFSM4QQZAKJQ> .

-- *Takuya Ikeda* Toyota Research Institute - Advanced Development WovenCity Management, Robotics [email protected] +81-80-9022-5182

[amcastro-tri]

This is a very interesting fluid dynamics problem. I wonder what could be found about its actual physics. What do we know about the suction pumps used? do we have parameters for those? It'd be interesting to know the pressure-flowrate characteristics of the pump in order to estimate the suction pressure.

The plugging metioned above seems to use an ad-hoc model for a force inversely proportional to the distnace. Even though with hardcoded parameters, that law is not a bad idea if nothing else is known.

For a quick prototype I'd probably try:

• A LeafSystem that connects to MultibodyPlant::get_applied_spatial_force_input_port(). Simpler.
• I would approximate the suction cup as a "point" on a body.
• The leaf system then would take a mutable MPB, the Body to which to attach the suction cup, the position p_BoCo of the cup Co in the body Bo, and a "normal" indicating the dirction of the cup "outwards" from the body. It should also take parameters such as vacuum pressure and a reference area of the cup.
• That leaf system can keep a reference to MultibodyPlant and then use MBP::get_geometry_query_input_port() to obtain a QueryObject to perform distance queries.
• Use QueryObject::SignedDistanceToPoint() to obtaine the distance and geometry id to closet object. Since we know p_BoCo, we have all we need for this query.
• Compute normal force according to some model. An inverse distance model could be a good start to get code and wiring going.
• Since from SignedDistanceToPoint() we know the geometry id of the closest object, we can apply the computed force on that object and on the object holding the cup.
• Output that in the port to be connected to MBP::get_applied_spatial_force_input_port().

Sorry if not clear, this was a quick brain dump. Feel free to ask more questions.

[SeanCurtis-TRI]

Some thoughts on @amcastro-tri's suggestion.

Generally a good idea, but hanging on to a pointer to MBP has a bad smell. Can we get away from that?

As I see it, the MBP pointer is intended to serve several purposes.

1. Define suction cup point C relative to body B.
   • We have geometry frame F associated with geometry B (where X_BF = I). So, upon construction, pass the geometry::FrameId for the GeometryFrame the suction cup point is fixed to: p_FoCo (and, more generally, (X_FC). The QueryObjectcan give usp_WFso we can easily computep_WCo`.
2. Acquiring an instance of QueryObject from the MBP.
   • Problematic and unnecessary. The SuctionSystem would have access to its own Context and not MBP's (which would be necessary to evaluate its get_geometry_query_input_port(). It's much simpler for the system to directly connect to the QueryObject-valued port.
3. Going from signed-distance between geometries to force applied to bodies.
   • This is where I believe we are currently stuck. MBP is the only entity that knows how to relate geometry frames back to bodies. But the applied spatial force port value must be expressed in terms bodies. MBP could certainly expose a map<FrameId, BodyIndex> data that the SuctionSystem could use to do that mapping, and that would eliminate the system-system dependency. Even if we defer any action to remove this specific dependency, I'd still strongly advocate items 1 & 2 to reduce direct dependencies of one System on another System's API (as opposed to ports).

[amcastro-tri]

upon construction, pass the geometry::FrameId

In this idea I sketched, there is no additional geometry. There is only a point Co on body B. That's a nice side effect of being able to use SignedDistanceToPoint() directly.

Acquiring an instance of QueryObject from the MBP

Ah! I do agree on that one. Yes, getting the QueryObject from its own input port is definitely a more systems:: correct approach.

MBP could certainly expose a map<FrameId, BodyIndex>

We have MPG::GetBodyFromFrameId()

[SeanCurtis-TRI]

In this idea I sketched, there is no additional geometry.
Nor is there in my alternative. But it does assume that the MBP bodies do have registered geometry frames.

We have MPG::GetBodyFromFrameId()

Exactly. And to call a method on an MBP instance, you need to have a pointer. Whereas an API that returns a map that can live independently of the instance is how you get rid of storing a pointer. The point of everything I'd written was to the express purpose of eliminating a stored pointer to another system.

[amcastro-tri]

Ah, I forgot about scalar conversion. In any case, I imagine that if scalar conversion is not a problem, I'd go that route for quick prototyping. The big thing is that we don't know if a model like this will work in terms of numerical stability.

[jwnimmer-tri]

See also #19011.

[chen-tianjian]

We (Amazon) are interested in contributing a simple suction cup as an example (not a library since it is not general enough) in Drake. If there are some existing work on TRI side, please share and we can work on it together.

[amcastro-tri]

An example would be a great start for us to see what you need.

[RussTedrake]

For tracking -- I have an ugly implementation of the simplest version of this idea now in my manipulation repo / deepnote. It adds a floating joint between the suction gripper and the objects, and then updates the locked state of that joint between calls to simulator.AdvanceTo().

Being able to do this via an input port (#20571) would be much cleaner for users (we could implement the system once for everyone to use).