[ODE] Re: Joint position control

[Daniel Marbach]

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)


	dJointSetHingeParam(hingeJoint, dParamVel, v);
	// cout << actualAngle << ", " << desiredAngle << ", " << v << endl;

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

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