30 Example 8

Author

Affiliations

Enrico Bertolazzi

University of Trento, Department of Industrial Engineering

Example taken from [1] example 2.1.

30.1 The DAE

We consider an index-five DAE system that models the planar motion of a crane.
The goal is to determine the horizontal velocity u_1 and the angular velocity u_2 of a winch of mass M_1, so that the attached load of mass M_2 follows a prescribed path p_1(t) and p_2(t).

The DAEs, derived from Newton’s second law, are given by:

\left\{ \begin{aligned} x' &= v_x \\ z' &= v_z \\ d' &= v_d \\ r' &= v_r \\ M_2 v_x' &= -\tau \sin\theta \\ M_2 v_z' &= - \tau \cos\theta + m g\\ M_1 v_d' &= - C_1 v_d + u_1 + \tau\sin\theta \\ J v_r' &= - C_2 v_r - C_3 u_2 + C_3^2 \tau \\ x &= r\sin\theta + d \\ z & = r\cos\theta \\ x &= p_1(t) \\ z &= p_2(t) \end{aligned} \right.

where:

(x, z) denotes the position of the load,
\tau is the tension in the cable,
\theta is the angle of the cable with the vertical,
J is the moment of inertia of the winch,
d is the position of the trolley,
r is the length of the cable.

The constants satisfy C_1 > 0, C_2 > 0, C_3 > 0, g > 0, and m > 0.

Semi-Explicit Formulation

For the previous DAE system introduce dummy variables

v_{x,\text{dot}}
v_{z,\text{dot}}
v_{d,\text{dot}}
v_{r,\text{dot}}

and the system decomposes into differential (blue) and algebraic (red) components:

\left\{ \color{blue} \begin{aligned} x' &= v_x \\ z' &= v_z \\ d' &= v_d \\ r' &= v_r \\ v_x' &= v_{x,\text{dot}} \\ v_z' &= v_{z,\text{dot}} \\ v_d' &= v_{d,\text{dot}} \\ v_r' &= v_{r,\text{dot}} \end{aligned} \right. \quad \bigoplus \quad \left\{ \color{DarkRed} \begin{aligned} 0 &= M_2 v_{x,\text{dot}}+\tau \sin\theta && \text{(Constraint 1)}\\ 0 &= M_2 v_{z,\text{dot}}+\tau \cos\theta - m g && \text{(Constraint 2)}\\ 0 &= M_1 v_{d,\text{dot}}+C_1 v_d -u_1 -\tau\sin\theta && \text{(Constraint 3)}\\ 0 &= J v_{r,\text{dot}} +C_2 v_r + C_3 u_2 - C_3^2 \tau && \text{(Constraint 4)} \\ 0 &= r\sin\theta + d - x && \text{(Constraint 5)}\\ 0 &= r\cos\theta - z && \text{(Constraint 6)}\\ 0 &= p_1(t) - x && \text{(Constraint 7)}\\ 0 &= p_2(t) - z && \text{(Constraint 8)} \end{aligned} \right.

For this DAE we have:

Differential variables: x, z, d, r, v_x, v_z, v_d, v_r .
Algebraic variables:- v_{x,\text{dot}}, v_{z,\text{dot}}, v_{d,\text{dot}}, v_{r,\text{dot}}, u_1, u_2. \tau, \theta

The algebraic variables do not appears linearly in the constraints. In particular \theta appears nonlinear as \sin\theta and \cos\theta. Introducing the new dummy variable \theta_{\text{dot}} =\theta' we have for the differential part:

and now

Differential variables: x, z, d, r, v_x, v_z, v_d, v_r, \theta.
Algebraic variables:- v_{x,\text{dot}}, v_{z,\text{dot}}, v_{d,\text{dot}}, v_{r,\text{dot}}, u_1, u_2. \tau, \theta_{\text{dot}}

and not constraints are linear in the algebraic variables.

Index Reduction Procedure

We attempt to solve for v_{x,\text{dot}}, v_{z,\text{dot}}, v_{d,\text{dot}}, v_{r,\text{dot}}, but the system is not yet solvable explicitly. We proceed by differentiating.

Step 1: Differentiate algebraic constraints

Now we must derivate the constraints in order to reduce the index. Here we want to differentiate only the algebraic equation that can reduce the index and do not introduce new differential equation. Here we want to avoid to introduce derivatove of the algebraic variable unless is strictly necessary.

We construct a dependency table to identify which constraints to differentiate:

\begin{array}{c|cccccccc} & v_{x,\text{dot}} & v_{z,\text{dot}} & v_{d,\text{dot}} & v_{r,\text{dot}} & \;\;u_1\;\; & \;\;u_2\;\; & \;\;\tau\;\; & \theta_{\text{dot}} \\[1em] \hline \\[0.1em] \text{(Constraint 1)} & 1 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\[0.5em] \text{(Constraint 2)} & 0 & 1 & 0 & 0 & 0 & 0 & 1 & 0 \\[0.5em] \text{(Constraint 3)} & 0 & 0 & 1 & 0 & 1 & 0 & 1 & 0 \\[0.5em] \text{(Constraint 4)} & 0 & 0 & 0 & 1 & 0 & 1 & 1 & 0 \\[0.5em] \text{(Constraint 5)} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\[0.5em] \text{(Constraint 6)} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\[0.5em] \text{(Constraint 7)} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\[0.5em] \text{(Constraint 8)} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{array}

a 0 means the the function do not depend on the variable on the column. A 1 means dependence. The last four constraints are candidate for differentiation as it doesn’t introduce new algebraic variables. Differentiate thus the last constraints and substitute known variable in differential form using the ODE part

\begin{aligned} \text{(Constraint 5)}'\quad 0 & = v_r\sin\theta +r(\cos\theta)\theta_{\text{dot}}+v_d-v_x \\ \text{(Constraint 6)}'\quad 0 & = v_r\cos\theta - r(\sin\theta)\theta_{\text{dot}}-v_z \\ \text{(Constraint 7)}'\quad 0 & = p'_1(t) - v_x \\ \text{(Constraint 8)}'\quad 0 & = p'_2(t) - v_z \end{aligned}

so that algebraic parte of the DAE become:

\begin{aligned} & \left\{ \color{DarkRed} \begin{aligned} 0 &= M_2 v_{x,\text{dot}}+\tau \sin\theta && \text{(Constraint 1)}\\ 0 &= M_2 v_{z,\text{dot}}+\tau \cos\theta - m g && \text{(Constraint 2)}\\ 0 &= M_1 v_{d,\text{dot}}+C_1 v_d -u_1 -\tau\sin\theta && \text{(Constraint 3)}\\ 0 &= J v_{r,\text{dot}} +C_2 v_r + C_3 u_2 - C_3^2 \tau && \text{(Constraint 4)} \\ 0 &= v_r\sin\theta +r(\cos\theta)\theta_{\text{dot}}+v_d-v_x && \text{(Constraint 5)}'\\ 0 &= v_r\cos\theta - r(\sin\theta)\theta_{\text{dot}}-v_z && \text{(Constraint 6)}'\\ 0 &= p'_1(t) - v_x && \text{(Constraint 7)}'\\ 0 &= p'_2(t) - v_z && \text{(Constraint 8)}'\\ \end{aligned} \right. \\ & \left\{ \color{DarkGreen} \begin{aligned} 0 & = r\sin\theta + d - x &\qquad\qquad\qquad\quad& \text{(Constraint 5)}\\ 0 & = r\cos\theta - z && \text{(Constraint 6)}\\ 0 & = p_1(t) - x && \text{(Constraint 7)}\\ 0 & = p_2(t) - z && \text{(Constraint 8)} \end{aligned} \right. \end{aligned}

The red algebraic equations are the new algebraic equation after the derivation. The green algebraic equation is the hidden constraints that is removed from the DAE system but that is satisfied by choosing the right initial conditions.

The red part cannot determine the algebraic variable and must be differentiated again.

Step 2: Differentiate (again) the algebraic constraints

To find the right candidate(s) here we write a table of the dependencies

\begin{array}{c|cccccccc} & v_{x,\text{dot}} & v_{z,\text{dot}} & v_{d,\text{dot}} & v_{r,\text{dot}} & \;\;u_1\;\; & \;\;u_2\;\; & \;\;\tau\;\; & \theta_{\text{dot}} \\[1em] \hline \\[0.1em] \text{(Constraint 1)} & 1 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\[0.5em] \text{(Constraint 2)} & 0 & 1 & 0 & 0 & 0 & 0 & 1 & 0 \\[0.5em] \text{(Constraint 3)} & 0 & 0 & 1 & 0 & 1 & 0 & 1 & 0 \\[0.5em] \text{(Constraint 4)} & 0 & 0 & 0 & 1 & 0 & 1 & 1 & 0 \\[0.5em] \text{(Constraint 5)}' & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\[0.5em] \text{(Constraint 6)}' & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\[0.5em] \text{(Constraint 7)}' & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\[0.5em] \text{(Constraint 8)}' & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{array}

The last two constraints are candidate for differentiation as it doesn’t introduce new algebraic variables. Differentiate thus the last constraints and substitute known variable in differential form using the ODE part

\begin{aligned} \text{(Constraint 7)}''\quad 0 & = p''_1(t) - v_{x,\text{dot}} \\ \text{(Constraint 8)}''\quad 0 & = p''_2(t) - v_{z,\text{dot}} \end{aligned}

and constraint part of the DAE become

\begin{aligned} & \left\{ \color{DarkRed} \begin{aligned} 0 &= M_2 v_{x,\text{dot}}+\tau \sin\theta && \text{(Constraint 1)}\\ 0 &= M_2 v_{z,\text{dot}}+\tau \cos\theta - m g && \text{(Constraint 2)}\\ 0 &= M_1 v_{d,\text{dot}}+C_1 v_d -u_1 -\tau\sin\theta && \text{(Constraint 3)}\\ 0 &= J v_{r,\text{dot}} +C_2 v_r + C_3 u_2 - C_3^2 \tau && \text{(Constraint 4)} \\ 0 &= v_r\sin\theta +r(\cos\theta)\theta_{\text{dot}}+v_d-v_x && \text{(Constraint 5)}'\\ 0 &= v_r\cos\theta - r(\sin\theta)\theta_{\text{dot}}-v_z && \text{(Constraint 6)}'\\ 0 &= p''_1(t) - v_{x,\text{dot}} && \text{(Constraint 7)}''\\ 0 &= p''_2(t) - v_{z,\text{dot}} && \text{(Constraint 8)}''\\ \end{aligned} \right. \\ & \left\{ \color{DarkGreen} \begin{aligned} 0 & = r\sin\theta + d - x &\qquad\qquad\qquad\quad & \text{(Constraint 5)}\\ 0 & = r\cos\theta - z && \text{(Constraint 6)}\\ 0 & = p_1(t) - x && \text{(Constraint 7)}\\ 0 & = p_2(t) - z && \text{(Constraint 8)} \\ 0 & = p'_1(t) - v_x && \text{(Constraint 7)}'\\ 0 & = p'_2(t) - v_z && \text{(Constraint 8)}'\\ \end{aligned} \right. \end{aligned}

Step 3: Differentiate (again) the algebraic constraints

To find the right candidate(s) here we write a table of the dependencies

\begin{array}{c|cccccccc} & v_{x,\text{dot}} & v_{z,\text{dot}} & v_{d,\text{dot}} & v_{r,\text{dot}} & \;\;u_1\;\; & \;\;u_2\;\; & \;\;\tau\;\; & \theta_{\text{dot}} \\[1em] \hline \\[0.1em] \text{(Constraint 1)} & 1 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\[0.5em] \text{(Constraint 2)} & 0 & 1 & 0 & 0 & 0 & 0 & 1 & 0 \\[0.5em] \text{(Constraint 3)} & 0 & 0 & 1 & 0 & 1 & 0 & 1 & 0 \\[0.5em] \text{(Constraint 4)} & 0 & 0 & 0 & 1 & 0 & 1 & 1 & 0 \\[0.5em] \text{(Constraint 5)}' & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\[0.5em] \text{(Constraint 6)}' & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\[0.5em] \text{(Constraint 7)}'' & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\[0.5em] \text{(Constraint 8)}'' & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \end{array}

There are no constraints candidate for derivation. We can check if the constraints are enough to determine algebraic variable.

We use sprank of MATLAB to check if the matrix can be non singular.

sparsity_pattern = [...
1, 0, 0, 0, 0, 0, 1, 0; ...
0, 1, 0, 0, 0, 0, 1, 0; ...
0, 0, 1, 0, 1, 0, 1, 0; ...
0, 0, 0, 1, 0, 1, 1, 0; ...
0, 0, 0, 0, 0, 0, 0, 1; ...
0, 0, 0, 0, 0, 0, 0, 1; ...
1, 0, 0, 0, 0, 0, 0, 0; ...
0, 1, 0, 0, 0, 0, 0, 0; ...
];

fprintf('Estimated rank = %d\n', sprank(sparsity_pattern) );

> Estimated rank = 6

Matrix is not full rank. To continue the algorithm we can apply elimination to remove redundancy. First of all rewrite the constraint as a linear system

\underbrace{ \begin{bmatrix} M_{2} & 0 & 0 & 0 & 0 & 0 & \sin(\theta) & 0 \\ 0 & M_{2} & 0 & 0 & 0 & 0 & \cos(\theta) & 0 \\ 0 & 0 & M_{1} & 0 & -1 & 0 & -\sin(\theta) & 0 \\ 0 & 0 & 0 & J & 0 & C_{3} & -C_{3}^{2} & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & r \cos(\theta) \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & -r\sin(\theta) \\ -1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & -1 & 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix} }_{\bm{A}} \begin{pmatrix} v_{x,\text{dot}} \\ v_{z,\text{dot}} \\ v_{d,\text{dot}} \\ v_{r,\text{dot}}\\ u_1 \\ u_2 \\ \tau \\ \theta_{\text{dot}} \end{pmatrix} = \underbrace{ \begin{bmatrix} 0 \\ m g \\ -C_{1} v_{d} \\ -C_{2} v_{r} \\ v_{x}-v_{d}-v_{r}\sin(\theta) \\ v_{z} -v_{r}\cos(\theta) \\ -p''_{1}(t) \\ -p''_{2}(t) \end{bmatrix} }_{\bm{b}}

given \bm{L}_1 defined forward we have

\begin{aligned} \bm{L}_1 &= \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ M_{2}^{-1} & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & M_{2}^{-1} & 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \\ \bm{L}_1\bm{A} &= \begin{bmatrix} M_{2} & 0 & 0 & 0 & 0 & 0 & \sin(\theta) & 0 \\ 0 & M_{2} & 0 & 0 & 0 & 0 & \cos(\theta) & 0 \\ 0 & 0 & M_{1} & 0 & -1 & 0 & -\sin(\theta) & 0 \\ 0 & 0 & 0 & J & 0 & C_{3} & -C_{3}^{2} & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & r\cos(\theta) \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & -r\sin(\theta) \\ 0 & 0 & 0 & 0 & 0 & 0 & M_2^{-1}\sin(\theta) & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & M_2^{-1}\cos(\theta) & 0 \end{bmatrix} \end{aligned}

given \bm{R}_1 defined forward we have \begin{aligned} \bm{R}_1 &= \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & \cos(\theta) & -\sin(\theta) & 0 & 0 \\ 0 & 0 & 0 & 0 & \sin(\theta) & \cos(\theta) & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & \cos(\theta) & -\sin(\theta) \\ 0 & 0 & 0 & 0 & 0 & 0 & \sin(\theta) & \cos(\theta) \end{bmatrix} \\ \bm{R}_1\bm{L}_1\bm{A} &= \begin{bmatrix} M_{2} & 0 & 0 & 0 & 0 & 0 & \sin(\theta) & 0 \\ 0 & M_{2} & 0 & 0 & 0 & 0 & \cos(\theta) & 0 \\ 0 & 0 & M_{1} & 0 & -1 & 0 & -\sin(\theta) & 0 \\ 0 & 0 & 0 & J & 0 & C_{3} & -C_{3}^{2} & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & r \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & M_{2}^{-1} & 0 \end{bmatrix} \end{aligned}

\bm{R}_1\bm{L}_1\bm{A} have two rows zero, thus multiplying by \bm{R}_1\bm{L}_1 the constraints permits to find two algebraic constraints that can be derived.

\bm{R}_1\bm{L}_1 \begin{bmatrix} M_2 v_{x,\text{dot}}+\tau \sin\theta \\ M_2 v_{z,\text{dot}}+\tau \cos\theta - m g \\ M_1 v_{d,\text{dot}}+C_1 v_d -u_1 -\tau\sin\theta \\ J v_{r,\text{dot}} +C_2 v_r + C_3 u_2 - C_3^2 \tau \\ v_r\sin\theta +r(\cos\theta)\theta_{\text{dot}}+v_d-v_x \\ v_r\cos\theta - r(\sin\theta)\theta_{\text{dot}}-v_z \\ p''_1(t) - v_{x,\text{dot}} \\ p''_2(t) - v_{z,\text{dot}} \end{bmatrix} = \begin{bmatrix} M_2 v_{x,\text{dot}} + \tau \sin(\theta) \\ M_2 v_{z,\text{dot}}+ \tau \cos(\theta) - m g \\ M_{1} v_{d,\text{dot}}+C_{1} v_{d}-u_{1}-\tau\sin(\theta) \\ J v_{r,\text{dot}}+C_{2} v_{r}+C_{3} u_{2}-C_{3}^{2} \tau \\ v_{d}-v_{x}+ \cos(\theta)+r\theta_{\text{dot}}+ \sin(\theta) v_{z} \\ (v_{d}-v_{x}) \sin(\theta)-\cos(\theta) v_{z}+v_{r} \\ \cos(\theta) p''_{1}(t)-p''_{2}(t) \sin(\theta) +M_2^{-1}\sin(\theta) m g \\ \cos(\theta)p''_{2}(t)+p''_{1}(t) \sin(\theta) +M_2^{-1}(\tau -\cos(\theta)) m g \end{bmatrix}

Collecting the recombined constraints

\left\{ \color{BrickRed} \begin{aligned} 0 &= M_2 v_{x,\text{dot}}+\tau \sin\theta & \text{(C1)} \\ 0 &= M_2 v_{z,\text{dot}}+ \tau \cos(\theta) - m g & \text{(C2)} \\ 0 &= M_{1} v_{d,\text{dot}}+C_{1} v_{d}-u_{1}-\tau\sin(\theta) & \text{(C3)} \\ 0 &=J v_{r,\text{dot}}+C_{2} v_{r}+C_{3} u_{2}-C_{3}^{2} \tau & \text{(C4)} \\ 0 &=v_{d}-v_{x}+ \cos(\theta)+r\theta_{\text{dot}}+ \sin(\theta) v_{z} & \text{(C5)} \\ 0 &=(v_{d}-v_{x}) \sin(\theta)-\cos(\theta) v_{z}+v_{r} & \text{(C6)} \\ 0 &= \cos(\theta) p''_{1}(t)-p''_{2}(t) \sin(\theta) +M_2^{-1}\sin(\theta) m g & \text{(C7)} \\ 0 &=\cos(\theta)p''_{2}(t)+p''_{1}(t) \sin(\theta) +M_2^{-1}(\tau -\cos(\theta)) m g & \text{(C8)} \end{aligned} \right.

where we have renamed constraints because now are mixed by the multiplication of \bm{L}^{-1}. The constraints \text{(C6)} and \text{(C7)} can be derived.

\begin{aligned} \text{(C6)}' &= \left((v_{d}-v_{x}) \theta_{\text{dot}}-v_{z,\text{dot}}\right)\cos(\theta)+ \left(\theta_{\text{dot}}v_{z}+ v_{d,\text{dot}}-v_{x,\text{dot}}\right) \sin(\theta)+v_{r,\text{dot}} \\[0.5em] \text{(C7)}' &= p'''_{1}(t)\cos(\theta) -p'''_{2}(t)\sin(\theta) -\left(p''_{2}(t)\cos(\theta) +p''_{1}(t)\sin(\theta)\right) \theta_{\text{dot}}+ M_{2}^{-1} \theta_{\text{dot}}\cos(\theta) m g \end{aligned}

and the constraints and hidden constraints become:

\begin{aligned} & \left\{ \color{DarkRed} \begin{aligned} 0 &= M_2 v_{x,\text{dot}}+\tau \sin\theta && \text{(C1)} \\ 0 &=M_2 v_{z,\text{dot}}+ \tau \cos(\theta) - m g && \text{(C2)} \\ 0 &= M_{1} v_{d,\text{dot}}+C_{1} v_{d}-u_{1}-\tau\sin(\theta) && \text{(C3)} \\ 0 &=J v_{r,\text{dot}}+C_{2} v_{r}+C_{3} u_{2}-C_{3}^{2} \tau && \text{(C4)} \\ 0 &=v_{d}-v_{x}+ \cos(\theta)+r\theta_{\text{dot}}+ \sin(\theta) v_{z} && \text{(C5)} \\ 0 &= \left((v_{d}-v_{x}) \theta_{\text{dot}}-v_{z,\text{dot}}\right)\cos(\theta)+ \left(\theta_{\text{dot}}v_{z}+ v_{d,\text{dot}}-v_{x,\text{dot}}\right) \sin(\theta)+v_{r,\text{dot}} && \text{(C6)}' \\ 0 &= p'''_{1}(t)\cos(\theta) -p'''_{2}(t)\sin(\theta) -\left(p''_{2}(t)\cos(\theta) +p''_{1}(t)\sin(\theta)\right) \theta_{\text{dot}}+ M_{2}^{-1} \theta_{\text{dot}}\cos(\theta) m g && \text{(C7)}' \\ 0 &=\cos(\theta)p''_{2}(t)+p''_{1}(t) \sin(\theta) +M_2^{-1}(\tau -\cos(\theta) m g && \text{(C8)} \end{aligned} \right. \\ & \left\{ \color{DarkGreen} \begin{aligned} 0 & = r\sin\theta + d - x &\qquad & \text{(Constraint 5)}\\ 0 & = r\cos\theta - z && \text{(Constraint 6)}\\ 0 & = p_1(t) - x && \text{(Constraint 7)}\\ 0 & = p_2(t) - z && \text{(Constraint 8)} \\ 0 & = p'_1(t) - v_x && \text{(Constraint 7)}'\\ 0 & = p'_2(t) - v_z && \text{(Constraint 8)}'\\ 0 &=(v_{d}-v_{x}) \sin(\theta)-\cos(\theta) v_{z}+v_{r} && \text{(C6)} \\ 0 &= \cos(\theta) p''_{1}(t)-p''_{2}(t) \sin(\theta) +M_2^{-1}\sin(\theta) m g && \text{(C7)} \end{aligned} \right. \end{aligned}

Step 4: Differentiate the algebraic constraints?

To find the right candidate(s) here we write a table of the dependencies

\begin{array}{c|cccccccc} & v_{x,\text{dot}} & v_{z,\text{dot}} & v_{d,\text{dot}} & v_{r,\text{dot}} & \;\;u_1\;\; & \;\;u_2\;\; & \;\;\tau\;\; & \theta_{\text{dot}} \\[1em] \hline \\[0.1em] \text{(C1)} & 1 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\[0.5em] \text{(C2)} & 0 & 1 & 0 & 0 & 0 & 0 & 1 & 0 \\[0.5em] \text{(C3)} & 0 & 0 & 1 & 0 & 1 & 0 & 1 & 0 \\[0.5em] \text{(C4)} & 0 & 0 & 0 & 1 & 0 & 1 & 1 & 0 \\[0.5em] \text{(C5)} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\[0.5em] \text{(C6)}' & 1 & 1 & 1 & 1 & 0 & 0 & 0 & 1 \\[0.5em] \text{(C7)}' & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\[0.5em] \text{(C8)} & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \end{array} There are no constraints candidate for derivation. We can check if the constraints are enough to determine algebraic variable.

We use sprank of MATLAB to check if the matrix can be non singular.

sparsity_pattern = [...
1, 0, 0, 0, 0, 0, 1, 0;...
0, 1, 0, 0, 0, 0, 1, 0;...
0, 0, 1, 0, 1, 0, 1, 0;...
0, 0, 0, 1, 0, 1, 1, 0;...
0, 0, 0, 0, 0, 0, 0, 1;...
1, 1, 1, 1, 0, 0, 0, 1;...
0, 0, 0, 0, 0, 0, 0, 1;...
0, 0, 0, 0, 0, 0, 1, 0;...
];

fprintf('Estimated rank = %d\n', sprank(sparsity_pattern) );

> Estimated rank = 7

the matrix is singular. Instead of apply LU try to combine \text{(C5)} and \text{(C7)}' to eliminate \theta_{\text{dot}}

\begin{aligned} \widetilde{\text{C7}} &= \text{(C5)} \left( M_2^{-1}mg\cos(\theta) - p''_{2}(t)\cos(\theta) - p''_{1}(t)\sin(\theta)\right)+ \text{(C7)}'r \\ &= \left( \cos(\theta)p'_1(t) -\cos(\theta) v_{d} -\sin(\theta)p'_2(t) \right) \left(\cos(\theta) p''_{2}(t) +\sin(\theta) p''_{1}(t)\right) \\ &+\left(\sin(\theta)p'''_{2}(t)-\cos(\theta) p'''_{1}(t)\right) r \\ & +M_2^{-1}\left( \cos(\theta) v_{d} -\cos(\theta) p'_1(t) +\sin(\theta)p'_2(t) \right) \cos(\theta) m g %& = %-\sin(\theta)^{2} p'_2(t)p''_{1}(t) \\ %& +\left( % \left( % M_2^{-1}p'_2(t) m g % -(v_{d}-p'_1(t)) p''_{1}(t) % -p'_2(t) p''_{2}(t) % \right) % \cos(\theta) % +rp'''_{2}(t) %\right) %\sin(\theta) %\\ % & % +\left( % (p'_1(t)-v_{d})p''_{2}(t) % +M_2^{-1}\left(v_{d}-p'_1(t)\right) m g % \right) % \cos(\theta)^{2}-r\cos(\theta)p'''_{1}(t) \end{aligned}

and

\begin{aligned} \widetilde{\text{C7}}' &= \left( M_2^{-1} \left( 2 p'_2\theta_{\text{dot}} - p''_1+ v_{d,\text{dot}} \right) m g + \left( p'_1-v_{d} \right) p'''_{2} +p''_1p''_{2} +2 \theta_{\text{dot}}(p'_1-v_{d}) p''_{1} -\left( 2 \theta_{\text{dot}}p'_2+v_{d,\text{dot}} \right) p''_{2} \right) \cos(\theta)^2 \\ &+\left( \left( M_2^{-1} \left( p''_2(t) + 2\theta_{\text{dot}}(p'_1(t)-v_{d}) \right) mg +( p'_1-v_{d})p'''_{1}-p'''_{2}p'_2+p''_{1}p''_1 -p''_{2}p''_2+ \left(-2 \theta_{\text{dot}}p'_2-v_{d,\text{dot}}\right) p''_{1} + \left(-2 \theta_{\text{dot}}p'_1+2 v_{d}\theta_{\text{dot}}\right)p''_{2} \right) \sin(\theta) +r\theta_{\text{dot}}p'''_{2}-rp''''_{1}-v_{r}p'''_{1} \right) \cos(\theta) \\ & +\left(-p'_2p'''_{1}-p''_2p''_{1} \right) \sin(\theta)^2+ \\ & \left(r \theta_{\text{dot}} p'''_{1}+rp''''_{2}+v_{r}p'''_{2}\right) \sin(\theta)-M_2^{-1}p'_2 \theta_{\text{dot}} m g +\left(-\theta_{\text{dot}} p'_1+v_{d}\theta_{\text{dot}}\right) p''_{1}+p''_{2}\theta_{\text{dot}}p'_2 \end{aligned}

TO BE COMPLETED