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, 2020 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, 2020 at 14:34
  • 1
    $\begingroup$ How do you solve this with ode45? I would expect bvp4c, as this is a boundary value problem. $\endgroup$ Mar 21, 2020 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, 2020 at 12:55

1 Answer 1



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

  • $\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, 2020 at 12:04

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .