This very brief example shows how to use casadi framework to solve a very simple optimization problem in which we just want to minimize a function

clc

clear all

close all

import casadi.*

opti = Opti();

w = opti.variable(1); % Define the optimization variable as a scalar

p = 0.2*exp(0.2*w).*(sin(w)+5*cos(w)); % Just a function we want to minimize

% Show the function

wplot = 0:0.001:20;

p_plot = 0.2*exp(0.2.*wplot).*(sin(wplot)+5.*cos(wplot));

plot(wplot, p_plot)

title("Function we are trying to minimize")

xlabel("w")

ylabel("p(w)")

Here we setup the optimization problem

opti.minimize(p); % Cost function

opti.subject_to(w>=0) % Contraint

opti.solver('ipopt'); % Solver used to actually solve the optimization

opti.set_initial(w, 5); % Set initial value. Note that this may affect the result (try to set it at 15)

sol = opti.solve() % Solve

Now display the solution

wstar = sol.value(w) % Parameters that minimize the cost

wstar = 3.5364

pstar = sol.value(p) % Value of the cost function at wstar

pstar = -2.0285

figure

plot(wplot, p_plot)

hold on

scatter(wstar, pstar)

title("Minimization")

xlabel("w")

ylabel("p(w)")

Here we do a bit of practice using the casadi symbols. It will be then used as a basis for a more interesting optimization.

The system under analysis is shown in figure. The equation of motion is written using CasADi symbols. Note that being this a linear dynamical system, the differential equation can be solved using Laplace Transform. Here we do not do so just to show how casadi can be used.

import casadi.*

m=1; k=5; c=0.7; dt=0.01; % Parameters of the oscillator system

x1 = SX.sym('x1') % Define a variable x1 as a symbol (position)

x2 = SX.sym('x2') % Define a variable x2 as a symbol (velocity)

U = SX.sym('F') % Define a variable F as a symbol (force)

X = [x1;x2]; % state vector

x0 = [1; 0]

The dynamics is linear, so Laplace Transform can be used. This is just an example to show the CasADi tools

ode = [x2; (-k*x1 - c*x2 + U)/m] % dynamics of the system

Now we need a mapping from the state and control to the future state of the system

% Now we can create a CasADi function, which basically

% just put the arguments: X and U into the output: ode

f = Function('f', {X, U}, {ode}) % This could have been the kinematics model of a robot

f([0; 2], 0)

Now we can simulate the motion of the system. To do so we need to integrate the dynamics.

There are different ways to do so. In this case we will use a simple explicit Euler Integration, but CasADi also implements integration function with many options

% If we want to get the state at the next timestep we have to integrate

x_new = x0 + f(x0, 0)*dt

Now simulate the system for 10 seconds

T = 10; %5 seconds of simulation

N = round(T/dt,0);

t = linspace(0, T, N+1);

x_int = cat(2, x0, zeros(2, N));

for i=1:N

x_int(:, i+1) = x_int(:, i) + full(f(x_int(:, i), 0)*dt);

end

figure

plot(t, x_int(1, :))

title("Simulation of x")

xlabel("t[s]")

ylabel("x(t) [m]")

Now some more interesting stuff.

We move the mass away from its equilibrium (i.e. we are stretching the spring) and we ask: "How should an external force behave in order to stabilize (bring the system to zero position and velocity) the system as fast as possible?

opti = Opti()

xs = opti.variable(2, N+1); % state

us = opti.variable(1, N); % control

wx = 10; wu = 0.1; % weights of the cost function

cost = 0;

opti.subject_to(xs(:, 1) == x0); % initial condition is to start at some x0 displacement

for i=1:N % this is a sum over all the timesteps

cost = cost + ( wx*sumsqr(xs(1, i)) + wu*sumsqr(us(i)))*dt; % Cost function is zero only at equilibrium

opti.subject_to(xs(:, i+1) == xs(:, i) + f(xs(:, i), us(i))*dt); % We have to impose continuity constraints. (basically dynamics)

end

opti.minimize(cost) % Minimize the cost

opti.solver('ipopt')

opti.solve()

Now that the optimization has been solved, let's check the results

xsol = opti.value(xs)

usol = opti.value(us)

figure

plot(t, x_int(1, :), 'b--')

title("Difference between free response and forced")

hold on

plot(t, xsol(1, :))

xlabel("t[s]")

legend("x(t) Free", "x(t) Controlled")

This is the control action that drives the system in that way

figure

stairs(t(1:end-1), usol)

title("Optimized control action")

xlabel("t[s]")

ylabel("[N]")