# constrained differential equation

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.

• Note that $f(x)$ is not correct. The correct representation is $f(x,\dot x)$ so the movement equations change. Mar 16, 2020 at 14:17
• You should be aware of the reference change from $\phi, p_1, p_2$ to $x_1,x_2,x_3$. Mar 16, 2020 at 14:34
• How do you solve this with ode45? I would expect bvp4c, as this is a boundary value problem. Mar 21, 2020 at 14:21
• Thanks, I was not aware that there is a solver for boundary value problems. I used the shooting method to solve it as a initial value problem Mar 22, 2020 at 12:55

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)$$

• Thanks for your hint, I will edit my text above about the further steps I did now. The Solution of the ODE system ist still not what I expected (the object is not moving only forward). Mar 16, 2020 at 12:04