[ODE] Re: Re: Joint position control

[Daniel Marbach]

Sorry, I'm using a crappy mail program and I accidently sent the previous email
before I finished it... So here's the complete message:


Thanks to Shamyl Zakariya's help I solved the problem. Since joint position
control has been discussed before on this mailing list, I post my solution
for future reference:


double actualAngle = dJointGetHingeAngle(hingeJoint);
double desiredAngle = the desired trajectory of your controller;

if (desiredAngle > highStop)
  desiredAngle = highStop;
else if (desiredAngle < lowStop)
  desiredAngle = lowStop;

// You have to find out a good value for param by trial and error
double v = param*(desiredAngle - actualAngle);

// Maximum angular rate of the servo
double static const vMax = ...;

if (v > vMax)
  v = vMax;
else if (v < -vMax)
  v = -vMax;

dJointSetHingeParam(hingeJoint, dParamVel, v);


---

IMPORTANT:

When creating the joint, set the low and high stops a little bit higher than you
actually want them to be:

// I used +/- 5°, smaller values might work as well...
dJointSetHingeParam(hingeJoint, dParamLoStop, lowStop - degToRad(5));
dJointSetHingeParam(hingeJoint, dParamHiStop, highStop + degToRad(5));
dJointSetHingeParam (hingeJoint,dParamFMax, maxForce);


Daniel

PS1: All angles are in radians!

PS2: If your trajectory never passes the low/high stops, it can be done
simpler...