Numerical solution for Euler–Lagrange equation

[YingboMa]

For the Euler–Lagrange equation, we need to move the generalized acceleration term into the LHS. I propose the following implementation.

using ForwardDiff

∂L∂q(L, t, q, q̇) = ForwardDiff.gradient(a->L(t,a,q̇), q)
∂L∂q̇(L, t, q, q̇) = ForwardDiff.gradient(a->L(t,q,a), q̇)
Dtq(L, t, q, q̇) = ForwardDiff.derivative(a->∂L∂q(L,a,q,q̇), t)
Dqq̇(L, t, q, q̇) = ForwardDiff.jacobian(a->∂L∂q̇(L,t,a,q̇), q)
Dq̇q̇(L, t, q, q̇) = ForwardDiff.hessian(a->L(t,q,a), q̇)

function generalized_acceleration(L, t, q, q̇)
    F= ∂L∂q(L, t, q, q̇)
    lhs = Dq̇q̇(L, t, q, q̇)
    rhs = F - Dqq̇(L, t, q, q̇)*q̇ - Dtq(L, t, q, q̇)
    bkfact!(lhs)\rhs  # Using Bunch-Kaufman factorization since we can assume that for a physical system, the matrix will be symmetric
end

This implementation won't work until this issue is fixed.