Friday, December 14, 2012

Implicit springs in the Open Dynamics Engine (ODE)

I recently spoke with someone who lamented the lack of implicit springs within ODE.  At the time, I mentioned that I thought it should be straightforward to implement implicit springs as another joint type.
It turns out that ODE, by default, actually models all constraints as extremely stiff springs with implicit first order integration.  In the end, I think it's fair to say that nature models all constraints as extremely stiff springs as well since at the atomic level, we end up with electromagnetic fields pushing and pulling against each other according to functions that look rather spring-like. 

Here's how it works:

First off, ODE use semi-implicit Euler integration.  It attempts to compute the forces that satisfy the linear constraints and then multiplies the forces by change in time over mass to get change in velocity.  It then adds the velocity (multiplied by change in time) to the position.

  $v_{(t+\Delta t)} = v_t+\frac{F_t}{m}\Delta t$
  $x_{(t+\Delta t)} = x_t+v_{(t+\Delta t)}\Delta t$


The naive implementation of a damped spring has you applying forces directly to the bodies according to the ideal dampened-spring equation: $F_t = -k_px_t - k_dv_t$.  At this point in the game, the only velocity we have access to is the current velocity: the velocity that brought us to our present position.  So this $F_t$ is based on Explicit-Forward Euler calculations.  So we'll get
$v_{(t+\Delta t)} = v_t+\frac{-k_px_t - k_dv_t}{m}\Delta t$

With springs that are very stiff relative to the size of the timestep, $\Delta t$, this will tend to be unstable.

However, the ODE manual informs us that if we appropriately set the CFM and ERP parameters, then the constraint behaves like a spring with implicit first order integration.  For a given $k_p$ and $k_d$, the appropriate parameters are $\text{ERP} = \frac{\Delta tk_p}{\Delta tk_p+k_d}$ and $\text{CFM} = \frac{1}{\Delta tk_p+k_d}$.

What does that mean?  The "implicit first order integration" portion means that ODE finds the velocity that agrees with the spring equation computed using the future position and velocity instead of the current values:
  $v_{(t+\Delta t)} = v_t+\frac{-k_px_{(t+\Delta t)} - k_dv_{(t+\Delta t)}}{m}\Delta t$.
This is cool and much more stable, although it's stable, in part, because it tends to absorb energy from the system.  I should emphasize that ODE is not directly solving the implicit equations, but that the right combination of parameters produces the same result.

With just a little bit of symbol pushing, we can find the spring constants (or PD gains) for a given selection of parameters: $k_p = \frac{\text{ERP}}{\Delta t\text{CFM}}$ and $k_d = \frac{1-\text{ERP}}{\text{CFM}}$.  With floating-point precision, the default values are $\text{ERP}=0.2$ and $\text{CFM}= 1\times 10^{-5}$.  With a time step of $\Delta t=\frac{1}{60}$, that gives us $k_p=1.2\times 10^6$ and $k_d=8\times 10^4$.  A spring is critically damped when $\frac{k_d}{2\sqrt{k_pm}}=1$.  A critically damped system converges to its set point as fast a possible without oscillating (for the given spring strength).  If we plug the effective default gains into this equation and solve for mass, we get $m=\frac{{k_d}^2}{4k_p}\approx 1.33\times 10^3$ which is a bit more than the mass of a cubic meter of water if we're using meters and kilograms.  If the effective mass acting on a constraint is less than that (as is normally the case with a human body, for example), the system is over-damped and will take a little longer to converge than is needed but with such stiff springs, it will still converge in just a few steps.  Greater masses will tend to oscillate a bit before settling down.

Let's look at an example to compare the first-order implicit spring with the explicit spring.  For convenience, we'll consider a body with mass $m=1$ using the default gains described above.  If, at time $t$, the body is one unit away from its set-point ($x_t=1$) with no velocity ($v_t=0$), then the explicit spring calculation says that we should apply a force of $F_t = -1.2\times 10^6$.  With unit mass, that force translates directly into acceleration, which in turn gives us $v_{(t+\Delta t)}=-2\times 10^4$ and $x_{(t+\Delta t)}\approx -333$ when we use a timestep of $\Delta t=\frac{1}{60}$.  Clearly, our spring has gone a bit too far and the next timestep will be much much worse as our system explodes.  This is bad.

If, instead, we apply an ODE constraint with the default parameters, ODE will set the target velocity of the body to $\hat{v}_{(t+\Delta t)}=\frac{-\text{ERP}x_t}{\Delta t}={-12}$.
This implies a desired acceleration $a_t={-720}$.
ODE will actually find $F_t=\frac{ma_t}{1+m\text{CFM}\Delta t}\approx -719.568$ because of the constraint force mixing.  So we'll have a new state $v_{(t+\Delta t)}=-11.992804$ and $x_{(t+\Delta t)}\approx0.8001199$.
If we calculate the spring force for the previous time point at that new state, we get $F_t=(-1.2\times 10^6)0.8001199+(8\times 10^4)11.992804=-719.56$.  Neat!

This is not going to explode.  It's quite stable as it asymptotes to the set-point.

If we choose $k_p=1\times10^6$ and then find the CFM and ERP for a critically dampened unit mass, we get $\text{ERP}\approx 0.8928$ and $\text{CFM}\approx 5.357\times 10^{-5}$.  The same mass converges much faster to its constraint target.  We can compare the convergence rates plotted on a log-scale.
The punchline is that ODE has a pretty good built-in spring.  Furthermore, the recently added DBall joint lets you specify a point-to-point constraint that pulls two points to some set distance

After writing this up, a friend pointed me to Erin Catto's GDC 2011 talk which does a much better job than I do at extolling the virtues of this approach.  I particularly like how the article points out that the CFM/ERP can be easily computed on the fly for each frame since we need to compute the effective mass felt by each constraint anyway.

No comments:

Post a Comment