Physics-based skeleton fit, inverse kinematics, inverse dynamics, and retargetting

Introduction

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. 

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.

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.

Grass-roots Reverse Advertising

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.

Project: Extruded 3D text

I work with 3D virtual environments.  I've had multiple occasions to place 3D text in the world as part of a 3D user interface or for other purposes.  I've been quite happy with FTGL which allows me to load a font and produce extruded text.  However, I was thinking that it would be fun to add to this. 

FTGL is great.  It uses the Freetype library to load and parse the fonts.  On top of the 3D modeled fonts, you can also generate texture fonts and other methods for displaying text with OpenGL.  It also has utilities for layout.  However, there are a few things that I wish it could do.  Primarily, I wish that it was a little better about exposing the OpenGL.  The display-lists that it uses tend to fight with the display lists that I use.  I would also like to have direct access to the vertices that are used to render the text.  This is handy if I want to apply texture to the text or if I want to apply perturbations to the vertices (dents or protrusions).  Once I have the vertices, I can do whatever I want with them. 

While we're at it, perhaps we can improve upon the extrusion capabilities by incorporating the GLE tubing and extrusion library.  Some of this functionality might already exist in gle32.

Hypersphere from points on surface

If you want to compute the center and radius of a hypersphere in $\mathbb{R}^n$, you need $n+1$ points ($p_i$ for $i=1$ to $n+1$) to fully specify it.  The $j^{th}$ coordinate of the $i^{th}$ point is $p_{ij}$.  The center point is $m$ with scalar radius $r$.

We'll consider two formulae:

    1.  $\sum_{j=1}^n(q_j-m_j)^2 - r^2 = 0$ (any point $q$ for which this is true is a point on the surface of the sphere).
And:
     2.  $\sum_{j=1}^n(aq_j^2 + b_jq_j) + c = 0$ (where $a$, $b_j$, and $c$ are scalar coefficients).

For reasons that are not entirely clear to me (but this discussion on finding a sphere from 4 points deserves the credit), we can write this equation as the determinant of a matrix:

$\left| \begin{array}{ccccc}
\sum_{j=1}^n q_j^2 & q_1 & \cdots & q_n & 1\\
\sum_{j=1}^n p_{1j}^2 & p_{11} & \cdots & p_{1n} & 1 \\
\vdots & & \ddots & & \vdots \\
\sum_{j=1}^n p_{(n+1)j}^2 & p_{(n+1)1} & \cdots & p_{(n+1)n} & 1
\end{array}\right| = 0$

When we find this determinant in terms of all the $q_j$s, it will be in the form of Eqn. 2 above.  Since we want to extract the center point and radius, we'll make Eqn. 1 meet us half-way by expanding the squared part to get:

    3.  $\sum_{j=1}^n(q_j^2-2q_jm_j+m_j^2) - r^2 = 0$

In this form, it's clear that the $q_j^2$ components don't have coefficient $a$.  So we divide that into the $b_j$ and $c$ components of Eqn. 2:
    4.  $\sum_{j=1}^{n+1}(q_j^2 + \frac{b_j}{a}q_j) + \frac{c}{a} = 0$

Then we can make all of the pieces fit into Eqn. 3:
$\begin{align}
-2m_j &= \frac{b_j}{a}\\
\sum_{j=1}^{n+1}m_j^2 - r^2 &= \frac{c}{a}
\end{align}$

And from there, it's straightforward to find $m_j$ and $r$. 

 
Now, if you happen to have more than $k>n+1$ points, what I want to do is find the fit that minimizes $\sum_{i=1}^k\left|\sum_{j=1}^n(p_{ij}-m_j)^2 - r^2\right|$, which is to say, the sum of squared distances from the surface of the sphere. 

I can't seem to find a way to do that without using a numerical, non-linear optimization approach (not too far removed from, guess and check to see if a nearby solution is better than what you've found so far).  If anyone happened to stumble upon this page and knew of a fancier method, that would be a nice comment to leave for posterity.  Or if you can explain why the determinant thing works, we'd all appreciate that too.