[ODE] Better way to find 'Desired Velocity'

[Andrew Schroeder]

I decided to try another approach to this problem. I want to create a
simulation where the strength of the joints (Fmax) can change over
time as your character gets stronger and stronger, so I can't really
'tune up' a situation good for the model at one time, and have it work
throughout.  I need a calculation to pick desired velocity
intelligently based on the maxforce, distance from target, etc...
He's what I've got so far....
Jon's comment made me take a closer look at the ODE Wiki and find that
the "ThrustControlLogic" had some relevant points.  Specifically, the
equation:

Maximum stoppable velocity (Vmax) = sqrt( 2 * maximum breaking force *
remaining distance to target / mass)

My thought process was, if we always strive to go the max stoppable
velocity considering how far we have left to go, then we'll always be
going as fast as we can while still being able to stop.  However, like
Jon says, this will work best if it is predictive. So instead of
asking, "what is the max stoppable velocity given the remaining
distance to go,"  we should ask, "What is the max stoppable velocity
ASSUMING WE GO THAT SPEED OVER THE NEXT TIMESTEP."
Re-stated: what is the highest speed we will be able to stop if we go
that speed for the next timestep.

So our original equation was:
F=MaxForce
M=Mass
Vmax = sqrt(2*F*(targetPos - currentPos) / M)

So I figure, replace currentPos with .5(Vmax+Vcurrent) * t
(basically replace currentPos with pos after accelerating to Vmax for
one timestep)
BUT WAIT!!!
ode uses a _first order integrator_,  which I now know means that
after calculating the new velocity for a body, it assumes it has been
moving that speed for the whole timestep! (wrong!)  That's why the
docs make a point of this inaccuracy, I never understood this
before....
So instead you replace currentPos with the much simpler (currentPos+Vmax*t)
resulting in this mess...
Vmax = sqrt(2*F*(targetPos - (currentPos+Vmax*t)) / M)

if you solve for Vmax again, you get a quadratic, and when solved gives:
(-2*F*t + sqrt(4*F^2*t^2 + 8*M*F*(Target-currentPos)) / 2*M

I've tested this formula out extensively and have found that it gives
good desired velocities with a wide range of maxforce, timestep and
target position values. I think this is going to work well for me
because there is no "tuning" necessary.

Some notes:
-If (Target-currentPos) is negative, then your sqrt will give you
imaginary numbers.. Instead use absolue value of (Target-currentPos)
in the formula, and then if (Target-currentPos) < 0 then take your
formula's entire result times -1.
- THIS FORMULA BY ITSELF WON'T ACTUALLY WORK!! (wha???)  This only
works well if your Mass number is right.  If you use this formula for
finding a desired Angular Velocity, then mass needs to be replaced
with "Moment of inerta," which can be tough to calculate... If the
bodies you're moving with the joint are also joined to other bodys (of
course they are), then finding the correct Mass value to use above can
be very hard, Moment of Inertia is even harder, to the point for me
that I found it imposible. At one point I was ready to give up on this
because I thought I could not get past this impossible requirement of
finding the mass/moment of inertia for the entire island of connected
bodies...
   I solved this problem by calculating the "actual max force" each
timestep.  Basically, if your (Fmax) param is 2, and last time you
wanted to accelerate the joint from 0m/s to 2m/s, but now you're only
going 1m/s, the your "actual max force" is only 1, not 2. Use this
"actualMaxForce" in the Vmax calculations instead.

I realize this is a poor explination of complex work, so I've attached
some sample code that hopfully can help explain...



On Fri, 28 Jan 2005 14:56:20 -0800, Jon Watte <hplus-ode at mindcontrol.org> wrote:
> 
> There is some talk about this on the ODE Wiki already.
> 
> To get your body to move X in time t, you have to have a velocity
> that is X/t. If you currently have a velocity that is V, you have
> to apply a force that is   F = (X/t - V) * m   for mass m.
> 
> "t" in this case is the size of your time step.
> 
> Personally, I find that systems behave best if I control them based
> on prediction:
> 
> Given my current position and velocity, where will I be n steps
> from now? Where do I want to be n steps from now? Apply a force that's
> proportional to the difference between these two positions, times some
> scaling factor. This gives very natural and well dampened movement,
> and has two tuning variables: the number of steps n, and the scaling
> factor (which should be multiplied by n before application if you want
> to be able to vary them somewhat independently).
> 
> Cheers,
> 
>                        / h+
> 
> 
> -----Original Message-----
> From: ode-bounces at q12.org [mailto:ode-bounces at q12.org]On Behalf Of
> Andrew Schroeder
> Sent: Friday, January 28, 2005 2:17 PM
> To: Royce Mitchell III; ODE at q12.org
> Subject: Re: [ODE] Better way to find 'Desired Velocity'
> 
> I'm thinking that the equation
> 
> DesiredVelocity = Error * Gain
> 
> is actually NOT a simple P-Gain situation that you are saying I should
> start with.  I think this because the Input (Error) is in POSITION,
> and the output is in VELOCITY, thus with the above equation we
> actually set an Integrated Controller, we have a simple I-Gain,
> instead of the recommended P-Gain.
> Is my thinking correct on this? If so is there a way to create a
> simple P-Gain like you suggest without setting the position directly?
> (obviously I don't want to set the position of my ode bodies directly)
> 
> If my setpoint was a velocity then I would say the equation was a
> P-Gain, but it's a position.
> 
>
-------------- next part --------------

'This is VB.NET code...

'This sub is called every timestep for the joint to recalculate it's desired velocity
Public Sub UpdateFrame(ByVal t As Single)

        If Me.Active Then

            For i As Integer = 0 To Me.AxisCount - 1

                If DesiredPositionDriven(i) Then

		    'if the joint can't figure it's own angular velocity (aMotor), then just leave actual max force = maxforce
                    If Single.IsNaN(Me.GetCurrentVelocity(i)) Then
                        _actualMaxForce(i) = Me.MaxForce(i)
                    ElseIf Math.Abs(_lastDesiredVelocity(i) - _lastVelocity(i)) > _actualMaxForce(i) * _lastTimeStep Then

			'if the desired change in velocity was more than our max force could deliver, then look at the actual
			'change in velocity to set our actualMaxForce...
                        _actualMaxForce(i) = Math.Abs(Me.GetCurrentVelocity(i) - _lastVelocity(i)) * Me.JointedMass / _lastTimeStep
                    End If

                    Me.DesiredVelocity(i) = Me.GetDesiredVelocity(t, i)

		    'keep track of this so we can figure out actualMaxForce next time...
                    _lastTimeStep = t
                    _lastDesiredVelocity(i) = Me.DesiredVelocity(i)
                    _lastVelocity(i) = Me.GetCurrentVelocity(i)

                End If

            Next

        End If

    End Sub
    Public Function GetDesiredVelocity(ByVal t As Single, ByVal i As Integer) As Single

        Dim returnValue As Single
        Dim distanceFromDesiredPosition As Single

        distanceFromDesiredPosition = _desiredPosition(i) - Me.GetCurrentPosition(i)

        If Math.Abs(distanceFromDesiredPosition) < 0.001 Then
            returnValue = 0
        Else

	    'here is the big math....
            returnValue = CSng((-2 * _actualMaxForce(i) * t + Math.Sqrt(4 * _actualMaxForce(i) * _actualMaxForce(i) * t * t + 8 * Me.JointedMass * _actualMaxForce(i) * Math.Abs(distanceFromDesiredPosition))) / 2 * Me.JointedMass)
            If distanceFromDesiredPosition < 0 Then returnValue = -returnValue

        End If

        Return returnValue

    End Function