Problems I've run into. Solutions I've found. Ideas I've had that someone almost certainly had before me. Thoughts, troubles, and answers I've worked on. Hopefully, someone else won't have to work quite as hard.
When tracking a rigid body by following a bunch of points on the body, we need to be able to find the rotation matrix and translation vector that tell us the new orientation and position of the body relative to some canonical orientation and position.
Consider a collection of $n$ points $A = [a_1 \cdots a_n]$ in $\mathbb{R}^m$ (where, it's likely that $m=3$). If we assume that $A$ has been rotated by $R$, translated by $t$, and corrupted by Gaussian noise to produce a corresponding collection of points $B = [b_1 \cdots b_n]$ s.t. $b_i=Ra_i+t+w$ where $w\sim \mathcal{N}_m(0,cI)$. If we want to recover $R$ and $t$, we can use Umeyama's paper to approximately do so. For those not interested in the proofs, the key equations are numbers 34-42.
It boils down to this:
Find the mean and variance around the mean of both point sets (eqs. 34-37).
Find the covariance of the mean-centered point sets (eq. 38).
Take the singular value decomposition of the the covariance.
Replace the diagonal matrix of singular values with an identity matrix, negating the last value if the determinant of the covariance matrix is less-than zero (eq. 39).
Multiply the matrix back out and you have the rotation matrix, $R$ (eq. 40).
Rotate the mean of the first set and scale by the trace of the singular value matrix (with the same negation as in step 4). Subtract this from the mean of the second set and you have the translation $t$ (eqs. 41-42).
Step 4 will probably not be necessary in most cases for the purpose of tracking a rigid body with motion capture markers. It is intended to prevent the algorithm from finding a mirrored copy of the rotation matrix in cases where the noise is particularly severe. The scaling in step 6 should also be small if we're dealing with an actual rigid body; so we might optionally skip that part as well.
In pretty equations:
$\bar{a} = \frac{1}{n}\sum_{i=1}^n a_i $
$\sigma_a^2 = \frac{1}{n}\sum_{i=1}^n \left\| a_i - \bar{a}\right\|^2$ note that this is not the covariance of the points around the mean, but rather the variance of the scalar distance from the mean.
$\Sigma_{ab} = \frac{1}{n}\sum_{i=1}^n (b_i - \bar{b})(a_i - \bar{a})^\text{T}$
$UDV^\text{T} = \text{SVD}(\Sigma_{ab})$
$S = I$ --- If $\text{det}(\Sigma_{ab})<0$ then $S_{mm}=-1$.
$c = \frac{1}{\sigma_a^2}\text{tr}(DS)$
We then find:
$R = USV^\text{T}$
$t = \bar{b} - cR\bar{a}$
In matlab, we have:
m = size(a,1);
n = size(a,2);
abar = mean(a,2);
bbar = mean(b,2);
aa = a - repmat(abar, 1,n);
bb = b - repmat(bbar,1,n);
va = mean(sum(aa.*aa),2);
sig = bb*aa'*(1.0/n);
[U D V] = svd(sig);
S = eye(m);
if prod(diag(D))<0)
S(m,m)=-1;
end
R = U*S*V';
c = trace(D*S)/va;
t = bbar - c*R*abar;
This seems to do the trick, and with a little understanding of the singular value decomposition, it's not too unintuitive. Many thanks to Shinji Umeyama.
I found that I needed a better understanding of exactly what is going on in the Open Dynamics Engine physics simulator. So I dove into the code. Most of what's going on is pretty clear, but some of it is confusing and not precisely documented. I decided to write about it to clarify things in my own mind.
Dynamic State
Given $n$ 3d bodies, the dynamic state of the $i^{\text{th}}$ body at time $t$ is its position, orientation, linear velocity, and angular velocity: $\left[\begin{array}{cccc}x_{it}&R_{it}&v_{it}&\omega_{it}\end{array}\right]$. The body also has mass $m_i$ and inertia tensor $\mathcal{I}_i$. The body also has geometry for the purposes of collision, but we won't worry about that here. We will assume that all of these values are framed in the global coordinate system (this means that $\mathcal{I}$ is actually a function of time since it's affected by $R$, but we'll assume that's taken care of).
Integration Step
The dynamics equations start-out straightforward: $f = ma$. A single
3d rigid body has six degrees of freedom; so we get $a = M^{-1}\left[\begin{array}{c} f\\ \tau \end{array}\right]$ where
$M=\left[\begin{array}{cc}mI&0\\0&\mathcal{I}\end{array}\right]$ and $f$ and $\tau$ are force and torque. After a time step
of $h$ using semi-implicit Euler integration (and lumping gyroscopic effects into $\tau$), the future velocity is the current velocity plus the acceleration, scaled by time: $\left[\begin{array}{c}v_{t+h}\\ \omega_{t+h}\end{array}\right] =
\left[\begin{array}{c}v_{t} \\ \omega_{t}\end{array}\right] + ha$. Then the future position is the current position plus the future velocity scaled by time:
$x_{t+h} = x_t + hv_{t+h}$ and
$R_{t+h} = \mathcal{R}(h\omega_{t+h})R_t$
where $\mathcal{R}()$ is a function that makes a rotation matrix out
of time-scaled angular velocity (when using ODE's FiniteRotationMode, otherwise, the orientation, in quaternion form, is updated the same way as position and then re-normalized).
Constraint Equation
So far, so good.
When a rigid body is constrained in some way, we need an extra step to
figure out the forces resulting from constraints. If our $n$ bodies are all part of a single interacting system and
we constrain $k$ degrees of freedom, then we need to create a matrix $J$
of size $\left\{k \times 6n\right\}$. Each row of $J$ describes a
linear relationship between all of these bodies.
The ODE document on creating new joints
does a good job describing how to compute individual rows of $J$.
Russell Smith (original author of ODE) expanded on this in "Constraints
in rigid body dynamics" (Game Programming Gems, 2004). Erin Catto also
put together a nice paper that covers forming and resolving constraints.
This paper also describes the iterative Projected Gauss-Seidel
algorithm for solving the constraint equations. The ODE function, dWorldQuickStep(), uses this method. For my work, I prefer the dWorldStep() solver which uses a slower, but more accurate algorithm sometimes called Lemke's algorithm and sometimes called the Dantzig-Cottle simplex algorithm. David Baraff seems to get credit for introducing this algorithm as a solver for physical simulation with contacts and friction.
In
ODE, constraints start out being applied to velocities, but then are
actually solved as accelerations. A single "constraint row" applies to a
single degree of freedom. We frame a constraint stating that a body
can't move along the $x$ axis as
$[\begin{array}{cccccc}1&0&0&0&0&0\end{array}]\left[\begin{array}{c}v_{t}
\\ \omega_{t}\end{array}\right] = 0$. Solving this constraint
is easy. We just need a force that cancels out whatever forces and
momentum exist along that axis. This leads to zero acceleration (or
acceleration canceling our current velocity) for a final velocity
of zero.
If we have many such constraints, it still
boils down to solving a linear system. When solved as a purely linear
system, however, if we aren't careful and accidentally over-constrain
the system, things break. This is why ODE applies constraint-force
mixing (CFM) component which, as discussed in previous posts, allows
constraints to slip proportional to the force required to satisfy them.
If we turn to the ODE manual we see the $\text{CFM}$ augmented constraint framed as,
$J\left[\begin{array}{c}v_{t+h} \\ \omega_{t+h}\end{array}\right] = c + \text{CFM}\lambda$. The right hand side should actually read,
$c - \text{CFM}\lambda$, because that is how it's used in the code.
The $j^{\text{th}}$ row of $J$ looks like this:
$\left[\begin{array}{ccccc}p_{j1}&q_{j1}&\cdots&p_{jn}&q_{jn}\end{array}\right]$
where $p_{ji}$ is applied to $v_i$ and $q_{ji}$ is applied to $\omega_i$. Good examples exist in the sources cited above. Each row of $J$ also has associated elements that define limits on the force ($\text{lo}_j$ and $\text{hi}_j$), springiness of each degree of freedom ($\text{cfm}_j$), the desired relative velocity along the constraint ($c_j$), and a reference to another row ($\text{findex}_j$) that modulates $\text{lo}_j$ and $\text{hi}_j$ for friction constraints (to make friction proportional to normal force).
So we have our desired constraint relationships: $J\left[\begin{array}{ccccc}v_1&\omega_1&\cdots&v_n&\omega_n \end{array}\right]^\intercal = c - \text{CFM}\lambda$. In ODE, the rows of $J$ are restricted to dealing with two or fewer bodies, with all other columns set to zero. That simplifies the code, but there are interesting constraints that involve three or more bodies and you can't easily create those.
Final Equation in Terms of Acceleration
For the solver, we re-frame the system as constraints on acceleration. When we project the velocity into acceleration, we divide both sides by
time and then replace the left hand-side (acceleration) with inverse-mass times
force: $JM^{-1}J^\intercal\lambda$ (here
$M$ represents the diagonal mass and the rotated inertia tensor
together). Grouping terms involving $\lambda$, we get
$\left(JM^{-1}J^\intercal +
\frac{1}{h}\text{CFM}\right)\lambda = \frac{1}{h}c$. Since
$c$ is the desired velocity, converting it into an
acceleration this way assumes that the starting velocity is zero. To
correct for that, we subtract out the current velocity, projected onto the constraint space. We also need to
subtract out the effects of external forces, $F$ (gravity, gyroscopic torques, and any forces the user may have applied) since they affect the bodies' velocities. This gives us the final
equation:
$\left(JM^{-1}J^\intercal +
\frac{1}{h}\text{CFM}\right)\lambda = \frac{1}{h}c -J\left(M^{-1}F+\frac{1}{h}v_t\right)$.
where $\text{CFM}$ is a diagonal matrix and $F$ is the vector of externally applied forces and torques.
For convenience, we group the pieces of this equation into three variables: $A\lambda = b$. In
spirit, this equation is still $m^{-1}f = a$, and we need to solve
for $f$. The solver finds $\lambda$ that satisfies the above system subject to constraints $\text{lo}\leq\lambda\leq\text{hi}$. The solver algorithms are quite interesting, but are a topic for another day. Once we have solved for $\lambda$, we project out of constraint space back into force space and add it to the other forces, $F_{\text{new}} = F_{\text{old}} + J^\intercal\lambda$, and we integrate to find our new velocities and positions.
Conservation of angular momentum is a bit questionable in ODE. If your rigid bodies have inertial tensors whose principal axes are all equal, everything is fine. Otherwise, you will get into trouble. A freely rotating body will tend to gain energy rather rapidly, eventually spinning fast enough to crash the simulator. This was discussed somewhat recently on the ODE mailing list. I have generally worked around this problem by either adding global angular "friction" to the system or applying a maximum threshold to angular velocity. This is fine but a little ugly. You can also disable the gyroscopic term for each body which will result in conservation of angular velocity instead of angular momentum: a spinning figure skater pulling her arms in won't experience any acceleration.
The newsgroup discussion referenced a paper by Claude Lacoursière that addresses this problem (building on a paper by Anitescu and Potra). The essence of the paper seems to be that you can achieve stability by manipulating the inertia/mass matrix to account for the gyroscopic torques.
It seems worthwhile to start with the computations affecting angular velocity in ODE's time step. I'll use the notation in Lacoursière's paper. Before resolving constraints for each time step of $h$ seconds, ODE first computes the inverse inertia matrix for time $n$ in the global frame: $\mathcal{I}^{-1}_{n} = R_n\mathcal{I}^{-1}_{0}R^{\intercal}_n$. If gyroscope mode is enabled, it then computes the extra gyroscopic torques for the time step: $\tilde{\tau}_n = [{\mathcal{I}_n}\omega_n]\times\omega_n + \tau_n$. Gravitational forces are also computed at this time, but they're not relevant for this discussion.
After computing the constraint Jacobian, ODE prepares a system for the LCP solver. The solution is effectively found in "acceleration space". Essentially, we start with $F=ma$ and then find the $F$ that satisfies $m^{-1}F=a$ where $a$ is the acceleration necessary to satisfy our constraints. For rotation, the conserved value will be computed as $\mathcal{I}^{-1}_{n}\tilde{\tau}_n+\frac{\omega_n}{h}.$ Once the solver finds the constraint forces, the new angular velocity will be computed:
$\omega_{n+1} =\omega_{n}+h\mathcal{I}^{-1}_{n}\tilde{\tau}^*_n$ (where $\tilde{\tau}^*_n$ includes the constraint forces).
Ignoring the constraint forces, this expands to the explicit time step:
$\omega_{n+1} = \omega_n+h\mathcal{I}^{-1}_{n}[\hat{L}_n\omega_n+\tau_n]$ where $\hat{L}_n$ is the angular momentum, $\mathcal{I}_n\omega_n$, in "cross-product-matrix" form. As mentioned before, this explicit form gains energy much like an explicit spring. That is bad.
The paper goes on to compute the implicit form in body-local coordinates and finds that it's equivalent to solving the explicit form with a different inertia matrix: $\tilde{\mathcal{I'}}_n=\mathcal{I}_0-h\hat{L'}_n$. According to the paper, this "is rather bizarre since then, the modified inertia tensor is non-symmetric" (pg. 8). The paper shrugs at this form, stating that it would cause problems for most multibody frameworks and is therefore not very interesting, but they still give a function for the stepper:
$\omega_{n+1}'=[I+h\alpha C_n +h^2\alpha C^2_n]\omega_n' + h[I+h\alpha C_n + h^2\alpha C^2_n]\mathcal{I}^{-1}_0\tau_n'$
where
$\alpha = 1/(1+h^2\kappa)$
$\kappa = \det(\mathcal{I}^{-1}_0)\hat{L'}^{\intercal}_n\mathcal{I}_0\hat{L'}_n$
and
$C_n = \mathcal{I}^{-1}_0\hat{L'}_n$
I derived things a little differently. ODE solves things in global space, rather than local space. If we start with eq. 5.2 from the paper: $\tilde{\mathcal{I'}}_n\omega'_{n+1}=\mathcal{I}_0\omega'_n+h\tau'$ and move things into global space, we get the following:
$\omega_{n+1}=\tilde{\mathcal{I}}^{-1}_n\mathcal{I}_n\omega_n+h\tilde{\mathcal{I}}^{-1}_n\tau_n$
If we set the right-hand side to be equal to the explicit computation and then solve for $\tilde{\tau}_n$, we get $\tilde{\tau}_n=\frac{1}{h}\mathcal{I}_n[\tilde{\mathcal{I}}^{-1}_n\mathcal{I}_n\omega_n+h\tilde{\mathcal{I}}^{-1}_n\tau_n-\omega_n]$.
This is a lot uglier than $\tilde{\tau}_n = [{\mathcal{I}_n}\omega_n]\times\omega_n + \tau_n$, but as long as $\tilde{\mathcal{I}}_n$ is invertible, ODE shouldn't have much trouble with it. The LCP solver never needs to see the funky mass matrix. If we group terms a little better, we can see that there's not all that much extra to compute:
$\tilde{\tau}_n=\mathcal{I}_n\tilde{\mathcal{I}}^{-1}_n (\frac{1}{h}L_n)+\mathcal{I}_n\tilde{\mathcal{I}}^{-1}_n\tau_n-(\frac{1}{h}L_n)$
We already need to compute $\mathcal{I}_n=R_n\mathcal{I}_{0}R^{\intercal}_n$.
$L_n = \mathcal{I}_n\omega_n$ is cheap.
$\tilde{\mathcal{I}}_n=\mathcal{I}_n-h\hat{L}_n$ is cheaper.
I believe that inverting a 3x3 matrix can be done without too much
cost and my feeling is that this matrix should always be invertible,
although the matter merits additional attention.
I tested a straightforward implementation in ODE and found that it takes 3 to 7 times longer than the simple matrix-multiply and cross-product of the explicit method, but it's stable instead of exploding and that computation is still pretty small compared to the constraint solver or collision detection.
Local angular velocity of a single spinning body in ODE. Inertia tensor is diag(10,4,2). Initial angular velocity is (1,2,3). Time step is h=1/60. With gyroscope disabled, the body experiences conservation of angular velocity. With explicit gyroscopic torques, it explodes. With implicit computations, it converges. It ought to maintain a closed fixed orbit that looks like the explicit one.
If we plot the local angular velocity of a spinning body, we get something that looks a lot like Fig. 8.1. from Lacoursière's paper (but cooler with animation).
Interestingly, the paper's justification for their inertia tensor of choice is
...it has three different values but is not an extreme case of a long thin object. This illustrates many of the difficulties that can be encountered in practice. (pg. 11)
However, when I went to visualize the geometry of an object with such an inertia tensor, I found that I couldn't do it. It's impossible to create a box or ellipsoid of uniform density with that tensor unless you allow it to have imaginary dimensions. My intuition says that you can't create any object with that tensor. Stack-exchange verifies that the principal axes of an inertia tensor need to satisfy the triangle equality.
If, instead, we create a box with the density of water using the same ratio for dimensions: $[1,.4,.2]\text{m}$, then we get an $80\text{kg}$ box with inertia tensor $I_0\approx\text{diag}(1.33,6.93,7.73)$. The end effect with this mass moment is pretty similar: the implicit method converges to a stable point and the explicit explodes.
I tried averaging the two methods or alternating between methods on successive passes, but, at least for this set of conditions, the trajectory was almost the same as the implicit trajectory. The paper discusses other approaches that are "symplectic and time-reversible" and are much better at approximating the correct behavior, but these seems to use completely different integration methods and might not be amenable to implementation with popular rigid-body physics engines.
One remaining issue that should be addressed is the fact that I used a matrix inverse in my computation. As I have experimented with it, I've found that there's a danger of encountering numeric instabilities with regular masses.
I recently had a paper accepted and published that discussed using the Open Dynamics Engine (ODE) as a simple tool for inverse kinematics and inverse dynamics. While I was presenting, I was fortunate to have an opportunity to discuss the paper with Victor Zordan who had a similar paper published at SIGGRAPH in 2003. After a few minutes of confusion, he pointed out to me that I was abusing the terminology. Part of what I was calling "Inverse Kinematics" is known as skeleton fitting and has its own body of literature.
Anyway, I've been feeling like the paper was not as clear as it should be for a technique that is pretty simple; so I decided to try again here.
My end goal is to produce coordinated dynamic movements in simulated humanoid characters to accomplish some end goal. To get there, we are attempting to draw inspiration from human movements since it seems that humans manage to control themselves pretty well. This leads to a sub-goal of capturing the important features of a human movement. We begin with motion capture data from a real human and a physically-simulated humanoid model.
Method
We obtain the motion capture data using a 16 camera Phasespace system. This system tracks optical markers (LEDs) that pulse in known patterns. These active markers make it so that there's less risk of markers getting mixed up when they pass near each other--a problem that optical systems with passive (IR reflective) markers have to deal with. Downsides to active markers are that they're more expensive and require electrical wiring and a power source (so they can't be used to track to a flying pigeon, for example).
Model
We implement the humanoid model as a collection of rigid bodies connected by joint constraints. Automatically finding the kinematic skeleton parameters belongs partially to the domain of skeleton fitting which has processes for figuring out where joints should go and what degrees of freedom or angular limits they should have based on the relative locations of markers observed over time. We bypass that step by simply constructing a "good-enough" model in ODE.
Figure 1. Humanoid model, designed by hand and simulated with ODE.
The model has 48 internal degrees of freedom.
The model is based roughly on my own dimensions and flexibility. We use universal joints for the elbows, wrists, knees, and ankles. We use hinge joints to connect the toes to the heels. All other joints are ball-and-socket joints with three angular degrees of freedom: hips, shoulders, collar-bones, upper-neck, lower-neck, upper spine, and lower spine. This gives a total of 48 internal degrees of freedom.
ODE's joints provide built-in joint limit constraints, although for ball-and-socket joints, you need to use a second joint known as an "angular motor". This is because it is difficult to keep three independent angular degrees of freedom well-defined. We use the joint limit constraints to keep the body from adopting poses that are not usually possible, such as bending the elbows backwards.
The engine also provides joint motors. These allow you to set a target velocity for the joint as well as limits on the amount of force or torque that can be used to achieve that velocity. For example, one might set the elbow joint to bend at a fixed rate of two radians per second, using no more than 200 N m of torque. If the permitted amount of torque is insufficient, the system will not be able to reach the target velocity, but will get as close as it can. The joint motors are incorporated into the solver along with all other constraints and the system is solved simultaneously.
We use the joint motors to provide a resting pose for the model. For each degree of freedom, we define a target angle and a maximum amount of torque for achieving that angle. We then set the target joint velocity as necessary for the joint to achieve its target orientation in one simulation frame. This, along with the joint limits, can be thought of as a "prior" over the space of all relative body poses that biases the body to toward particular solutions and also decreases rapid oscillation between different body orientations.
If the resting pose is a prior, we need some evidence in order to create a likelihood and ultimately find an a posteriori probable body pose. For this, we use the motion capture data. The basic approach is very simple: we attach the markers to the model and they drag the body along. The actual implementation of this is only a little bit more involved, but for the sake of formality, we will first define a set of symbols.
Definitions
We have a set of $n=21$ rigid bodies $B=[b_1, ...,b_2]$. Each body has shape, dimensions, and an inertia tensor computed from its shape and dimensions, assuming a constant density of $800\frac{\text{kg}}{\text{m}^3}$, which gives the model approximately the same total mass as me. We have $h=20$ joints $J=[j_1,...,j_h]$ connecting the $n$ bodies. Each joint has 1 to 3 angular degrees of freedom. We then have position data from $k=32$ motion capture markers: $M=[m_1,...,m_k]$. The data are sampled over discrete intervals of time and the simulation engine works with discrete time; so we will also refer to time discretely with $T$ frames of time, with each frame representing $\Delta$ seconds. We will refer to the column vector position of a marker or body at frame $t$ as $p(m_i,t)$. Its linear velocity is $v(m_i,t)$. For bodies, we will also refer to the orientation $q(b_i,t)$ (a quaternion) and angular velocity $\omega(b_i,t)$. In our implementation, each angular degree of freedom of each joint has its own scalar value, and we treat each joint as if it had three angular degrees of freedom $a(j_i,t)$ with the extra values being assigned to zero when there are fewer than three actual degrees of freedom.
We create a rigid body in ODE for each marker. We instruct ODE to treat these markers as "kinematic" bodies. To the simulator, this means that the marker bodies are treated as though they had infinite mass and their trajectories cannot be changed by external forces. When we create the marker bodies, we place each according to its initial position: $x(m_i,0)$. For each frame of simulation, we set the velocity of each marker body by finite differencing so that it will get where it needs to be on the next frame. By convention, we are actually setting the velocity for the next rather than the current frame: $v(m_i,t+1) = \frac{x(m_i,t+1)-x(m_i,t)}{\Delta}$.
Skeleton pose fitting
We then attach each marker $m_i$ with a ball-and-socket joint to some point on a corresponding body $b_h$. Skeleton fitting literature provides automatic ways to figure out what that point should be. We just assigned the points by hand. To the physics engine, since $m_i$ is kinematic, this essentially just defines a constraint on $b_h$, stating that the point the marker is attached to must also have the velocity $v(m_i,t+1)$, plus a little extra if the attachment point is not equal to $x(m_i,t)$. The constraint solver then tries to make that happen. We can then record the resulting body pose, $x(b_i,t)$ and $q(b_i,t)$ for each frame of time.
The orientation of some bodies may be under-constrained by the markers (there are no markers on the neck). Fortunately the "resting-position" prior will take care of that. Other bodies may be over-constrained because the markers will move around a little bit and human joints cannot be modeled perfectly accurately in the simulator. This is not a problem either, because, as I mentioned before, constraints in ODE can actually be thought of as very stiff springs with implicit integration. If two markers attached to a single body part go different directions and other constraints prevent the part from following them, the constraints "stretch" to accommodate the move and then snap back into place when possible. Because we want to be sure that the marker constraints stretch before the internal body joints do, we tell ODE to use "looser springs" for the markers than the internal joints $J$ by setting the "constraint force mixing" parameter (CFM) to be $1\times 10^{-4}$ while the other joints use a value ten times smaller. With these values, the body still follows the markers with high accuracy, but keeps the body parts together as it should. However, these values were chosen somewhat arbitrarily. In future work, we will make more principled decisions, keeping in mind that we are trying to balance a number of competing springs.
Inverse kinematics
To me, the distinction between finding the skeleton pose and inverse kinematics is very small. If you want to constrain a point on the skeleton to follow a path, it makes little difference if the path comes from a motion capture marker or is synthesized. The physics engine only sees constraints and solves them all simultaneously, as far as is possible. This means that it is very easy to create additional constraints to prevent ground penetration or foot-skate. We use the standard collision model and contact joints with friction to accomplish this.
The simulation finds a body pose that does a good job satisfying the marker constraints. Once we have that body pose, we can use it as a new resting pose and modify the movement using the same technique. If we want the hand to reach a little farther, we can constrain it to do so. It is important, however, to balance our springs. If we try to move the hand but the arm joints are too stiff, we might end up dragging the entire body instead of just extending our reach. The nice thing is that this method provides an intuitive way to control the result that you will get. You can weaken the springs or modify the setpoints controlling the arms to bias the movement to the joints of your choice, or you can add additional constraints to keep the waist in place or the feet planted.
The method works well, in real-time, even when the simulated model dimensions and mass are changed drastically, allowing you to quickly retarget the markers to another body model.
Simply changing the model without any other changes will not do great retargeting, of course. Unfortunately, I do not believe that there is a perfect way automatically accomplish high-quality, general-purpose retargeting without a firm understanding of the purpose of the movement. If the movement is clapping hands, you need to constrain the hands to meet at the right time. If the movement is locomotion, you need to constrain the limbs to produce the appropriate ground forces at the right times and you need to decide if a larger body should walk faster because of its longer legs or if it should use a short stride to match the original movement. These decisions cannot really be made automatically because there are situations where an animator might want either. Once you decide what you want, however, you might be able to implement it as a constraint in the physics engine.
Inverse Dynamics
For some purposes, it is useful to know how much effort is required to accomplish a particular movement. Perhaps we wish to control a real robot and need to know the torques to apply at each joint. Perhaps we want a measure of the effort that a person exerts when performing a particular action. Once we have a kinematic sequence of body poses, we can use the physics engine to extract dynamics.
To accomplish this, we simply need to begin with the character model in the appropriate starting state and then constrain the internal joints to reproduce the relative movement that the markers induced at each frame. The constraint solver functions by first dividing the desired velocities by the size of the timestep to produce accelerations. It then does a lot of matrix math to solve, essentially, $F=ma$. Once the solver has found the appropriate forces and torques, the physics engine integrates these to get a change in position and orientation for each body. The torques and forces found along the way are available if they are needed.
Figure 2. Workflow for accomplishing inverse dynamics using ODE. The finite difference in marker data over time provides a velocity constraint on the state of an articulated character model. The finite difference over model poses between different frames of time provides an internal constraint on a character model.
Results
The good news is that these methods are straightforward and robust to implement. Unfortunately, discrepancies between the model and reality will make it so that the dynamic model falls over unless action is taken to stabilize it. In this work, we simply used "Hand of God forces". We attached a joint to the model's waist that would constrain it to reproduce the orientations recorded during the pose-fitting pass. To minimize the effect of these external forces, we limited the amount of stabilizing torque available.
Figure 3. A simulated character imitates a human movement, shifting balance from one foot to the other.
Figure 4. Measured ground forces for the right and left feet (red and green) are very close to the ground forces computed using the physics engine (magenta and blue for right and left feet respectively). Yellow and cyan lines show the external stabilization forces, limited to 30 N m along the pitch and roll axes.
Some validation of this method is available in a simple experiment shown in Figures 3 and 4. Using a pair of Nintendo Wii Balance boards (using the wiiuse library to record data), I measured vertical ground forces for both feet while transitioning from bipedal stance, to balancing on my left and then right leg (Fig. 3). We then recovered the movements and the forces using the method described above (Fig. 4). The computed forces closely match the measured forces, showing that it may be possible to compute inverse dynamics, even with multiple contact constraints. There are some large perturbations during the transition interval, but even these are not terribly serious and can be blamed on discrepancies between the contact surfaces and collision computations (my feet are not, in fact, cylinders) and the fact that the force plates are smoothing their output, but we report the computed values without any filtering.
Conclusion
Although ODE has been described as a tool for creating games, it can be used for many other purposes. The constraints solver provides a simple mechanism for controlling an animated character or even extracting joint torques for quantifying human behavior or controlling other devices. This is pretty cool and easy to do.
Benefits
This approach has some advantages over related work. In general, people solve the inverse kinematics problem by minimizing squared marker error. Noisy markers are a big problem if you do this. A blip in the motion capture that causes a measured marker location to jump causes a big squared error. This kinks the skeleton. This approach also requires a lot of markers to fully specify the skeleton. The ability to bias the skeleton solution with some prior is nice. Although it is probably possible to include additional terms in the error minimization function, multi-objective optimization can be touchy work. On the other hand, setting the spring stiffnesses may be more-or-less the same problem.
For inverse dynamics, the big advantage of this approach is that the entire system (including contact forces) is computed simultaneously.
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.
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.
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.
Advertising and marketing often make my skin crawl. There definitely exists a need for companies producing a product or a valuable service to connect with those who value and will consume the product or service. Consumers want to know the pros and cons, the specs, of different competing products so that they can apply their own utility function to the list of options and procure the one that maximizes their utility.
Unless a business is a clear winner for most people, however, businesses don't want that. Businesses want people to consume their product or service regardless of its relative merits in comparison with competing businesses. This is not to say that they are entirely greedy and corrupt, but it's rational and expected for them to act out of self-interest. Therefore, they need to make consumers aware of their product and possibly convince consumers to change their utility function so that the offered product is a winner: convince consumers that they should really value your product's strengths and not care about your product's weakness. Alternatively they need to obfuscate the true nature of the product (hide the weakness and mislead about the strengths) so that consumers will choose it.
If a marketing company is hired to convince people to buy something they don't need or that isn't the best way to fill their need, then it feels like the marketing company has been hired to deceive. It feels like "Search Engine Optimization" businesses and advertising campaigns are not helping consumers. That's because the consumer hasn't hired them. The business hired them and the business only has money to continue paying them if they can convince consumers to give that money to the business.
There needs to be a way to remove the incentive to deceive, decrease the cost of connecting with consumers, and increase the incentive to make better products. I'm reluctant to ever consider "switching to Geico for my insurance" because I hear their name much too often. They can't possibly be spending enough money on providing me good insurance because they spend so much on advertising. Too much of my premium would be going toward animating a lizard with an accent and buying air time instead of rapidly and satisfactorily settling claims.
There needs to be an organization or group of organizations dedicated to providing the truth without spin. We need a way to see the merits and drawbacks of products and services in a way that benefits consumers.
