I'm working on a Multibody dynamics code using the finite element method to simulate the behaviour of flexible beams (using this paper if anyone is interested/ it is relevant). I'd like to model joints, and as I only need spherical friction free joints I believe this should be easy using Lagrange multipliers.
My understanding is that simple spherical joints are done by adding some extra rows and columns to the tangent stiffness matrix $K_t$ and extra rows to the residual force vector $g$ as below. Lets say we have 6 degrees of freedom at each node, $n$ nodes, and $p$ joints. For this example $p$ is 2 - The first joint links node $i$ to node $j$ and the second node $k$ to node $m$. The initial geometry places the linked nodes in the same place. The vector $\delta p$ is the incremental change in node position/ rotation and $F_J$ is the nodal forces required to keep the nodes coincident.
$$\begin{bmatrix} K_t & (\lambda_{2p \times 6n})^T \\ \lambda_{2p \times 6n} & 0 \end{bmatrix}\begin{bmatrix} \delta p \\ F_J \end{bmatrix} = \begin{bmatrix} g \\ 0 \end{bmatrix}$$
Where $\lambda$ is the Lagrange multiplier which for this two joint system is calculated as follows like this: $$\lambda=\begin{bmatrix} 0 & \cdots &0&1_{1,6i-5} & 0 &\cdots& 0 & -1_{1,6j-5} & 0 & \cdots &0\\ 0 & \cdots &0&1_{2,6i-4} & 0 &\cdots& 0 & -1_{2,6j-4} & 0 & \cdots &0 \\0 & \cdots &0&1_{3,6i-3} & 0 &\cdots& 0 & -1_{3,6j-3} & 0 & \cdots &0\\0 & \cdots &0&1_{4,6k-5} & 0 &\cdots& 0 & -1_{4,6m-5} & 0 & \cdots &0\\ 0 & \cdots &0&1_{5,6k-4} & 0 &\cdots& 0 & -1_{5,6m-4} & 0 & \cdots &0 \\0 & \cdots &0&1_{6,6k-3} & 0 &\cdots& 0 & -1_{6,6m-3} & 0 & \cdots &0\end{bmatrix}$$
When I run the code, the two bodies part company immediately. Any ideas where I'm going wrong? I'm just simulating a double pendulum with 10 elements in each body for now.
