[ODE] How to simulate tolerance joints?

[Daniel Hein]

Hello,

I am still working on a simulator of a humanoid robot model. For 
simulating the servo joints of the real model with friction I used a 
hinge joint with an angular motor joint. This works so far. Now I want 
to expand model to simulate servos with a small (in the simulation 
adjustable) tolerance, caused by the gears.

In terms of ODE the ideal model of such a (hinge-)joint would be a hinge 
with an angle phi (the actual servo motor angle), a conventional 
friction F1 simulating the friction of the servomotor, and a tolerance 
friction F2 (with F2 << F1), that acts only within 
[phi-delta...phi+delta]. The servo motor angle phi can be changed either 
active through the servo motor or passive through an external force > F1.

My actual solution for simulating such a tolerance joint is using 4 
joints plus 1 extra body per real joint: to connect a body A with a body 
B I create an extra in-between body C (without a geom aspect, just a 
very small mass):
- A and C are connected by a hinge and an angular motor joint; the motor 
joint is set to vel=0.0 and an FMax that simulates the normal friction 
F1 of the joint.
- C and B are connected by a hinge and an angular motor joint; the motor 
joint is set to vel=0.0 and an FMax that simulates the tolerance 
friction F2. Additionally the Lo- and HiStop parameter are set for the 
hinge joint to simulate the amount of tolerance (delta).
- the axes of all joints are aligned
 

      Body A
     //    \\
 AMotor   Hinge Joint <= dParamVel+dParamFMax  (conventional friction)
     \\    //
      Body C
     //    \\
 AMotor   Hinge Joint  <= dParamLoStop+dParamHiStop / dParamVel+dParamFMax     (tolerance)
     \\    //
      Body B


Testing this construction with simply two boxes connected in that way 
works as expected. But using this joint
construction for the humnoid robot model results in heavy instability: 
Without moving or adding forces to the model,
for the first seconds it seems to be stable. But than suddenly one or 
more parts of the model get out of place, resulting in breaking the 
simulated model.
I already tried a lot of settings of the joint's CFM, StopCFM/StopERP 
parameter. A big influence has the weight of body C (actually 0.02 kg) 
(heavier=more stable) and the sidelength (larger=more stable) - but 
since I want to simulate the real model I do not want to increase this a 
lot.
I do not use any of the speed-optimization options of ODE. dWorldStep is 
set to 0.03. World's global CFM  is set to 1e-5,  the ERP to 0.1.

So actually I have two questions regarding this problem:
1. What is probably the reason for the instability of this joint 
construction and how I could manage it?
2. Is there a more simpler or more stable way to simulate such a joint 
tolerance?

Thanks a lot in advance,
Daniel Hein
Humboldt University of Berlin, Germany