Two algorithms from the 60's

The story goes that Michael (Mike) Powell was supposed to give a seminar at the University of Leeds on derivative free optimization one day in the early 60s, probably 1963. In November 1959 William (Bill) Davidon had just released a technical report on what he called the “variable metric method”. He was a physicist at the Argonne National Laboratory that was using gradient (and coordinate) descent methods and came to invent quasi-Newton methods. Powell had got his hands on the report in 1962 and so it seems had Colin Reeves, a chemist working on molecular structure problems and also the PhD advisor of Roger Fletcher at Leeds. Reeves gave the report to Fletcher, who implemented the method, as did Powell. After giving his talk Powell was surprised to find out that Reeves already knew the method and had implemented it. They decided to work together on the method and out came the first paper in 1963 on what was later called the Davidon-Fletcher-Powell (DFP) formula. The irony is that Davidon was rejected for publication and his paper was finally published only in 1991 in the first issue of SIAM Journal on Optimization.

The DFP work later lead to the renowned BFGS method and the whole class of quasi-Newton algorithms. The funny thing about BFGS is that it was not single paper with the acronym coming from the authors' initials, but it was actually four separate papers published independently. The matrix free or limited memory version of BFGS (L-BFGS) later become a very powerful and efficient method (due to Jorge Nocedal). Overall, the DFP, BFGS and similar methods brought significant improvements to the state of numerical optimization at that time which consisted mostly of steepest descent and Newton’s method.

One has to keep in mind that numerical analysis at that time was still incipient and that software libraries were in the process of being written. Every university had its own clunky computer, that filled entire rooms or even floors, and not even Fortran was yet standard. For instance, Davidon used block diagrams in his report and Fletcher coded in Algol. Some people used assembly, other early forms of compilers. These people were pioneers of numerical optimization in those days and many of them were not mathematicians per se. Colin Reeves started as a quantum chemist at Cambridge working on molecular orbitals and later became a computer scientist. Fletcher is described by chemists as a computer scientists who later left molecular simulations for a career in optimization. But after all, he was therefore an applied mathematician until the end. Powell was for most of his career a mathematician working at the Atomic Energy Research Establishment at Harwell, close to Oxford; he later moved to Cambridge as a professor. Davidon was a physicist (although Fletcher majored in physics at Cambridge too) who later became known as a peace activist, anti-Vietnam war protester and mastermind behind the FBI office burglary in 1971. On the other hand, Powell was interested only in the mathematics and not the physics part, as he admits that he could not follow Davidon’s arguments on the variable metric inspired from general relativity and differential geometry.

Fletcher soon went on after the DFP work to develop the nonlinear conjugate gradient (CG) solver for unconstrained nonlinear optimization. He claims that the idea was given to him by Reeves who realized that he could re-use the line search procedure inside the then classic CG solver for linear systems. This resulted in the Fletcher-Reeves method. In fact, Fletcher is of the opinion that he was given two good ideas by other people. In this way, Fletcher helped develop two methods that have become cornerstones of numerical optimization. Although, in a later interview he shared his doubts about NCG being a reliable method for large scale problems. But at that time, NCG and DFP/BFGS were pretty much the only efficient methods that were available in numerical libraries and computational quantum chemistry software suites and probably other places too. And then followed augmented Lagrangian, SQP, interior point methods and all that.

Fletcher was a man that relished practical problems and considered them as drivers of the field of numerical optimization. He considered himself a physicist rather than a mathematician until the end. In an interview he gave in 2015, before passing away one year later, he said the following: “I think for us in numerical analysis – in optimization in particular – because that’s the purpose of it: it’s to optimize; it’s not to produce pure mathematics... in my view, differing from other people’s view. But it’s to solve problems. And we just keep solving the same old problems – getting another 5 % here and another 5 % there: it’s a bit sterile.” I think these are very nice and true words coming from one of the giants of optimization.

Quaternion constraints

This post is mainly a distillation of the information given by Claude Lacoursière in his thesis and in a chapter from the Game Physics Pearls book. It is mostly the result of trying to implement these quaternion based constraints into a game physics engine. I will limit myself to only two examples that seemed most practical and are also part of the understanding process: the totally locked rotational constraint and the hinge joint. I will leave it to you to investigate more complex rotational joint types. My only goal here is to simplify a bit the original texts and give some direct results, ready for implementation.

