ICLOCS2

We can specify the system dynamics in function DoubleIntegratorTracking_Dynamics_Internal with

x2 = x(:,2);
u1 = u(:,1);
dx(:,1) = x2;
dx(:,2) = u1;

For this example problem, we do not have any path constraints, so function DoubleIntegratorTracking_Dynamics_Internal only need to return the value of variable dx .

Similarly, the (optional) simulation dynamics can be specifed in function DoubleIntegratorTracking_Dynamics_Sim.m .

First we provide the function handles for system dynamics with

InternalDynamics=@DoubleIntegratorTracking_Dynamics_Internal;
SimDynamics=@DoubleIntegratorTracking_Dynamics_Sim;

The optional files providing analytic derivatives can be left empty as we demonstrate the use of finite difference this time

problem.analyticDeriv.gradCost=[];
problem.analyticDeriv.hessianLagrangian=[];
problem.analyticDeriv.jacConst=[];

and finally the function handle for the settings file is given as.

problem.settings=@settings_DoubleIntegratorTracking;

Next we define the time variables with

problem.time.t0_min=0;
problem.time.t0_max=0;
guess.t0=0;

and

problem.time.tf_min=10;
problem.time.tf_max=10; 
guess.tf=10;

For state variables, we need to provide the initial conditions

problem.states.x0=[0 5];

bounds on intial conditions

problem.states.x0l=[0 5]; 
problem.states.x0u=[0 5]; 

state bounds

problem.states.xl=[-6 -10];
problem.states.xu=[6 10]; 

bounds on the absolute local and global (integrated) discretization error of state variables

problem.states.xErrorTol_local=[1e-6 1e-6];
problem.states.xErrorTol_integral=[1e-6 1e-6]; 

tolerance for state variable box constraint violation

problem.states.xConstraintTol=[1e-4 1e-4];

terminal state bounds

problem.states.xfl=[-6 -10]; 
problem.states.xfu=[6 10]; 

and an initial guess of the state trajectory (optional but recommended)

guess.time=[0 1.5 4.5 8 10];
guess.states(:,1)=[0 5 -5 5 -3];
guess.states(:,2)=[5 0 -1 -1 -5]; 

Lastly for control variables, we need to define simple bounds

problem.inputs.ul=[-10];
problem.inputs.uu=[10];

bounds on the first control action

problem.inputs.u0l=[-10];
problem.inputs.u0u=[10]; 

tolerance for control variable box constraint violation

problem.inputs.uConstraintTol=[0.1]; 

as well as an initial guess of the input trajectory (optional but recommended)

guess.inputs(:,1)=[0 0 0 0 0];

Since this problem only has a Lagrange (stage) cost , and no Mayer (boundary) cost ,we simply have

e1=x(:,1)-5.*sin(t);
e2=x(:,2)-5.*cos(t);
u1=u(:,1);
stageCost = e1.*e1+e2.*e2+u1.*u1.*0.0001;

boundaryCost=0;

for sub-function L_unscaled and E_unscaled respectively.

For this example problem, we do not have additional boundary constraints (in addition to varialbe simple bounds). Thus we can leave routines b_unscaled as it is.

Now we can fetch the problem and options, solve the resultant NLP, and generate some plots for the solution.

[problem,guess]=DoubleIntegratorTracking;          % Fetch the problem definition
options= problem.settings(30);                  % Get options and solver settings 
[solution,MRHistory]=solveMyProblem( problem,guess,options);
[ tv, xv, uv ] = simulateSolution( problem, solution, 'ode113', 0.01 );

Note the last line of code will generate an open-loop simulation, applying the obtained input trajectory to the dynamics defined in DoubleIntegratorTracking_Dynamics_Sim.m , using the Matlab builtin ode113 ODE solver with a step size of 0.01.

Using the Hermite-Simpson discretization scheme of ICLOCS2, the following state and input trajectories are obtained under a mesh refinement scheme starting with 30 mesh points. The computation time with IPOPT (NLP convergence tol set to 1e-09) is about 0.8 seconds with one mesh refinement iteration, using finite difference derivative calculations on an Intel i7-6700 desktop computer. Subsequent recomputation using the same mesh and warm starting will take on average 0.1 seconds and less than 10 iterations.