[ODE] Yet another question about servo emulation

[Shamyl Zakariya]

All,
I'm pretty new to ODE, and up till now, I've had a fair amount of 
success, thanks entirely to reading this list!

In my project, I need to model servo behavior, such that I can say in 
my code, more or less something like this:
	someServo->setPosition( angle )

I gather several people have need for similar functionality. Now, to do 
this, I wrote a servo class which basically creates a motor and 
attaches it to an existing hinge and the two bodies connected by this 
hinge. In the interest of simplicity, I've made the motor a dAMotorUser 
type, following the axis of the hinge its controlling. I'm using a 
straightforward approach of using a feedback loop, e.g., it checks what 
the current angle of the hinge is, and corrects the velocity (and 
direction) of the motor to bring it closer to the desired position, 
every timestep. When it gets to the desired position, it sets the 
motor's velocity to 0, and ups the dParamFMax to hold it there.

here's a snippet of the relevant parts of my servo class

ServoMotor::ServoMotor( dWorldID world, dJointID hinge, dBodyID 
parentBody, dBodyID childBody )
	:_hinge(hinge), _motor(0), _parent(parentBody), _child(childBody),
	_min( toRadians( -178 )), _max(toRadians(178)), _current(0), 
_desired(0),
	_speed(1.0), _force(1000.0), _verbose(false)
{
	
	_motor = dJointCreateAMotor( world, 0 );
	dJointAttach( _motor, _parent, _child );

	dJointSetAMotorMode( _motor, dAMotorUser );
	dJointSetAMotorNumAxes( _motor, 1 );
	
	dVector3 axis;
	dJointGetHingeAxis( _hinge, axis );
	
	// set joint zero, axis relative to first body, along axis of hinge
	dJointSetAMotorAxis( _motor, 0, 1, axis[0], axis[1], axis[2]);
	dJointSetAMotorParam( _motor, dParamFMax, _force );
	
	dJointSetHingeParam( _hinge, dParamLoStop, toRadians( -179.0 ) );
	dJointSetHingeParam( _hinge, dParamHiStop, toRadians( 179.0 ) );
	dJointSetAMotorParam( _motor, dParamFudgeFactor, 0.5 ); //arbitrary?
	dJointSetHingeParam( _hinge, dParamStopERP, 0.5 ); //arbitrary?
}


void ServoMotor::setPosition( dReal radians, dReal speed )
{
	if (radians < _min) radians = _min;
	else if (radians > _max) radians = _max;
	_desired = radians;
	
	if (speed < 0.0) speed = 0.0;
	else if (speed > 1.0) speed = 1.0;
	_speed = 0.1 + (0.9 * speed); //speed from 0.1 -> 1.0	
}

dReal ServoMotor::getCurrentPosition( void )
{
	_current = dJointGetHingeAngle( _hinge );
	return _current;	
}

void ServoMotor::step( void )
{
	dReal da = _desired - getCurrentPosition(); //magnitude and direction 
to turn

	if (fabs(da) > toRadians( 1.0 ))
	{
		dReal velocity = da * _speed;

		if (_verbose)
		{
			printf("ServoMotor::step()\tda: %.2f, velocity: %.2f\n", da, 
velocity);
			dumpState();
		}
		
		dJointSetAMotorParam( _motor, dParamFMax, _force );
		dJointSetAMotorParam( _motor, dParamVel, velocity );

	}
	else
	{
		if (_verbose)
		{
			printf("ServoMotor::step()\tLOCKDOWN\n");
			dumpState();
		}
	
		//stop motor, will hold via force setting
		dJointSetAMotorParam( _motor, dParamFMax, dInfinity );
		dJointSetAMotorParam( _motor, dParamVel, 0 );
	}
}

Now, by and large, this works beautifully. The servos move to the angle 
I request, and hold that position just fine, even when bearing a load.

The trouble is, if something occurs which "knocks" the servo out of 
position (like a collision, or the structure falling over) the servo(s) 
freak out. It's hard to describe, but the limb just starts "flailing" 
about swinging back and forth, and then, if it's really severe, the 
simulation explodes. I've tried to track the position of the servo when 
its wigging and it seems that it's trying to go to the position it 
wants to be at, but it just doesn't succeed. Perhaps excessive torque 
force is building up?

I'd really appreciate it if somebody here can point out what's wrong 
with my code. Perhaps there's a logical error in the feedback loop? 
perhaps I'm not fully understanding the dynamics of the way motors 
work, or how to use ERP or CFM adjustments. Note that I read about the 
problems with setting stops in powered joints, but my stops are only to 
prevent the joint from going past -PI and +PI (which is realistic for a 
real servo, and keeps the math cleaner). When I didn't have stops in 
place, the system blew up much more frequently.

Please, any help would be greatly appreciated.

Also, one more thing -- if anybody can give me some notion of how to 
get the torque force a joint/motor is experiencing while holding 
position I'd also appreciate it. I gather the function 
dJointGetFeedback() is the way to go, but I haven't tried it yet. And 
I'm sufficiently new to this API that I'm uncertain about how to go 
about doing this correctly.


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_