Tune contact CFM

[diegoferigo]

Is your feature request related to a problem? Please describe.

I'm performing humanoid robot simulations with Ignition Gazebo and the DART ign-physics plugin. I have always struggled with jumpy contacts at the feet, creating many troubles when the contact wrench extracted from the physics engine is used for the state machine used by the controller to select the number of active contacts. Using a low-pass filter does not help since the stiff default contacts simulated with DART affect dramatically the operation of the whole-body controller.

Gazebo Classic with ODE has a lot of exposed parameters that can be configured from SDF with //collision/surface/contact/ode. However, DART does not have anything similar. Tuning those parameters are paramount for stabilizing contacts as I described in gazebosim/gz-physics#82.

Describe the solution you'd like

I tried to fiddle with DART internals, and I managed to stabilize the contacts without losing simulation accuracy adding here the following:

@@ -544,6 +546,7 @@ void ConstraintSolver::updateConstraints()
 
       mContactConstraints.push_back(
           std::make_shared<ContactConstraint>(contact, mTimeStep));
+      mContactConstraints.back()->setConstraintForceMixing(0.01);
     }
   }

The resulting behaviour is much much better.

Additional context

What I did is hardcoding a softening of the constraint for all simulated contacts. This is just a temporary workaround. I didn't find any way to set this parameter in a cleaner way. Though, this is the only way I found to stabilize contacts, and being able to tune this parameter could save countless hours of headache to all those people trying to simulate something more complex than wheeled robots.

[diegoferigo]

cc @GiulioRomualdi @traversaro @xEnVrE

[diegoferigo]

I found that @scpeters in the past worked on CFM dartsim#1371, maybe he can add interesting details. It's not clear to me what could be the right entry point to tune this parameter.

[traversaro]

What I did is hardcoding a softening of the constraint for all simulated contacts. This is just a temporary workaround. I didn't find any way to set this parameter in a cleaner way.

While this probably may require to add some code in DART, the clean way to set this parameter seems to add a new attribute to the dart::dynamics::detail::DynamicsAspectProperties in https://github.com/dartsim/dart/blob/master/dart/dynamics/detail/ShapeFrameAspect.hpp#L89 , similarly to what has been done in https://github.com/dartsim/dart/pull/1424/files . Then it would be possible to modify the dart::constraint::ContactConstraint constructor to automatically compute the CFM of the constraint given the two CFM of the shape, using some kind of way to combine them (I do not know if there is a way that is shared in the literature, or we can just get what ODE is doing).

[scpeters]

While this probably may require to add some code in DART, the clean way to set this parameter seems to add a new attribute to the dart::dynamics::detail::DynamicsAspectProperties in https://github.com/dartsim/dart/blob/master/dart/dynamics/detail/ShapeFrameAspect.hpp#L89 , similarly to what has been done in https://github.com/dartsim/dart/pull/1424/files .

I agree, the best place to add this to the API is in the ShapeFrameAspect similar to how friction coefficients are set (and slip compliance in dartsim#1424)

Then it would be possible to modify the dart::constraint::ContactConstraint constructor to automatically compute the CFM of the constraint given the two CFM of the shape, using some kind of way to combine them (I do not know if there is a way that is shared in the literature, or we can just get what ODE is doing).

In ODE, the slip parameter is implemented as CFM relaxing the friction constraint. Physically, it is like the inverse of a damping coefficient, and in gazebo, we choose to combine these coefficients by adding them, which is comparable to dampers acting in series:

• https://github.com/osrf/gazebo/blob/gazebo11_11.2.0/gazebo/physics/ode/ODEPhysics.cc#L1172-L1178