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


$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


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


inserted in the first equation that means


so my final equations would be




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.

  • 1
    $\begingroup$ Note that $f(x)$ is not correct. The correct representation is $f(x,\dot x)$ so the movement equations change. $\endgroup$ – Cesareo Mar 16 at 14:17
  • 1
    $\begingroup$ You should be aware of the reference change from $\phi, p_1, p_2$ to $x_1,x_2,x_3$. $\endgroup$ – Cesareo Mar 16 at 14:34
  • 1
    $\begingroup$ How do you solve this with ode45? I would expect bvp4c, as this is a boundary value problem. $\endgroup$ – Lutz Lehmann Mar 21 at 14:21
  • $\begingroup$ 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 $\endgroup$ – Mathsfreak Mar 22 at 12:55


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

| cite | improve this answer | |
  • $\begingroup$ 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). $\endgroup$ – Mathsfreak Mar 16 at 12:04

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.