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) a<a_max 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 v(t_f)=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
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])