[ODE] dJointGetFeedback peculiarity

[Shamyl Zakariya]

Hi
I'm having trouble with the dJointGetFeedback function. My 
understanding is that that function is the way to go if you need to 
query what torque or force the bodies on a joint are feeling.

So, anyway, what I need is to model a real world device I've made, 
which is basically a standard servo with an attachment which allows it 
to measure the torque it's applying at any given moment to hold 
position. My simulation needs to be able to model this same behavior.

So, I've got a working servo emulation, thanks to Nate, but I'm not 
having any luck with dJointGetFeedback.

Here's a snippet from my servo class:

dReal ServoMotor::getTorque( void )
{
	dVector3 parent, child, axis, axisRel;	

	dJointGetHingeAxis( _hinge, axis );
	dJointFeedback *feedback = dJointGetFeedback(_hinge);

	dBodyVectorFromWorld( _parent,
		feedback->t1[0], feedback->t1[1], feedback->t1[2],
		parent );
		
	dBodyVectorFromWorld( _child,
		feedback->t2[0], feedback->t2[1], feedback->t2[2],
		child );

	printf(" t1 (%.2f, %.2f, %.2f); t2 (%.2f, %.2f, %.2f)\n",
		parent[0], parent[1], parent[2], child[0], child[1], child[2]);
		

	dBodyVectorFromWorld( _child, axis[0], axis[1], axis[2], axisRel );
	printf("    axis (%.2f, %.2f, %.2f)\n", axisRel[0], axisRel[1], 
axisRel[2]);

	return 0.0; //I don't know what to return yet
}

As is apparent, I'm transforming the torque vector from world 
coordinates into body relative ones. The trouble is, as I run the 
simulation, the values fluctuate wildly. Sometimes they oscillate; & if 
I track just the magnitude of the vector, sometimes it converges on a 
reasonable value, *but* it takes perhaps 100 or 200 simulation steps to 
do so.

Here's an example of the fluctuating output, copied from stdout. The 
structure is a box with four "legs" mounted at the center of each side. 
Legs 0 and 2 are holding the box up, and legs 1 and 3 are pointing 
straight out. Here's a screenshot, for clarification:
http://home.earthlink.net/~zakariya/files/winged.png

Can anybody help, or at least point me in the right direction?

Torque:
0:  t1 (-19.81, -141.69, 5.72); t2 (6.56, 15.37, 21.77)
     axis (-0.00, 1.00, 0.00)
1:  t1 (-3.42, 0.00, -0.37); t2 (-0.00, -0.00, 0.04)
     axis (0.00, -1.00, 0.00)
2:  t1 (20.05, 140.82, 3.18); t2 (8.16, 22.66, 19.99)
     axis (-0.00, 1.00, 0.00)
3:  t1 (3.40, 0.00, -0.46); t2 (0.00, 0.00, 0.04)
     axis (0.00, -1.00, -0.00)


Torque:
0:  t1 (13.33, -123.63, -7.99); t2 (-3.89, 6.41, -18.31)
     axis (0.00, 1.00, -0.00)
1:  t1 (-3.41, -0.00, 0.78); t2 (0.00, 0.00, -0.07)
     axis (-0.00, -1.00, -0.00)
2:  t1 (-13.87, 125.36, -6.43); t2 (-5.00, 11.13, -17.76)
     axis (0.00, 1.00, -0.00)
3:  t1 (3.46, -0.00, 0.55); t2 (-0.00, -0.00, -0.07)
     axis (-0.00, -1.00, 0.00)


Torque:
0:  t1 (13.57, -124.47, -7.94); t2 (-3.32, 3.72, -19.14)
     axis (0.00, 1.00, -0.00)
1:  t1 (-3.45, 0.00, 0.67); t2 (-0.00, -0.00, -0.07)
     axis (-0.00, -1.00, 0.00)
2:  t1 (-13.47, 122.70, -6.38); t2 (-5.25, 12.56, -16.89)
     axis (0.00, 1.00, -0.00)
3:  t1 (3.44, 0.00, 0.65); t2 (0.00, 0.00, -0.07)
     axis (-0.00, -1.00, -0.00)


Torque:
0:  t1 (-0.92, -126.40, 17.78); t2 (2.76, 5.22, 11.62)
     axis (0.00, 1.00, 0.00)
1:  t1 (-2.99, 0.00, -0.29); t2 (-0.00, 0.04, 0.03)
     axis (0.00, -1.00, 0.00)
2:  t1 (-8.84, 124.62, -10.89); t2 (-5.14, 13.48, -14.10)
     axis (0.00, 1.00, -0.00)
3:  t1 (3.89, 0.00, -0.34); t2 (0.00, -0.04, 0.03)
     axis (0.00, -1.00, -0.00)


Torque:
0:  t1 (-17.10, -122.34, 4.99); t2 (3.63, 4.06, 20.93)
     axis (-0.00, 1.00, 0.00)
1:  t1 (-3.43, 0.00, -0.31); t2 (-0.00, -0.00, 0.04)
     axis (0.00, -1.00, 0.00)
2:  t1 (17.48, 121.51, 2.64); t2 (5.28, 11.39, 19.51)
     axis (-0.00, 1.00, 0.00)
3:  t1 (3.39, 0.00, -0.40); t2 (0.00, 0.00, 0.04)
     axis (0.00, -1.00, -0.00)


Torque:
0:  t1 (-17.03, -121.86, 4.97); t2 (3.56, 3.78, 20.91)
     axis (-0.00, 1.00, 0.00)
1:  t1 (-3.43, 0.00, -0.31); t2 (-0.00, -0.00, 0.04)
     axis (0.00, -1.00, 0.00)
2:  t1 (17.41, 121.03, 2.62); t2 (5.21, 11.10, 19.50)
     axis (-0.00, 1.00, 0.00)
3:  t1 (3.39, 0.00, -0.40); t2 (0.00, 0.00, 0.04)
     axis (0.00, -1.00, -0.00)


Torque:
0:  t1 (9.01, -122.98, -11.23); t2 (-3.45, 5.81, -15.91)
     axis (0.00, 1.00, -0.00)
1:  t1 (-3.91, -0.00, -0.20); t2 (0.00, -0.05, 0.03)
     axis (-0.00, -1.00, -0.00)
2:  t1 (1.30, 124.73, 17.63); t2 (4.06, 11.16, 10.15)
     axis (0.00, 1.00, 0.00)
3:  t1 (2.96, -0.00, -0.39); t2 (-0.00, 0.05, 0.03)
     axis (0.00, -1.00, 0.00)


Torque:
0:  t1 (-1.00, -124.08, 17.39); t2 (2.44, 3.86, 11.75)
     axis (0.00, 1.00, 0.00)
1:  t1 (-2.99, 0.00, -0.29); t2 (-0.00, 0.04, 0.03)
     axis (0.00, -1.00, 0.00)
2:  t1 (-8.76, 122.30, -10.62); t2 (-4.81, 12.14, -14.17)
     axis (0.00, 1.00, -0.00)
3:  t1 (3.89, 0.00, -0.33); t2 (0.00, -0.04, 0.03)
     axis (0.00, -1.00, -0.00)



Shamyl Zakariya
   "this is, after all, one of those movies where people spend a great
   deal of time looking at things and pointing."
	From a review of _Fantastic Voyage_