Double-integrator with custom terminal cost example

This example serves to illustrate how to specify a custom terminal cost.

The scripts developed in this example can be opened by entering

open('example_terminal_cost')
open('cart')

in the command window.

Consider the double integrator example. In that example, we did not set the TerminalCost property and we enforced some termnal state constraint. Since we did not set the VFPenalty property, DynaProg used the default setting 'rift', adding a strong penalty term to terminal states that did not meet our constraints.

Suppose we want to disable this term and define our own terminal term, giving us more control on the relative importance of the constraints on the first state (position) and the second state (speed).

First, set up the DynaProg probelm as in the double integrator example.

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

Then, disable the default terminal penalty by setting the VFPenalty propert to 'none':

prob.VFPenalty = 'none';

Now create your terminal cost. Define a function that implements a quadratic cost to penalize deviations from a desired position and velocity:

function cost = quadraticCost(x, pos_final, velocity_final)
    cost = 1e3 * ( x{1} - pos_final ) .^ 2 + ...
        1e3 * ( x{2} - velocity_final ) .^ 2;
end

Now set the TerminalCost property to a function handle to our custom cost. Remember that this function handle must accept the states x as the only input.

prob.TerminalCost = @(x) quadraticCost(x, pos_final, velocity_final);

Solve the problem and visualize results.

% Solve the problem
prob = run(prob);
% Plot results
figure
plot(prob);

Since pos_final and velocity_final were set to 0.7 and 0 respectively, the plot shows that the quadratic cost is indeed effective in enforcing a terminal state for this problem.