Revision history [back]

Without more details, this might be a classical convergence problem for the projected Gauss-Seidel (PGS) algorithm implemented in ODE (quickstep). This happens when the constraint matrix Jinv(M)J' is ill conditioned (large mass ratios, i.e. chasis mass >> wheel mass).

Here are some suggestions to try. For the bodies touching the ground (wheels), try to keep contact_cfm as close to 0 as possible, and contact_erp around 0.2. Given the relationship between kp,kd and cfm,erp below:

contact.surface.soft_erp = h * kp / (h * kp + kd);
contact.surface.soft_cfm = 1.0 / (h * kp + kd);

If time step size h = 0.001 (1ms), following kp and kd values should be a decent starting point:

<kp  value="1.0e+9" />
<kd  value="5.0e+6" />

yielding soft_erp of ~0.17 and soft_cfm of ~1.7e-07 for contcts.

Further, raise number of inner iterations:

<stepIters>500</stepIters>

I tested these values by setting PR2 base mass to 45tons, and was able to simulated it ok.

By the way, are you running the diamondback or unstable release?

John