There are mnany different solutions. Perhaps you simply want to integrate the Lagrange function with ODE45.
Note that the simulation is more efficient using depending coordinates and a constraint matrix (see odeset->mass matrix and DAE options), that solving the equations to independent coordinates, because the trigonometrix stuff requires a lot of time.