Let's start from a completely locked quaternion-based rotational constraint:
\begin{equation}
g(q,p)=\mathbb{P}p^*q = G(p)q = 0,
\end{equation}
where $p$ represents the desired relative orientation and $q=p_1^*p_2$ is the relative quaternion between the two bodies. By $p_i$ we mean the constraint frame orientation quaternion in world space relative to body $i$. The projection matrix $\mathbb{P}$ extracts the vector part from a quaternion: $q_v = \mathbb{P}q$ and $q=(q_s, q_v)$.

$G$ is a helper matrix used to convert quaternion operations to matrix operations. We will be using two such matrices:
\begin{equation}
E(q) =
\begin{bmatrix}
-q_v && S(q)
\end{bmatrix},
\end{equation}
\begin{equation}
G(q) =
\begin{bmatrix}
-q_v && T(q)
\end{bmatrix}.
\end{equation}
The 3 by 3 matrices $S$ and $T$ are the transpose of each other so we can only use the latter:
\begin{equation}
\label{tmatrix}
T(q) = q_s I_3 + [q_v]^{\times},
\end{equation}
where the skew-symmetric matrix of a vector $v=(x,y,z)$ is:
\begin{equation}
    [v]^{\times}=
    \begin{bmatrix}
    0 && v.z && -v.y \\
    -v.z && 0 && v.x \\
    v.y && -v.x && 0
    \end{bmatrix}.
\end{equation}
Note that we are using a convention different than the one used by Lacoursiere, i.e. the transpose of his matrix (or with a flipped sign as for any skew-symmetric matrix $A$ we have $A^T=-A$).

In general, the time derivative of the constraint function (i.e. the velocity error) is of the form:
\begin{equation}
\label{velconstr}
\dot{g}=J_1 \omega_1 + J_2 \omega_2,
\end{equation}
where $J_1$ and $J_2$ are the angular Jacobians corresponding to the two bodies and we denote by $\omega$ their angular velocities. Note that I have considered that the constraint function is not time dependent; so make sure the constraint frame is not changing in time, otherwise you will have to include the time derivative of $g$ too.

I will now give the formula for the Jacobian. Keeping in mind that $J_2 = -J_1$ we have:
\begin{equation}
    J_1 = -\tfrac{1}{2}G(p_1 p) E^T(p_2).
\end{equation}
Using the desired target relative quaternion $p$ is a good thing when you want to set up a joint motor or to set up a locked constraint without actually providing a constraint frame (by setting $p$ to the initial relative orientation between the bodies, i.e. $p=q_1^*q_2$ using the body orientations at the time the constraint is set up). However, for most practical reasons (like implementing a hinge) you will require a constraint frame to be defined and then all is required is that $p$ is the identity quaternion (and thus $q$ too):
\begin{equation}
g(p)=\mathbb{P}q=q_v=0.
\end{equation}
That simply means that the two constraint frames expressed in the two body local frames should overlap (for a locked joint at least). We only need three constraint equations because there are only three rotational degrees of freedom. This is also why we use unit quaternions (as the $q_s=1$ constraint becomes implicit).

After getting rid of $p$, we can further simplify the Jacobian so that it  only consists of 3 by 3 matrices:

\begin{equation}
    J_1 = -\tfrac{1}{2}[q_{1,v} q_{2,v}^T + T(q_1)T(q_2)].
\end{equation}

A hinge constraint is defined as following:

\begin{equation}
    g(q) =
    \begin{bmatrix}
    a^T \\
    b^T
    \end{bmatrix}
    \mathbb{P}q = P_{hinge}q=
    \begin{bmatrix}
    a \cdot q_v \\
    b \cdot q_v
    \end{bmatrix},
\end{equation}
where $a$ and $b$ are two perpendicular unit vectors in the constraint frame - their cross product will be the hinge axis. Most of the time we will choose them to be two of the coordinate system axes.

For solving the constraint we define two vectors $u$ and $v$ as the columns of $J_1^T$ corresponding to the coordinate axes chosen as $a$ and $b$. Then the velocity error becomes:
\begin{equation}
    \eta=
    \begin{bmatrix}
    u^T \\
    v^T
    \end{bmatrix}
    (\omega_1 - \omega_2).
\end{equation}
If we define the symmetric matrix:
\begin{equation}
    \tilde{A}=
    \begin{bmatrix}
    u^TAu && u^TAv \\
    v^TAu && v^TAv
    \end{bmatrix},
