[ODE] Servo to reach a target position

[david@csworkbench.com]

Setting both velocity and stops is rarely a good idea, and since your code
is basically replacing the functionality of stops anyway, I recommend you
don't use them.  Something like this (before each step):

fmax = constant_fmax
if (scale_factor * fabs(target_pos - current_pos) < constant_velocity) {
  velocity = scale_factor * (target_pos - current_pos)  //for a smooth stop
}
else if (target_pos > current_pos) {
  velocity = constant_velocity
}
else {
  velocity = -constant_velocity
}

The first if statement lowers the velocity when you get close to the stop,
and the scale_factor determines how close you have to be relative to the
velocity.  We make it relative to the velocity so it will have a smooth
transition from the constant_velocity to the first calculated velocity. 
You might even want to use a more complex function (like squaring the
calculated velocity) to "round off" the slowing down segment of the
stop... just remember that whatever you put to the right of velocity =
(well, in the third parameter of dJointSetParam), put the absolute value
of that to the left of < constant_velocity in the if statement.

This method keeps you from getting to constant_velocity*timestep/2
distance from the target one step, then jumping to the other side on the
next step and oscillating back and forth.  I would also assume that real
servo controllers use similar techniques since they can't physically set
the servo velocity to 0 and expect the structure to react instantaneously.
 It also means that loads placed on the system arent handled by a
full-power push back towards the target, but a slight nudge (which is all
you needed to start with).


Your other (simpler) option is to keep velocity at 0, set fmax to a
reasonably low value, and set both the low and high stop to the target
position.  ODE will add up to FMax force at the correct angles to both
satisfy the joint and move it towards the stops.  But you lose control
over velocity in this method.  And I'm not exactly sure how it would
handle loads.... You could always try both methods and see which one you
like better.

David

> Hello,
>
> I have a servo motor and I want it to reach a target position.
>
> The servo is implemented as a hinge and it seems to me that in ODE
> hinges are controlable either by applying a torque on the body (tricky
> and dangerous according to the ODE manual), or by specifying a target
> velocity (dParamVel) and a maximum torque (dParamFMax).
>
> Since I just want to achieve a target position, I was considering
> setting the following:
>
> dParamFMax: to define the maximum acceleration.
> dParamVel: to define the target speed.
> dParamHiStop or dParamLoStop: to define the target position.
>
> My control program should read the current position of the servo and
> compute the following:
>
> // set dParamFMax to a constant value
> if (target_position > current_position) {
>   // set dParamVel to a constant positive value
>   // set dParamHiStop to target_position
>   // possibly set dParamLoStop to current_position
>   // run until target_position is reached
> }else {
>   // set dParamVel to a constant negative value
>   // set dParamLoStop to target_position
>   // possibly set dPareamHiStop to current_position
>   // run until target_position is reached
> }
>
> Does this sound to be the right way to do position control on servos ?
> Are these any issues I should be aware of ?
>
> -Olivier
>
>
> _______________________________________________
> ODE mailing list
> ODE@q12.org
> http://q12.org/mailman/listinfo/ode