Need minimalist high-confidence MultibodyTree/Plant

[sherm1]

Per discussion with Russ and Dynamics team, we need to migrate Drake to a version of RigidBodyTree in which we can have high confidence. (Some issues we encountered with RBTree.) Call that MultibodyTree (MBTree) for discussion purposes, with MultibodyPlant the compatible System 2 wrapper. This will involve some major changes not easily accomplished while attempting to satisfy all RBTree/Plant dependents with every PR, so we will leave RBTree alone during MBTree development.

We would like MBTree to become usable as early as possible so would like to determine the minimal functionality that would be needed to support particular applications. The purpose of this WIP issue is to collect requirements that define minimal utility.

Proposed target applications to be supported in version 0.1

• IIWA manipulation simulation: Schunk Gripper picks up boxes and cylinders? @sammy-tri, @naveenoid?
• Current automotive simulation: bicycle model of car? @jwnimmer-tri can you elaborate?

Proposed 0.1 feature list (please add comments to discuss minimal requirements that we have missed)

Basic guarantees:

• [x] Consistent notation, well-defined frames
• [x] Good unit tests for every exposed feature
• [x] Glass box, System 2-compatible implementation (MultibodyTree<T>, Context<T>)
• [x] Geometry handling via GeometryWorld
• [x] Styleguide compliant

Reduced subset of existing functionality:

• [x] Basic rotation/kinematics/spatial vector utilities using best known methods
• [x] Build an MBTree programmatically with fully-documented methods
• [x] Build an MBTree from urdf and sdf, with fully-documented mapping from files to internal frames
• [x] Joints: Quaternion Floating base (6dof), Fixed (Weld) joint (0dof), Revolute (Pin), Prismatic (Slider)
• [x] Compliant joint stops
• [x] Local (uniform) gravity field
• [ ] Forward dynamics

New functionality (MBTree first; RBTree backport under duress):

