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:
- Apply the holonomic constraint $\theta_{1}(t)=0$ and obtain the holonomic constraint force.
- 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.