[ODE] HELP: dJointSetAMotorAngle

[jnilson_99 at yahoo.com]

Has anyone had any experience with amotor's and
dJointSetAMotorAngle? Mine isn't working too well.

Here's how i create the amotor constraints in
dAMotorUser mode:

dJointSetAMotorNumAxes (constraint->constraintID,3);
dJointSetAMotorAxis (constraint->constraintID,0,2,
1,0,0);
dJointSetAMotorAxis (constraint->constraintID,1,2,
0,1,0);
dJointSetAMotorAxis (constraint->constraintID,2,2,
0,0,1);

Here's what i call every time step to get the rotation
of the upper arm and then send it to the amotor:

	const dReal* R2 =
dBodyGetRotation(bodyParts[BODY_TYPE_LEFT_UPPER_ARM]->bodyID);
	dVector3 xa1 = {1,0,0};
	dVector3 xa2 = {R2[0],R2[4],R2[8]};
	dVector3 ya1 = {0,1,0};
	dVector3 ya2 = {R2[1],R2[5],R2[9]};
	dVector3 za1 = {0,0,1};
	dVector3 za2 = {R2[2],R2[6],R2[10]};

	dVector3 xCross;
	dNormalize3 (xa1);
	dNormalize3 (xa2);

	dVector3 yCross;
	dNormalize3 (ya1);
	dNormalize3 (ya2);

	dVector3 zCross;
	dNormalize3 (za1);
	dNormalize3 (za2);

	dReal xDotAngle = dDOT(xa1,xa2) );
	dReal yDotAngle = dDOT(ya1,ya2) );
	dReal zDotAngle = dDOT(za1,za2) );

	
	dReal xAngleDiff = acos(xDotAngle);
	dReal yAngleDiff = acos(yDotAngle);
	dReal zAngleDiff = acos(zDotAngle);

  
dJointSetAMotorAngle(joints[JOINT_TYPE_LEFT_SHOULDER]->constraintID,0,xAngleDiff);

dJointSetAMotorAngle(joints[JOINT_TYPE_LEFT_SHOULDER]->constraintID,1,yAngleDiff);
	dJointSetAMotorAngle(joints[JOINT_TYPE_LEFT_SHOULDER]->constraintID,2,zAngleDiff);