constrained differential equation

Question

I'm struggeling with the derivation of the ode equations of forward kinematics of an oriented object. Assuming to be in $R^2$ and using the coordinates $(x_1,x_2,x_3):=(\phi,p_1,p_2)$, where the angle $\phi$ describes the orientation of the object and $(p_1,p_2)$ describes its position in space.

Now I want to calculate an optimal trajectorie from one position $(\phi^{0},p_1^0,p_2^0)$ to another $(\phi^{1},p_1^1,p_2^1)$ minimizing an arbitrary energy functional, for example

$\int^b_a ||\dot{x}||^2 dt=\int^b_a \dot{\phi}^2+\dot{p_1}^2+\dot{p_2}^2 dt$.

to be sure that the object moves forward I introduced the constrained that the direction of movement and the y-axis of the object should be perpendicular

$\dot{p_1}\cdot sin(\phi)-\dot{p_2}\cdot cos(\phi)=0$.

I found this acricle Solving constrained Euler-Lagrange equations with Lagrange Multipliers (Geodesics) where someone had a similar problem and the answer was to calculate lagrange multiplyers $\lambda$. I also read the Wiki Article about https://en.wikipedia.org/wiki/Lagrangian_mechanics. There I found the formula (Langranges equation)

$\frac{\partial{L}}{\partial x_i}-\frac{d}{dt}\frac{\partial{L}}{\partial \dot{x_i}}+\lambda(\frac{\partial{f}}{\partial x_i})=0$

with in my case

$L(x,\dot{x},t)=||\dot{x}||^2$

$f(x,\dot{x})=\dot{x_2}\cdot sin(x_1)-\dot{x_3}\cdot cos(x_1)$

that means inserted in the formula above I get the three equations

1) $-2\ddot{x}_1+\lambda (\dot{x_2}\cdot cos(x_1)+\dot{x_3}\cdot sin(x_1))=0$

2) $2\ddot{x}_2+\lambda cos(x_1)\dot{x}_1=0$

3) $2\ddot{x}_3+\lambda sin(x_1)\dot{x}_1=0$

my questions are

1) Is this derivation correct so far?

2) How can I get $\lambda$ to resolve the first equation?

the hint was

$\dot{x}_1 cos(x_1)\dot{x}_2+\dot{x}_1\dot{x}_3 sin(x_1)+\ddot{x}_2 sin(x_1)-\ddot{x}_3 cos(x_1)=0$

resolving equation 2 and 3 leads to

$\ddot{x}_2=-\frac{\lambda}{2} cos(x_1)\dot{x_1}$

$\ddot{x}_3=-\frac{\lambda}{2} sin(x_1)\dot{x_1}$

inserting in the fourth equation leads to

$\dot{x}_1 cos(x_1)\dot{x}_2+\dot{x}_1\dot{x}_3 sin(x_1)=0$

this can't be inserted well in the first equation, therefore it is differentiated again

$\ddot{x}_1(cos(x_1)\dot{x}_2+sin(x_1)\dot{x}_3)+\dot{x}_1(-sin(x_1)\dot{x}_1\dot{x}_2+cos(x_1)\dot{x}_1\dot{x}_3)=0$

resolved to $\ddot{x}_1$ I get

$\ddot{x}_1=-\frac{\dot{x}^2_1(-sin(x_1)\dot{x}_2+cos(x_1)\dot{x}_3)}{(cos(x_1)\dot{x}_2+sin(x_1)\dot{x}_3)}$

inserted in the first equation that means

$\lambda=-2\frac{\dot{x}_1^2(-sin(x_1)\dot{x}_2+cos(x_1)\dot{x}_3)}{(cos(x_1)\dot{x}_2+sin(x_1)\dot{x}_3)^2}$

so my final equations would be

$\ddot{x}_1=-\frac{\dot{x}_1^2(-sin(x_1)\dot{x}_2+cos(x_1)\dot{x}_3)}{(cos(x_1)\dot{x}_2+sin(x_1)\dot{x}_3)}$

$\ddot{x}_2=\frac{\dot{x}_1^3cos(x_1)(-sin(x_1)\dot{x}_2+cos(x_1)\dot{x}_3)}{(cos(x_1)\dot{x}_2+sin(x_1)\dot{x}_3)^2}$

$\ddot{x}_3=\frac{\dot{x}_1^3sin(x_1)(-sin(x_1)\dot{x}_2+cos(x_1)\dot{x}_3)}{(cos(x_1)\dot{x}_2+sin(x_1)\dot{x}_3)^2}$

but resolving them in MATLAB with ode45 results not in forward movement.

Did I use your hint right? I would appreciate any further hints or comments.

Answer 1

Hint.

Supposing that the equivalence $\dot{p_1}\sin(\phi)-\dot{p_2} \cos(\phi)=0$ and $f(x,\dot x)=\dot{x_2}\cdot \sin(x_1)-\dot{x_3}\cdot \cos(x_1)=0$ is correct, the fourth equation can be obtained by deriving $f(x,\dot x)$ regarding $t$ or with

$$ \dot x_1 \cos (x_1) \dot x_2+\dot x_1 \sin (x_1) \dot x_3+\sin (x_1) \ddot x_2-\cos (x_1) \ddot x_3 = 0 $$

Regarding the Euler-Lagrange movement equations with

$$ L = ||\dot x||^2+\lambda f(x,\dot x) $$

we have

$$ \left\{ \begin{array}{rcl} \lambda \left(\cos (x_1) \dot x_2+\sin (x_1) \dot x_3\right)-2 \ddot x_1 & = & 0\\ \lambda \cos (x_1)\dot x_1+2 \ddot x_2 & = &0\\ \lambda \sin (x_1) \dot x_1+2 \ddot x_3 & = & 0 \\ \dot x_1 \cos (x_1) \dot x_2+\dot x_1 \sin (x_1) \dot x_3+\sin (x_1) \ddot x_2-\cos (x_1) \ddot x_3 & = & 0 \end{array} \right. $$

the last equation is $\frac{d}{dt}\left(\partial_{\lambda}L\right)$