I'm trying to implement direct-multiple shooting method to my problem.

Objective function: tf
constraints       : q<q_max
                    v<v_max (v=dq/dt)
                    tau<tau_max (tau=M(q)a+B(q,v)+G(q))
                    C(q)=r_0-|P-P_0|  (obstacle avoidance)

Initial condition q(0)=q_0 (q_0 is given) 
                  q(t_f)=q_f (q_f is given) and 
                  v(0)= 0

As I understand from the theory, I have to divide the variables as state variables and control variables.

State variables are: q and v 
Control variable is: tau
In each time interval I'll generate cubic splines which are q(t)=a_0+a_1*t+a_2*t^2+a_3*t^3

Could you help me how I will implement it? I don't understand what is the ODE here and how I should construct the algorithm?

Are there any example about it?

edit to make the equations clear I'll rewrite them here again:

based on the link

state variables: x1(t) = (q1(t) , ··· ,qn(t))^T and x2(t) = (q˙1(t) , ··· ,q˙n(t))^T. and derivatives of the state variables are equal to x˙(t) = f(x(t) ,u(t)) where f is f(x(t), u(t)) = ((q˙1(t), . . . , q˙5(t))^T; M(x(t))−1· (u(t) − N(x(t)))

I don't know how to insert cubic polynomials in that equation system and how to solve ODE Will it be like [T,X]=ode45('f', [0 t_f], [q_0 q_f])

  • $\begingroup$ What exactly are you having trouble with? What ODE are you trying to make? Can you link to a page that explains the algorithm you're trying to use? $\endgroup$
    – Chuck
    Nov 5, 2015 at 14:53

2 Answers 2


You have asked a complex question, and from your question it seems that there a lot of basic underlying concepts that you are not familiar with. Since you are new to ODEs, path planning and possibly robotics I would advise to take a stepwise approach:

  1. Take a look at how s-curves trajectory planning is done for robotic manipulators. You will se how the trajectory equation looks like for positions, velocities, accelerations and possibly jerks. The way the trajectory is defined will surly explain what is the ODE when talking about trajectory planning. You can find explanations here and here here and second half of this document

  2. Take a look at path planning with obstacle avoidance. Here you can learn how optimality criteria (cost function) are defined and used. Here and here and the first half of this document

  3. Read about optimal control. Before solving complex problems with optimal control, solve simple examples and make sure you understand them. Introduction here

  4. Combine all of the above. If you know how trajectory planning works, how path planning works and how optimal control works, you are in a position to combine them.


@chuck: thanks for reading my answer and give advice that there is room for improvement. I would like to try out the edit button to improve my answer.

But to the topic:

A lock into history of optimal control may help to answer the question. A classical problem which is over 200 years old is the two body problem. Later in 1760 Leonhard Euler described the three-body-problem. In both cases, a mathematical system is used for describing the problem itself. The formula which describes the three body problem and the problem from the original post also are called "ordinary differential equation" (ODE). The solution for solving these kind of problem is to use a transfer function which is a backward ODE.

In robotics for most problems such a backward function is impossible to create because the mathematical theory has no answer. So in most cases the three-body-problem and also robotics problems are solved with numerical solver which are not very elegant but uses bruteforce searching to detemine the parameter.

Could you help me how I will implement it?

Optimal Control with obstacle avoidance can only be done with a numerical solver like A* or others. It is not poassible to use a ODE for this kind of task, it is a np-hard problem. An example implementation in matlab is done by. The author used the "OPTRAGEN 1.0 A MATLAB Toolbox for Optimal Trajectory Generation" for generating trajectory between obstacles.

  • $\begingroup$ Welcome to Robotics ManuelRodriguez. Thanks for your answer but we prefer answers to be self contained where possible. Links tend to rot so answers which rely on a link can be rendered useless if the linked to content disappears. If you add more context from the link, it is more likely that people will find your answer useful. $\endgroup$
    – Chuck
    Jul 21, 2016 at 18:09
  • $\begingroup$ Also, it looks like you may have created two accounts. If so, then please follow the instructions at I accidentally created two accounts; how do I merge them? Stack Exchange only allows an individual to actively use one account. Any and all duplicates should be merged into your main account. $\endgroup$
    – Chuck
    Jul 21, 2016 at 18:09
  • $\begingroup$ I will not downgrade my own answer. $\endgroup$ Jul 22, 2016 at 14:12
  • $\begingroup$ I'm not asking you to "downgrade" it, I'm asking you to expand it. What's the name of the paper? Who is the author? What's the name of the method? Where can you get the toolbox? $\endgroup$
    – Chuck
    Jul 24, 2016 at 18:31

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

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