Double-integrator example

The scripts developed in this example can be opened by entering

open('example_cart')
open('cart')

in the command window.

Consider a cart modelled as a point unit mass moving on a frictionless plane.

Define the system state as:

the cart's position along the x axis,
the cart's velocity.

Suppose the cart is subject to an external force

which can be controlled. The cart must go from an initial position

to a final position

in a finite time, while minimizing the energy spent by applying the external force. Therefore, we want to minimize

Also, the cart starts from standstill and must stop when the final position is reached, which means that both the initial and final velocity must be zero, i.e

Then, the system's dynamics is described by a set of two ODEs:

where m is the cart's mass.

Reformulating the problem in discrete time, the system dynamics can be rewritten as

where h is our time step. The stage cost is defined by

To solve this problem with DynaProg, we must define:

a system function which evaluates the system dynamics and running cost,
computational grids for the state and control variables,
the initial state,
terminal state constraints,
the number of stages (time intervals).

Define the system and cost function

Start with the basic function signature. You can start by opening a template m-file:

open('sysfun_template.m')

and creating a copy in your working folder.

function [x_new, stageCost, unfeas] = sysname(x, u, ~)
 ...
end

For more information on the system and cost function signature, see Set up a basic problem.

Write some code which evaluates the state update equations and the stage cost. Note that the state variables (x) and the updated state variables (x_new) are defined as cell arrays, where each cell refers to one of the state variables. Similarly, u is treated as a cell array, even though it only contains one cell as there is only one control variable.

Finally, write some code to define unfeasible combinations of state and control variables. Since we do not want to specify any constraint on the state and control variables in this example, we specify the third output as an empty array.

function [x_new, stageCost, unfeas] = cart(x, u, ~)
 
dt = 0.1; % time step
 
x_new{1} = x{1} + x{2}.*dt + zeros(size(u{1}));
x_new{2} = x{2} + (1/mass).*u{1}.*dt;
 
% Stage cost
stageCost = (u{1}.^2).*dt;
 
% Unfeasibility
unfeas = []; % No constraints to be set -> define the third output
             %  as an empty  array ([]).
end

Set up the problem structure

Create another script where you will define and solve the optimization problem.

Now, create all the required information that define an optimization problem. See also how to set up a basic problem.

% Initial and final conditions
pos_initial = 0;
pos_final = 0.7;
velocity_initial = 0;
velocity_final = 0;
time_final = 1;
 
% State variables grid
SVnames = {'Position', 'Speed'};
x1_grid = 0:0.01:1;
x2_grid = -0.2:0.01:1.2;
x_grid = {x1_grid, x2_grid};
% Initial state 
x1_init = pos_initial;
x2_init = velocity_initial;
x_init = {x1_init, x2_init};
% Final state constraints
x1_final = [pos_final-0.01 pos_final+0.01];
x2_final = [velocity_final-0.02 velocity_final+0.02];
x_final = {x1_final, x2_final};
% Control variable grid
CVnames = "Thrust";
u_grid = {-5:0.05:5};
 
% Number of stages (time intervals)
dt = 0.1; % timestep
Nint = time_final/dt;

Create the problem structure object by passing all relevant information to the DynaProg function.

prob = DynaProg(x_grid, x_init, x_final, u_grid, Nint, @cart);

Run the optimization and visualize results

Run the problem by using run:

prob = run(prob);

Make sure you return the problem structure as an output. Once the optimization algorithm is done running, the updated problem structure prob will also contain the simulation results. You can access them by querying prob.StateProfile, prob.ControlProfile and prob.CostProfile.

Plot the results using plot:

figure
plot(prob)

Assign names to the state and control variables and specify a time vector to substitute the stages in the x-axis.

% Add time vector to use in the plot
time = 0:dt:time_final;
prob.Time = time;
% Add SV and CV names to be used in the plot
prob.StateName = SVnames;
prob.ControlName = CVnames;
% Plot results
figure
plot(prob)

This could also have been achieved by specifying the relevant name-value pairs in constructing the problem object:

prob = DynaProg(x_grid, x_init, x_final, u_grid, Nint, @cart, ...
    'StateName', SVnames, 'ControlName', CVnames, 'CostName', "Energy", 'Time', time);