ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

This is something that I would like to change down the road by allowing users to set cfm and erp for contact surfaces directly rather than setting kp and kd.

Currently, when two surfaces come into contact, the contact properties are specified via equivalent stiffness kp and equivalent damping kd coefficients for each contact surface involved. There are two steps to deducing cfm and erp from the two pairs of stiffness and damping parameters (kp1, kp2, kd1 and kd2 for bodies 1 and 2).

First, an overall kp is computed from kp1 and kp2 as if they are two springs in series, and an overall kd is computed from kd1 and kd2 as two dampers in parallel:

kp = 1 / ( 1/kp1 + 1/kp2 )
kd = kd1 + kd2

Then, cfm and erp are computed via following equations (see R. Smith, How to make new Joints in ODE:

cfm = 1 / ( dt * kp + kd )
erp = dt * kp / ( dt * kp + kd )

Given that quickstep is a very stable algorithm compared to worldstep, cfm should be zero for rigid body contacts (a non-zero cfm yields "soft surface" / spring damper dynamics).

erp dictates transient behavior of the contact as the contact tries to repair any constraint violations (due to drift or fixed step size collision issues). Empirically 0.2 is a good approximation for well behaved system, larger erp values tends to induce oscillation or simulation instability; smaller erp values can result in slow penetration repair transients.

For your case, it is more desirable to target:

  • cfm ~ 0 (kp >> 1)
  • erp ~ 0.2 (kd = dt * kp * ( 1/ erp - 1) )
  • iterating your PGS to 0 (stepIters >> 1).

Especially given some of the mass ratios between bodies in your system are large, you might want to experiment making cfm near machine zero.

Hope this makes sense.

This is something that I would like to change down the road by allowing users to set cfm and erp for contact surfaces directly rather than setting kp and kd.

Currently, when two surfaces come into contact, the contact properties are specified via equivalent stiffness kp and equivalent damping kd coefficients for each contact surface involved. There are two steps to deducing cfm and erp from the two pairs of stiffness and damping parameters (kp1, kp2, kd1 and kd2 for bodies 1 and 2).

First, an overall kp is computed from kp1 and kp2 as if they are two springs in series, and an overall kd is computed from kd1 and kd2 as two dampers in parallel:

kp = 1 / ( 1/kp1 + 1/kp2 )
kd = kd1 + kd2

Then, cfm and erp are computed via following equations (see R. Smith, How to make new Joints in ODE:):

cfm = 1 / ( dt * kp + kd )
erp = dt * kp / ( dt * kp + kd )

Given that quickstep is a very stable algorithm compared to worldstep, cfm should be zero for rigid body contacts (a non-zero cfm yields "soft surface" / spring damper dynamics).

erp dictates transient behavior of the contact as the contact tries to repair any constraint violations (due to drift or fixed step size collision issues). Empirically 0.2 is a good approximation for well behaved system, larger erp values tends to induce oscillation or simulation instability; smaller erp values can result in slow penetration repair transients.

For your case, it is more desirable to target:

  • cfm ~ 0 (kp >> 1)
  • erp ~ 0.2 (kd = dt * kp * ( 1/ erp - 1) )
  • iterating your PGS to 0 (stepIters >> 1).

Especially given some of the mass ratios between bodies in your system are large, you might want to experiment making cfm near machine zero.

Hope this makes sense.