[ODE] Joint position control

[Daniel Marbach]

Hi,

Once again, problems with joint position control...

I work with modules that consist of a cube and some kind of lever that is
attached to the cube with a hinge joint. The lever oscillates (e.g. a sin from
-90° to 90°) and without joint stops it works fine. However, the joint should
block at -90° / 90° (hiStop, lowStop). If the oscillation passes these
boundries, I have a stability problemo:

Suppose a module with
initial angle 45° and amplitude 90° (i.e. desiredAngle oscillates from -45° to
135°, actualAngle from -45° to 90° = high_stop). At the
'high stop' it blocks, but the 'desired angle' continues going until 135°. When
it comes back it passes the 'barrier' of the 'high stop' and the motor abruptly
accelerates from 0 to max speed. At that point the simulation is instable: the
lever of the module is separated from the cube in a single time step, then ERP
gradually 'pulls' the lever back into its correct position. It doesn't just turn
too fast, it is really torn away from the axis!

Shouldn't the motor accelerate gradually, since it only uses fMax (which is the
max force of the servo, in my case 0.73 N/m)?


double actualAngle = dJointGetHingeAngle(hingeJoint);
double desiredAngle = A*sin(...);

double v = 15*(desiredAngle - actualAngle);

// max v of the servo is: 60°/0.16s
double static const vMax = degToRad(60.0) / 0.16;
if (v > vMax)
  v = vMax;
else if (v < -vMax)
  v = -vMax;

if (desiredAngle < high_stop && desiredAngle > low_stop)
  dJointSetHingeParam(hingeJoint, dParamVel, v);



Thanks in advance for your help!

----------------------------------------
Daniel Marbach
Bitziusstr. 9
CH-3006 Bern

Tel: 031 351 11 09
WWW: http://icwww.epfl.ch/~marbach/
----------------------------------------