Controlling a pendulum with constraints

Of course, the most straightforward way to express the equations of pendulum and control it is by using one single angular degree of freedom. But the way you would implement a pendulum in a physics engine is through a constraint (in particular a hinge). Of course in 3D and for rigid bodies things a little more complicated, but I will stick for now with a simple 2D point-mass pendulum. So without further ado, here's how the constrained equations of a pendulum would look like in Cartesian space:

\[
\begin{align}
\ddot{x}&=\lambda x, \\
\ddot{y}&=g + \lambda y, \\
\Theta(x,y)&=\tfrac{1}{2}(x^2+y^2-l^2)=0.
\end{align}
\]
Next we add an angular motor in the hinge. We could be targeting a certain angle $\theta^*$ or a certain velocity $\dot{\theta}^*$ or both. The position level constraint looks like this: $\Psi(\theta)=\theta - \theta^*=0.$ The velocity level constraint is then: $\dot{\Psi}(\dot{\theta})=\dot{\theta} - \dot{\theta}^*=0.$
Mixed together we get $\dot{\Psi}(\dot{\theta})+\tfrac{\beta}{h}\Psi(\theta)$, where $\beta$ is a bias (or Baumgarte stabilization) factor. However, we need to express these constraints in Cartesian space too. We are mainly interested in going from Cartesian coordinates $(x,y)$ to the angle $\theta=\text{atan}(\tfrac{x}{y})$. The Jacobian of this transform is also the Jacobian of the motor constraint: $\mathbf{J}_M=\tfrac{1}{x^2+y^2}[y \, -x]$ (which makes sense as the controlling force should be perpendicular to the pendulum link, i.e. tangent to the circle). We can now express the velocity level constraints as: $\mathbf{J}_M\mathbf{v} - \dot{\theta}^*= 0$, where $\mathbf{v}=[\dot{x} \, \dot{y}]^T$.

At this point, before delving into more detail, we can stop for a minute and note a couple of things. First we can see that in general motors correspond to the actuated degrees of freedom; in the case where all DOFs are actuated the Jacobian of the motor constraints are then taking you from Cartesian velocities to reduced coordinates velocities. Second, for a case like this (of the pendulum), where converting back and forth between maximal and reduce coordinates is trivial you can actually compute the target angle and/or target angular velocity and feed it directly to the motor. This may no longer be the case for long articulated chains where motion for the end-effector or all joints is prescribed.

So, alternatively, we can describe the control constraint as acting directly on the point-mass, i.e. to target a certain position: $\Phi(\mathbf{q})=\mathbf{q}-\mathbf{q}^*=0$, where $\mathbf{q}=[x \, y]^T$. The time derivative is trivial and we can see that the Jacobian is the 2 by 2 identity matrix. So now we can replace the motor constraint by this new constraint and it will achieve the same net effect if we compute $\mathbf{q}^*$ from a given $\theta^*$. Remember that we still have the length constraint $\Theta$ in both cases. In general $\Phi$ might fight against $\Theta$. In the case of our pendulum this results in a singular system matrix. Even in the case of the powered hinge, we need to lower $\beta$ to around 0.1 so that the length constraint is not violated. In the case of $\Phi$, we choose to solve it decoupled (i.e. separately) from $\Theta$, as it already removes all the degrees of freedom from the system.

Now let's consider we have a solver that computes Lagrange multipliers (i.e. constraint force magnitudes) for all constraints. Conceptually, if we use $\Phi$ instead of $\Psi$ the difference is that we apply a force directly at the point-mass instead of applying a torque at the hinge (like you do in robotics). For our pendulum case, the difference is not that big in Cartesian space, as you end up with a force only in the end, but things can get messier for rigid bodies. Our aim is then to only apply those forces and torques that we can apply (through actuators). So we need to convert from "godly" constraint forces to motor forces (for actuated DOFs only).

Virtual work comes to the rescue and we can equate the work of the torque to that of the constraint forces: $\delta W= \tau \delta \theta=\lambda_E^T \mathbf{J}_E$, where $\tau$ is the motor Lagrange multiplier and the other multipliers are denoted by $\lambda_E$ (E stands for "end-effector" constraint $\Phi$).

Knowing that $\delta \theta = \mathbf{J}_M \delta \mathbf{q}$, we end up equating constraint forces:
\[
\begin{equation}
\label{eq}
\mathbf{J}_M^T \tau = \mathbf{J}_E^T\lambda_E.
\end{equation}
\]

The problem with this equation is that it's an over-determined system in $\tau$. The idea that comes to mind is to use a pseudo-inverse solution. So we multiply to the left by $\mathbf{J}_M$. The solution we get in the end for the pendulum is:
\[ \tau = y\lambda_x - x\lambda_y, \]
which is nothing else but the cross product in 2D of the radial vector and the constraint force $f_E=\lambda_E=[\lambda_x \, \lambda_y]^T$. And this makes sense as this just the definition of the torque. So this is a strong indicator that this a general rule and we can just go ahead and solve the rectangular system (in a least squares sense) when the number of actuated DOFs does not match the number of goal constraints. The system can still be expressed like in \eqref{eq}, but the Lagrange multipliers will have different sizes.

To sum it up, it is enough to have a constraint solver that can first solve the regular constraints and and then the goal constraints (e.g. end-effector). After that we only apply the regular constraint forces, and we use the goal ones to compute the motor forces (by solving the extra rectangular system) and only then apply the latter.

Here is the accompanying Octave code:

numFrames = 300;
length = 1;
gravity = -10;
h = 0.001;
beta = 1;

x = zeros(numFrames,1);
y = zeros(numFrames,1);
x(1) = 0;
y(1) = -length;
vx = zeros(numFrames,1);
vy = zeros(numFrames,1);
vx(1) = 5;
vy(1) = 0;

thetaTarget = 0.5;
xTarget = length * sin(thetaTarget);
yTarget = -length * cos(thetaTarget);

for i=1:numFrames-1
  vxNew = vx(i);
  vyNew = vy(i) + h * gravity;
  lenSqr = x(i)^2 + y(i)^2;
  error = 0.5 * (lenSqr - length^2);
  lambda = -(x(i) * vxNew + y(i) * vyNew + beta * error / h) / lenSqr;

  error1 = x(i) - xTarget;
  error2 = y(i) - yTarget;
  lambda1 = -(vxNew + 0.1 * error1 / h);
  lambda2 = -(vyNew + 0.1 * error2 / h);
  tau = y(i) * lambda1 - x(i) * lambda2;

  vx(i+1) = vxNew + lambda * x(i) + tau * y(i);
  vy(i+1) = vyNew + lambda * y(i) - tau * x(i);
  x(i+1) = x(i) + h * vx(i+1);
  y(i+1) = y(i) + h * vy(i+1);
end

plot(x, y, "marker", "o");