\end{equation}
then the hinge constraint reduces to solving the 2 by 2 linear system:
\begin{equation}
    \tilde{A}\lambda + \eta + \tfrac{\beta}{h}g(q)=0.
\end{equation}

Transversal stability of constraints

Often, constraint solvers get unstable, especially if they are working at velocity level. Actually the root cause is that they are explicit solvers, so both SHAKE and velocity time stepping (VTS) can suffer from similar problems. Interestingly enough, SHAKE right away explodes if it's stressed enough, while VTS manifests what is known as "jittering" (in the same conditions). And so far, nobody really knew what jittering really is and how to measure it. In what follows I will attempt to formulate jittering mathematically as oscillations transversal to the constraint directions. Actually, this fact was gradually hinted by a series of papers: [Servin et al. 2011], [Kaufman et al. 2014], [Tournier et al. 2015], [Andrews et al. 2017]. But what I'm trying to do is show that this is formally true, at least for bilateral constraints.

Another important thing to note is that instabilities don't come from the constraints themselves, but from how constraint forces are distributed to the other constraints and how these constraints are geometrically related to each other. This is is the essence of the geometric stiffness concept: that constraint forces and constraint orientations can act as stiff forces (e.g. angular springs) in the transversal directions. Take as example a thread made of particles and massless rigid constraints between them: the constraint forces that act along constraints to restrict half of the degrees of freedom (DOFs), also act on the rest of the DOFs (i.e. the angles of the multi-pendulum) and couples them together (like in the chaotic behavior of the double pendulum).

Consider we are given a dynamical system with $n$ degrees of freedom and $m$ constraints. I will consider the case $m<n$ here, and leave the over-constrained case for future treatment. The constraint are given implicitly by $m$ constraint functions: $\psi(q)=0$. In fact, these functions measure the displacement along the constraint direction, so they can be considered as a coordinate.

The trick I will use is to consider a coordinate change from $q$ to another set of coordinates $(\psi, \theta)$. As you can see, we already know $m$ coordinates given by $\psi$, and we need to pull out the rest of $n-m$ coordinates $\theta$ from somewhere. This is quite simple to do for a thread (multi-pendulum) where we have closed formulas for all the coordinate changes, but not so straightforward for the general case. Turns out we don't need to know these nonlinear transformation formulas, but rather their Jacobians. This approach is inspired from an older way of solving constraints, known as the local coordinates approach. The main reference used here will be [Potra and Rheinboldt 1991] (On the Numerical Solution of Euler-Lagrange Equations).

