Gazebo freaks when joints hit limits

edit

What you are seeing is numerical instability of your simulation. Assuming it's a problem of numerical stiffness of your simulated system, one way to fix this problem is by making the time steps smaller - i.e. update your world file's <physics:ode> block by decreasing <stepTime>, for example:

<physics:ode>
  <stepTime>0.00001</stepTime>
  <gravity>0 0 -9.8</gravity>
  <cfm>0.0000000001</cfm>
  <erp>0.2</erp>
  <quickStep>true</quickStep>
  <quickStepIters>10</quickStepIters>
  <quickStepW>1.3</quickStepW>
  <contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
  <contactSurfaceLayer>0.001</contactSurfaceLayer>
</physics:ode>

Another fix is to force the constraint solver to provide a better solution of the system by increasing the number of inner iterations by increasing <quickstepIters>:

<physics:ode>
  <stepTime>0.001</stepTime>
  <gravity>0 0 -9.8</gravity>
  <cfm>0.0000000001</cfm>
  <erp>0.2</erp>
  <quickStep>true</quickStep>
  <quickStepIters>1000</quickStepIters>
  <quickStepW>1.3</quickStepW>
  <contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
  <contactSurfaceLayer>0.001</contactSurfaceLayer>
</physics:ode>

Note that both modifications above help stabilize simulation but slow things down as well.

Alternatively, you can modify the joint constraint properties by tuning stopKp and stopKd for your joint (try lowering ERP at the joint limit). For example, add this XML extension to your model urdf:

<gazebo reference="your_problematic_joint">
  <stopKd value="1000000.0" />
  <stopKp value="1000000.0" />
</gazebo>

Note that the relationship between time step size h, kp, kd, cfm and erp are (as extracted from ODEPhysics.cc in gazebo):

    // Compute the CFM and ERP by assuming the two bodies form a
    // spring-damper system.
    kp = 1.0 / (1.0 / geom1->surface->kp + 1.0 / geom2->surface->kp);
    kd = geom1->surface->kd + geom2->surface->kd;
    contact.surface.soft_erp = h * kp / (h * kp + kd);
    contact.surface.soft_cfm = 1.0 / (h * kp + kd);

More details on the CFM and ERP parameters can be found in the ODE manual.

If you can't stabilize the system via one or all of the above suggestions, please provide

• Your complete model specification.
• Specify what kind of controller are you using (e.g. pid force control, velocity control, ... etc.).
• Plots of position/velocity/force vs. time for the joint right before the instability happens will be very helpful as well.
• And preferably a way for others to reproduce the same issue you are seeing.