Spring tuning

Given that constraints in ODE can be thought of as really stiff springs, we begin to wonder if there's a better way to select the parameters controlling those constraints than trial and error.  The Erin Catto talk linked in the previous post discussed choosing a frequency and damping ratio and setting the parameters from there.  This is definitely better than guessing and checking.

To me, however, what we really want to control is the time to convergence.  I'll assume that we're working with critically damped springs.  A critically damped spring asymptotes to its setpoint, only reaching its target as time approaches infinity.  Instead of dealing with infinite time, what we can do is say that we want our spring to cover some fraction $(1-\alpha)$ of the initial distance $x_0$, during time $t$: $x_t=\alpha x_0$.

Wikipedia gives us the closed form solution for the position of a critically damped spring over time: $x_t = (A+Bt)e^{-\omega_0t}$ where $A=x_0$, $B=v_0+\omega_0 x_0$, and $\omega_0=\sqrt{\frac{k_p}{m}}$.
Plugging those values in we get
$x_t = (x_0+(v_0+\sqrt{\frac{k_p}{m}} x_0)t)e^{-\sqrt{\frac{k_p}{m}}t}$.
If we assume that the starting velocity is zero, then our desired state at time $t$ is
$\alpha x_0=(x_0+\sqrt{\frac{k_p}{m}}t x_0)e^{-\sqrt{\frac{k_p}{m}}t}$.

We simplify with an intermediate variable $c=\sqrt{\frac{k_p}{m}}t$ to get $\alpha x_0=(x_0+c x_0)e^{-c}$. We then engage in some symbol pushing:
$\alpha =(c+1)e^{-c}$
$-\alpha =-(c+1)e^{-c}$
$-\frac{\alpha}{e} =-(c+1)e^{-(c+1)}$
At this point the equation is in proper form to make use of the Lambert W function.  I had never heard of it before working on this, but it turns out to be exactly what we need.  This funny function has two branches.  We want the one that goes between $-1$ (a spring with no pull at all) and $-\infty$ (an infinitely strong spring is the only one that will cover 100% of the distance in a finite amount of time). This is denoted as $W_{-1}()$.  It tells us:
$-(c+1)=W_{-1}\left(-\frac{\alpha}{e}\right)$.
So...
$c=-W_{-1}\left(-\frac{\alpha}{e}\right)-1$
$\sqrt{\frac{k_p}{m}}t=-W_{-1}\left(-\frac{\alpha}{e}\right)-1$
$k_p=(-W_{-1}\left(\frac{-\alpha}{e}\right)-1)^2t^2m$

If we don't assume a starting velocity $v_0=0$, then things end up a little bit uglier: $k_p=(-W_{-1}\left(\frac{-\alpha}{e^{\frac{v_0t}{x_0}+1}}\right)-\frac{v_0t}{x_0}-1)^2t^2m$.

As Catto's slides mentioned, we need to compute the effective mass $m$ for each constraint anyway (at least, we need the inverse) and we already know or determine the values of all the other variables in that equation.  So we can find the spring stiffness that gets our system where we want it to be when we want it there, as long as we can evaluate $W_{-1}()$ which is not a particularly efficient thing. 

Since we're assuming a critically damped spring, we get $\zeta = 1 = \frac{k_d}{2\sqrt{mk_p}}$  Therefore, once we know $k_p$, we get $k_d=\frac{1}{2\sqrt{mk_p}}$.  And from those two values, we know how to find the appropriate CFM and ERP settings. 

This approach might be computationally excessive.  I haven't tried yet.  If, as I suspect, Lambert's W is too costly, then perhaps we can simply precompute a lookup table and do some simple interpolation.  When $v_0=0$, $W_{-1}()$ only uses $\alpha$ as a parameter.  We could compute that once for $\alpha=0.01$ and still have the freedom to manipulate the stiffness by changing $t$.  If, for some reason, we really needed to do so, we could create a table for the uglier function, being sure to cover value ranges of $t$, $x_0$ and $v_0$ that we expect to encounter.

This does nothing to address under-damped springs, which are desirable when modeling things like muscles (or actual springs), but when you want to control the convergence of the constraint solver and still maintain the stability that ERP and CFM give you, this seems like the thing.