Lagrange Multipliers in Multi-body Finite Element Code

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.

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.

I've posted the question here in full as it allows me to show equations: http://scicomp.stackexchange.com/questions/14446/lagrange-multipliers-in...