The Jacobian of $\psi(q)$ is denoted by $J=\nabla_q \psi(q)$ and is usually easy to compute and necessary for constraint solving. This is not true though for $L$, the Jacobian of $\theta(q)$. But [Potra and Rheinboldt 1991] show that $L$ exists locally and can be computed numerically (through QR factorization of J). Moreover, it can be chosen so that $L^TL=1$ (i.e. orthonormal). Once we have $L$ (even if locally), we can start looking at the equations of motion and how they would look like in $\theta$-space (i.e. transversal direction). So let's see the equations of motion in maximal coordinates $q$ with only constraints and no other forces:
\begin{equation}
M\ddot{q} = J^T \lambda.
\end{equation}
Move the mass on the right-hand side and multiply to the left by $L$:
\begin{equation}
L\ddot{q} = L M^{-1} J^T \lambda.
\end{equation}
We do this because we know the fact that $L=\nabla_q \theta(q)$ and so $\dot{\theta}=L\dot{q}$. Differentiate another time and we get $\ddot{\theta}=L\ddot{q} + \dot{L}\dot{q}$. Therefore we get:
\begin{equation}
\ddot{\theta} = L M^{-1} J^T \lambda + \dot{L}\dot{q}.
\end{equation}
I will call the last term a gyroscopic term, as it only appears because of the nonlinear change of coordinates and I will ignore it from now on, because I know it's not the culprit for instability. But I do know that the term  $L M^{-1} J^T \lambda$ is the one that's stiff. Most of this insight came (to me and others) from the first two papers mentioned in the first paragraph. So, in order to quantify the stiffness, we need to linearize this term (keep in mind we are already in a restricted vicinity of $q$ because of $L$). So let's differentiate the matrix $A = L M^{-1} J^T$ with respect to $\theta$:
\begin{equation}
\frac{\partial A}{\partial \theta}=\frac{\partial L}{\partial \theta} M^{-1} J^T + L M^{-1} \frac{\partial J^T}{\partial \theta}.
\end{equation}
But here's the catch: the transversal directions $L$ should not depend on the transversal coordinates $\theta$ but only on the other coordinates $\psi$. A similar choice of constraint directions $J$ can be made so that they only depend on $\theta$. What I cannot prove right now is that such a choice for both $J$ and $L$ always exists. However, I will assume it does, and continue based on that - namely, that $\frac{\partial L}{\partial \theta}=0$. Another thing we want to do, is express all terms as much as possible in terms of $\theta$, so we note that:
\begin{equation}
\frac{\partial J^T}{\partial \theta}= L^T \frac{\partial^2 \psi}{\partial \theta^2}.
\end{equation}
We can now finish our linearization, and after dumping the gyroscopic term, we get the following approximate local equation:
\begin{equation}
\ddot{\theta} = C + LM^{-1}L^T \left( \frac{\partial^2 \psi}{\partial \theta^2}\lambda \right) \theta = C + EK\theta,
\end{equation}
where $C$ is a constant term, $E$ is the inverse effective mass in transversal space and $K$ is the stiffness matrix as seen by the free degrees of freedom $\theta$. What we would like to show now is that there is a relation between this matrix $K=\frac{\partial^2 \psi}{\partial \theta^2}\lambda$ and the geometric stiffness $\tilde K = \frac{\partial^2 \psi}{\partial q^2}\lambda$. Given that $\frac{\partial^2 \psi}{\partial \psi^2} = \frac{\partial^2 \psi}{\partial \theta \partial \psi} = 0$, I was able to show that $\tilde K = L^T K L$. I may have to go again through the calculations, but it makes sense that the matrix $K$ is "pushed forward" in $q$-space and vice-versa.

Now, let's try and bring the last equation from transversal space back to $q$-space. Note that we linearized and approximated the original equations, so what we are bringing back now is basically just a portion of all the forces, but most importantly they are the stiff forces. This why we also leave out the constant term. Therefore, we multiply the equation to the left by $L^T$:
\begin{equation}
L^T\ddot{\theta} = L^T LM^{-1}L^T K \theta.
\end{equation}
We can now use the fact that $L^TL=1$:
\begin{equation}
\ddot{q} = M^{-1}L^T K \theta(q).
\end{equation}
We need again to linearize this equation (around a point $q^*$), so that stiffness shows its face:
\begin{equation}
\ddot{q} = M^{-1}L^T K L (q - q^*) + M^{-1}f^*.
\end{equation}
It is clear now to see that:
\begin{equation}
M\ddot{q} = \tilde K (q - q^*) + f^*.
\end{equation}
Again, we should ignore the constant terms and notice the fact that the stiffness matrix of the transverse forces in $q$-space is just $\tilde K$ - the geometric stiffness. Of course, this is just a linearization of the real force, but it is what matters locally, especially when doing numerical integration. Linear stability analysis is all we have, so the values of the $\tilde K$ matrix are very important for stability analysis in $q$-space (as the values of $K$ are in $\theta$-space). If the highest eigenvalue $\omega^2$ of $M^{-1}\tilde K$ does not satisfy $\omega < 2/h$, then the simulation becomes unstable. This is an idea that has already been proposed in [Andrews et al. 2017], but I was just not sure if it was true. Now that I've shown it is, it's time to focus on the modal analysis of $M^{-1}\tilde K$ as the proper way of studying stability in constrained based dynamics.

Implicit velocity projection

During the time I spent looking closer at PBD and what it really is I learned one clear thing: PBD is nothing else than a form of implicit integration (Backward Euler) of the constraint forces. Goldenthal calls it Implicit Constraint Directions (ICD). His optimized version he called Fast Projection (FP). FP solves a linear system at every nonlinear iteration. PBD runs just one Gauss-Seidel (or Jacobi) step at every iteration, resulting in what is known as Nonlinear Gauss-Seidel (NGS). 

