mbp: Computation of joint limit stiffness may not be conservative enough?

[EricCousineau-TRI]

UPDATE: See this comment below: https://github.com/RobotLocomotion/drake/issues/12775#issuecomment-591637572

Don't have a clear picture on the full issue, but there's some odd things going on.

Slack convo: https://drakedevelopers.slack.com/archives/C2WBPQDB7/p1582738032009900

\cc @ggould-tri

[ggould-tri]

Looks to me like a fixed joint and a prismatic joint with near-zero limits behave very differently. This joint:

    <joint name="${name}_spring" type="fixed">
      <parent>${name}_joint_spring_base</parent>
      <child>${name}</child>
    </joint>

Gives this behavior: While in the same place this joint -- prismatic with 2mm limits:

    <joint name="${name}_spring" type="prismatic">
      <parent>${name}_joint_spring_base</parent>
      <child>${name}</child>
      <axis>
        <xyz> 0 0 1</xyz>
        <limit>
          <effort>0</effort>
          <lower>-0.001</lower>
          <upper>0.001</upper>
        </limit>
        <dynamics>
          <damping>15.0</damping>
          <friction>1.0</friction>
        </dynamics>
      </axis>
    </joint>

Gives this:

[sherm1]

Are the joint limits being enforced? The state for a joint with limits can still be anything until some computation enforces those limits. Seems odd that the initial state wouldn't be just 0 though, which does seem like it would be within those limits!

[ggould-tri]

The visualized pose there is steady-state (you can see the contact forces in the long green arrows -- the racks are resting on one another). So the simulation would have to somehow entirely lack joint limit enforcement.

[EricCousineau-TRI]

Here is my repro: https://github.com/RobotLocomotion/drake/compare/master...EricCousineau-TRI:issue-12775-wip

It has 2 x 2 options: joint_type = ("prismatic", "fixed"), and display_mode = ("kinematic", "sim")

As of 7ff797dab: (a) prismatic, kinematic: Looks good. (b) prismatic, sim: Looks wrong. The drawers drop??? (similar to what Grant sees) (c) fixed, kinematic: Looks good (d) fixed, sim: Looks good

So the prismatic joint with sim seems weird, which could be a repro of grants issue.

[EricCousineau-TRI]

As of eff0b51f0, it seems like setting the MBP's time step to 1e-3 makes (b) prismatic, sim, look better.

[ggould-tri]

Empirically my problem goes away when I make my fake nonphysical intermediate link have mass 1kg instead of 1g. So some relationship between low-mass links and joint limits in relation to timesteps seems to be implicated.

(FYI Anzu runs with at 3ms timestep)

[EricCousineau-TRI]

@sherm1 @amcastro-tri Grant ~is going to play~ played with masses in the Anzu dishwasher sim to see if that might be.

From looking through some of the MBP code, it looks like the stuff that CalcPrismaticJointPenaltyParameters chooses the least-stiff penalty stuff between the joint's parent and child bodies based on the MBP time step (not the penalty method time scale) and the body masses. Since Grant chose a low dummy mass, perhaps it's choosing an abnormally low stiffness for this, which allows the joint limits to be violated quite a bit.

Does that seem accurate? If so, can y'all think of a more intelligent solution to this, aside from tuning masses?

We've noted that SDFormat has the parameters //joint/axis/limit/stiffness (which seems like a penalty method value, vs. the physical value in //joint/dynamics/spring_stiffness): http://sdformat.org/spec?ver=1.6&elem=joint Should we make this tunable via the MBP API? Or is there better math to use for computing the full upstream mass when doing the critically damped response?

EDIT: Gonna throw the ball in y'all's court for now.

[sherm1]

There are mysterious @amcastro-tri heuristics in there, I believe -- are they sensitive to articulated mass properties, Alejandro? If not, that might explain what Grant saw with his 1g little part.

[amcastro-tri]

Ok, thank you for this great work @ggould-tri and @EricCousineau-TRI, I understand the problem now. Your reasoning is entirely correct. CalcPrismaticJointPenaltyParameters chooses the most conservative case. Since it doesn't really know anything about your system, it assumes the worst case, which would be that of the two bodies being aligned. Therefore, in order to guarantee the time scale introduced by the penalty parameters is within the desired penalty_time_scale, it chooses the least stiff parameter. This works well for one big massive body (say your dishwasher) and a smaller body (say a rack) or for two free bodies of similar masses.

Your case is different in that the two prismatic joints add dofs that are orthogonal to each other. This is great because it'd allow you to set a stiff parameter for the z direction in order to hold the rack without affecting the stability of the smaller "spring_base" which is constrained to only move in the x direction. However, it only works for this case and if they were not orthogonal it'd affect stability.

When I wrote CalcPrismaticJointPenaltyParameters I only considered the simpler cases since is what we needed to solve first. Your case would need to consider three bodies instead of two.

As @sherm1 points out, a heuristic based on articulated body inertias could work better, though that'd require some research.

I think the simplest solutions would be:

1. Implement a planar joint (with this I mean only translation in a plane is possible. better name?), a 2 DOF joint. This would remove the intermediate "spring_base" link and MBP's heuristics would still work.
2. Implement API for setting the penalty stiffness, as @EricCousineau-TRI proposes.

[EricCousineau-TRI]

Related StackOverflow post: https://stackoverflow.com/q/61605694/7829525

[amcastro-tri]

As @sherm1 pointed out, the heuristics would work best if using articulated body inertias. I'll work on this and push a fix PR.

[EricCousineau-TRI]

Another user ran into this: https://stackoverflow.com/a/61789069/7829525

[sherm1]

See discussion in #13245. IMO it will be better to avoid heuristics altogether, just pick a documented default for stiffness and dissipation. The heuristics can still be supplied as a helper function that a user could call to obtain some reasonable starting values. That would take the mystery out of these parameters and empower users to solve these problems themselves.

[sherm1]

@amcastro-tri here is another report of a joint limit problem with the discrete solver. OP fixed it by reducing the step size. Not clear what was happening: https://stackoverflow.com/questions/61788760/prismaticjoint-limits-not-enforced

[amcastro-tri]

As I work on the new solver I keep this in mind. The new solver must be robust to joint limits (whether discrete or continuous).

[EricCousineau-TRI]

I believe this was asked again on StackOverflow: https://stackoverflow.com/questions/63769864/is-drake-obeying-joint-limits-in-this-case-and-how-can-i-check (\cc @hongkai-dai)