Holonomic constraint forces using Lagrange multipliers

Question

Consider a multibody robotic system with Lagrangian $$ L =\frac{1}{2} \dot{q}^{T}M\dot{q} - V $$ and equations of motion of the form $$M(q)\ddot{q}+C(q,\dot{q})\dot{q}+A^{T}(q)\lambda+N(q)=0 $$

where $N=\frac{\partial V(q)}{\partial q}$ and $\lambda$ represents the Lagrange multipliers for the constraint forces (MLS pg. 269; pg. 287 in pdf). $A$ is the constraint matrix from the Pfaffian form $A\dot{q}=0$. We are concerned with holonomic constraints, which can be differentiated to obtain the Pfaffian form. $ C=\sum_{k=1}^{n} \Gamma_{ijk} \dot{q_{k}} $ where $\Gamma_{ijk}$ are the Christoffel symbols of the first kind.

The Lagrange multipliers can be calculated using the formula (MLS pg. 270, pg. 288 in pdf) $$\lambda =-(AM^{-1}A^{T})^{-1}AM^{-1}(C\dot{q}-N)$$

This gives the holonomic constraint forces $A^{T}\lambda$.

I would like to see some examples of this method for calculating the holonomic constraint forces for planar systems.

I have seen this method being applied to calculate the tension in a simple pendulum (MLS pg. 270, pg. 288 in pdf), but I would like to see examples of planar systems with more degrees of freedom.

I'm specifically interested in the case of a 2R manipulator shown below.

The equations of motion are on pg. 164-165 (pg. 182-183 in the pdf) in MLS.

My objectives are:

1. Apply the holonomic constraint $\theta_{1}(t)=0$ and obtain the holonomic constraint force.
2. Show that we get the dynamics of a simple pendulum for the constrained system for initial conditions $\theta_{2}(0)=0, \dot{\theta_{2}}(0)=0$.

I have done the calculation for the constraint $\theta_{1}(t)=0$ but I'm not able to show 2. The expression I get for $\lambda$ is

$$\lambda= \begin{array}{c} \frac{g (d \text{m2} \text{r2} (\sin \text{t2})-\text{m2} \text{r2} (\cos \text{t2}) ((\cos \text{t2}) b+d))+b d (\sin \text{t2}) \dot{\text{t2}} \dot{\text{t2}}}{d} \\ \end{array} $$

where $(t1, t2)=(\theta_{1},\theta_{2})$ and $a, b, d$ are the geometric and inertial parameters $\alpha, \beta, \delta$ described in MLS (pg. 165; pg. 183 in the pdf).

Suggestions for other methods to calculate holonomic constraint forces would be helpful too.

References:

MLS refers to

Murray, R. M., Li, Z., Sastry, S. S., & Sastry, S. S. (1994). A mathematical introduction to robotic manipulation. CRC press.

Answer 1

With multiple bodies it starts getting so complicated that I would forego doing it by hand and instead implement it in MATLAB or python using matrices. There are a few ways to do that (Lagrange isn't actually the most optimal one, as you're doing some unnecessary conversion to the energy domain), but it can be used. I could type out some equations, or post screenshots of my book. But my prof probably dos a far better job of explaining it:

https://www.youtube.com/watch?v=20v3XVnKP8k&t-27m25s