The other thing I learned is that PBD is very stable just because it is fully implicit. And this is why you can do cloth with PBD, but not with velocity based methods, which I'll denote hereon as velocity time stepping (VTS). This is because articulated systems can get very unstable with VTS. The short explanation is that VTS is using the constraint direction at the beginning of the frame (i.e. explicit directions) and not the one at the end (implicit directions). Therefore a part of the system is integrated explicitly (the part transversal to constraints) and it is well known that explicit integrators can get easily unstable and even blow up. The good part is that VTS is very similar to SHAKE (pretty much a linearized version of it) which is symplectic. This means energy gets conserved better in the transverse direction - I will detail these things in another post.

Unfortunately conserving energy is not always what we want. Stability comes a lot of time for free from implicit integrators because they dissipate energy artificially. This may be bad for science projects, but not for games, graphics and real-time simulation. Therefore I've been looking for a way to make VTS stable without going really the PBD way. The differences between the two comes a lot from what we are trying to solve for: VTS solves for velocity constraints, PBD for position constraints. So how can we keep the VTS way without altering it too much, and also get the stability of PBD? The answer: integrate using implicit constraint directions but keep the velocity constraints.

Let's throw in some equations for this case:

\begin{align}
Mv^{l+1} &= Mv^l +hf_{ext} + hJ(x^{l+1})^T \lambda \\
x^{l+1} &= x^l + hv^{l+1} \\
0 &= \tfrac{\beta}{h} c(x^l) + J(x^l)v^{l+1}
\end{align}

We can express this as a system in the unknowns $v$ and $\lambda$:
\begin{align}
Mv &= Mv^l +hf_{ext} + hJ(x^l + hv)^T \lambda \\
0 &= \tfrac{\beta}{h} c(x^l) + J(x^l)v = G(v)
\end{align}

While the second equation is linear, the first one is not and we need to apply Newton's method to it. The Jacobian for the whole system then becomes:

\begin{equation}
\begin{bmatrix}
M-h^2 H(x)\lambda & -h J(x)^T \\
J(x^l) & 0
\end{bmatrix},
\end{equation}
where $x = x^l + hv$.We can proceed just like in the case of FP and ignore the geometric stiffness term $K=H\lambda$, where $H=\nabla J = \nabla^2 c$, and also set the residue of the first of the first equation to be always 0. We then get the following linear system:
\begin{equation}
\begin{bmatrix}
M & -h J(x_0)^T \\
J(x^l) & 0
\end{bmatrix}
\begin{pmatrix}
\Delta v_1 \\ \Delta \lambda_1
\end{pmatrix} =
\begin{pmatrix}
0 \\ -G(v_0)
\end{pmatrix}
\end{equation}

We can now take directly a Schur complement and obtain the usual constraint solver equation:

\begin{equation}
hJM^{-1}J^T\Delta\lambda_1 + G(v_0) = 0,
\end{equation}

where we can take by approximation $J$ to be the same on both side, i.e. $J(x_0)$. Then the change  in velocity is $\Delta v_1 = hM^{-1}J^T\Delta\lambda_1$ and the update step is given by:
\begin{align}
v_1 &= v_0 + \Delta v_1, \\
x_1 &= x^l + h v_1.
\end{align}
If you iterate this scheme you get implicit VTS and it's way more stable than VTS. I only tested this on a 2D thread/cable made of distance constraints and I haven't got to make measurements, but it looks just like PBD. But the algorithm looks a lot like VTS - there are two main differences though:

1. at every iteration, after you compute the new velocities, you compute new positions too, and
2. at every iteration you recompute the Jacobians of the constraints based on the new positions.

Otherwise, everything stays the same. You could use an exact solver for the Lagrange multiplier or better just use one Gauss-Seidel step, which results basically in NGS. The only trick is that you don't set $\beta$ too close to 1 or it explodes (why, I don't know yet).

Here's how it looks like for some extreme initial velocity conditions:

The same scenario, using VTS.

And for completeness, the PBD case.

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");

Making progress with the uber-solver

Here are some videos I took live from my physics engine application. They are real-time although not at 60 Hz as the simulation time step is set, but the code is not yet optimized and it was run on a i5 laptop. As soon as I get it on the desktop PC and start parallelizing I shoulg get more speed.

Anyway there's still a lot of work to do. The highlight for now is that I am able to simulate rigid bodies (and particles) with friction and cloth in the same solver (using PBD). The soft body (cow) is using a special version of the finite element method based on constraints and it's also running in the same solver with the cloth, hence the 2 way coupling. Constraint FEM vs. rigids is on the way and at some point they will all run together.