• [x] Compliant contact with friction & stiction
• [ ] Contact torsion either in model or via tesselation
• [x] Use official OSRF sdf file parser (see issue #4337)

Initial limitations in version 0.1:

• Open tree topology only
• Compliant contact only
• No constraints

[jwnimmer-tri]

For automotive, as I understand it most of our models do not and will not soon use MBTree or MBPlant. (As discussed elsewhere, the main things we want initially are System2 & Simulator2, GeometryWorld, and Visualization APIs.)

For MBPlant specifically, what we are simulating right now with RBPlant is prius.sdf on terrain; it has a steering bar and PD controllers on the wheels. It would be helpful to retain (rebuild?) something like that demo using MBPlant, so that we can help iterate on our needs versus the evolving MBPlant design.

It doesn't necessarily have to be that particular SDF. We already have a few things we don't like about it, so maybe the time is ripe to make a more appropriate physical model for this purpose, to coincide with MBPlant's capability level.

[RussTedrake]

on quick inspection, i think the schunk gripper (https://github.com/RobotLocomotion/drake/blob/master/drake/examples/schunk_wsg/models/schunk_wsg_50.sdf) has kinematic loops. The prius model steering column does, too, but @jwnimmer-tri articulated that that particular model is not a priority.

The reporting (via output ports) of contact forces and potentially the acceptance of external wrenches (via input ports) would be on my very short list. But might not be strictly required for those two applications in v0.1. They would be needed for walking. + @siyuanfeng-tri for his thoughts.

[siyuanfeng-tri]

I just want to make sure that the Jacobian stuff are in the list somewhere (probably covered by the forward kinematics part). I'd like to compute J, and Jdot, where xdot = J * v. Forward dynamics probably covers the mass matrix and the nonlinear term. It would be really nice to have the centroidal momentum matrix and its dot as well, but I can live without it. Also in the current RBT, computing the mass matrix and such requires a non const kinematics cache, which is a bit cumbersome to work with. It'd be nice to remove that limitation in MBT.

@sherm1, just to be clear, if we do have loops / constraints, for example, valkyrie standing / gripper grabs something, how is the simulator / MBT going to stabilize it? I guess all stabilization happen in generating the appropriate force? There won't be any velocity mapping back to zero or similar things?

@RussTedrake the contact point forces / equivalent wrench can be reported in ContactResults with RBT. @SeanCurtis-TRI knows better than I do, but I think those can be pretty easily ported over to MBT. Point forces comes from whatever contact model, and bullet gives us geometry. Doing the transformation should be the same. Having input external wrenches would be nice to test disturbance rejections and for developing fancier state estimation, but I don't need that in the short term.

[edrumwri]

Siyuan, contact constraints are compliant, which is somewhat like Baumgarte stabilization. There is otherwise no stabilization being done (no rigid contact, no sticking contact, no quaternion normalization, no loop joint stabilization).

---

Evan Drumwright Research Scientist http://positronicslab.github.io Toyota Research Institute Palo Alto, CA

On Thu, Dec 29, 2016 at 8:10 AM, siyuanfeng-tri notifications@github.com wrote:

I just want to make sure that the Jacobian stuff are in the list somewhere (probably covered by the forward kinematics part). I'd like to compute J, and Jdot, where xdot = J * v. Forward dynamics probably covers the mass matrix and the nonlinear term. It would be really nice to have the centroidal momentum matrix and its dot as well, but I can live without it. Also in the current RBT, computing the mass matrix and such requires a non const kinematics cache, which is a bit cumbersome to work with. It'd be nice to remove that limitation in MBT.

@sherm1 https://github.com/sherm1, just to be clear, if we do have loops / constraints, for example, valkyrie standing / gripper grabs something, how is the simulator / MBT going to stabilize it? I guess all stabilization happen in generating the appropriate force? There won't be any velocity mapping back to zero or similar things?

@RussTedrake https://github.com/RussTedrake the contact point forces / equivalent wrench can be reported in ContactResults with RBT. @SeanCurtis-TRI https://github.com/SeanCurtis-TRI knows better than I do, but I think those can be pretty easily ported over to MBT. Point forces comes from whatever contact model, and bullet gives us geometry. Doing the transformation should be the same. Having input external wrenches would be nice to test disturbance rejections and for developing fancier state estimation, but I don't need that in the short term.

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/RobotLocomotion/drake/issues/4633#issuecomment-269635101, or mute the thread https://github.com/notifications/unsubscribe-auth/ACHwz8rTMx0-04jX8jJAj5vU4LEp5oIJks5rM79dgaJpZM4LXQke .

[siyuanfeng-tri]

@edrumwri cool, that matches what I thought.

[sammy-tri]

I think that looks good for what manipulation will need in the short term, minding @RussTedrake 's comments about the mechanics of the gripper model (I'm open to changing the model if we can find an effective alternate method of enforcing the motion of the fingers).

We also should make sure we don't do anything which would break GravityCompensator (I don't think this will be a problem but it wasn't explicitly mentioned above).

[siyuanfeng-tri]

@sammy-tri @naveenoid I think grav comp should be fine as long as we still have the nonlinear term from forward dynamics.

[liangfok]

Once a PR containing MultiBodyTree and MultiBodyPlant exists, should that be an indicator to all other developers to stop improving RigidBodyTree and RigidBodyPlant since any effort spent will need to later be ported? Or, should I stop working on it now given the existence of this issue? (This thought came up while I was working on #4685.)

[sherm1]

@liangfok, I would suggest coordinating with @amcastro-tri on RBTree improvements, and @SeanCurtis-TRI if the changes are geometry or contact related. RBTree will continue to be used heavily for some time and it will be the initial source and inspiration for both MBTree and GeometryWorld. There will come a time in the not-too-distant future when effort should move to MBTree & GW instead, but we're not there yet.

[sherm1]

@amcastro-tri do you think we can close this now?

[amcastro-tri]

Only FD and torsional friction (low priority IMO) are not supported yet from this list. So yes, I believe we can close this now and probably open an updated one with current needs. Thanks