Commit 4932c49c authored by Stefan Ecklebe's avatar Stefan Ecklebe
Browse files


parent 982dad49
# -*- coding: utf-8 -*-
Basic commented example (double integrator) illustrating the use of MPCSolver
Author: Patrick Rüdiger
import numpy as np
import casadi as ca
from mpc import MPCSolver
from system import System
from plotter import Plotter
from ocp import OptimalControlProblem
def create_system():
# Set the system name.
name = 'double_int'
# Try to load system from "systems/sys_cache.dat".
sys = System.load(name)
return sys
except (KeyError, FileNotFoundError):
# Set dimensions.
x_dim = 2 # dimension of system state
u_dim = 1 # dimension of system input
p_dim = 2 # dimension of parameter vector
# CasADi symbolic variables (only CasADi SX and MX symbolics are supported)
# Note: MX symbolics are preferable for saving a string representation of
# the system's rhs via Regardless, SX symbolics will be used
# internally for all future calculations.
x = ca.MX.sym('x', x_dim) # symbolic variable for state
u = ca.MX.sym('u', u_dim) # symbolic variable for input
p = ca.MX.sym('p', p_dim) # symbolic variable for parameter vector
# Collect all symbolic variables in dictionary sym_dict.
# This dictionary must contain entries 'x' and 'u'.
# Additional entries with user-defined keys will be treated as parameters.
sym_dict = {'x': x, 'u': u, 'p': p}
# Assign parameter values to all additional entries in sym_dict.
# Note: Time-variant system parameters are currently not supported. All
# parameters will be replaced by their respective values on construction.
# However, providing a parametrization allows changing the system
# parameters via System.set_p_vals().
p_val_dict = {'p': np.ones(2)}
# Optional: Add user-defined descriptions.
sxup_dict = {'name': name,
'x_0': 'position', 'x_1': 'velocity',
'u_0': 'acceleration',
'p_0': 'd_0', 'p_1': 'd_1'}
# Provide state-space representation (rhs) of the system using the
# previously defined symbolic variables including parameter symbols.
x_dot_p = [p[0]*x[1], p[1]*u[0]]
# Use vertical concatenation for MX expressions.
x_dot_p = ca.vertcat(*x_dot_p)
# Create System object. See constructor docstring for details.
sys = System(x_dot_p, sym_dict, p_val_dict, sxup_dict)
# Save the created system to the system cache file "systems/sys_cache.dat".
# This cache file will contain all saved systems. The filenames of all
# saved systems will be listed in "systems/sys_filenames.txt".
# Note: The system's rhs is saved as string representation due to problems
# saving SwigPyCasadiObject objects on Windows. Unfortunately, this
# currently works only for certain MX expressions.
return sys
def create_ocp():
# Set the ocp mode and name.
mode = 'free'
name = sys.sxup_dict['name'] + '_' + mode
# Try to load ocp from "ocps/sys_cache.dat".
# Set initial system state and previous system input.
x_0 = np.array([1,0.1 ])
uprev = np.zeros(sys.n_u)
# Set constant state and input reference.
x_ref = np.zeros((1, sys.n_x))
u_ref = np.zeros((1, sys.n_u))
# Specify time grid for simulation and discretization via time step size T
# and final time in seconds t_f.
T = 0.05
t_f = 1.5
tgrid = np.linspace(0, t_f, int(t_f/T)+1)
assert tgrid[-1] == t_f and tgrid[1] - tgrid[0] == T
# Set lower bounds for state 'x', input 'u' and change of input 'Du'.
lb = {'x': np.array([-2, -2]),
'u': -5,
'Du': -5}
# Set upper bounds for state 'x', input 'u' and change of input 'Du'.
ub = {'x': np.array([2, 0]),
'u': 5,
'Du': 5}
# Create OptimalControlProblem object. See constructor docstring for
# details.
ocp = OptimalControlProblem(sys, mode, x_0, tgrid, x_ref, u_ref,
kwargs={'lb': lb, 'ub': ub,'uprev': uprev, 'ca_fun_dict': None})
# Save the created ocp to the system cache file "ocps/ocp_cache.dat".
# This cache file will contain all saved ocps. The filenames of all
# saved ocps will be listed in "ocps/ocp_filenames.txt".
# Note: Custom CasADi function objects are saved as string representation
# due to problems saving SwigPyCasadiObject objects on Windows.
# Unfortunately, this currently works only for certain MX expressions.
return ocp
def create_solver():
# Specify keyword arguments of MPCSolver constructor.
kwargs = {'N': 10,
'receding': 'ltd',
'u_guess': None,
'x_guess': None,
'verbosity': 0}
# Create MPCSolver object. See constructor docstring for details.
solver = MPCSolver(ocp, **kwargs)
return solver
def print_results():
name = sys.sxup_dict['name'] + '_' + ocp.mode + '_' + solver.receding
print('{:<20} {:<10} {:<10}'.format('', 'J_f', 'e_f'))
print('{:<20} {:<10.1f} {:<10.1e}'.format(name, sol['J_f'], sol['e_f']))
def plot_results():
to_plot = [[['x_0'], ['x_1'], ['u_0']]]
fig_size = [30/2.54, 20/2.54]
font_size = 20
fig_name = sys.sxup_dict['name'] + '_' + ocp.mode + '_' + solver.receding
show = True
tud_blue = '#00305d'
colors = [[[tud_blue], [tud_blue], [tud_blue]]]
drawstyles = [[['default'], ['default'], ['steps-post']]]
linewidths = [[[2], [2], [2]]]
x_min = -0.03
x_max = 1.53
x_ticks = [0, 0.25, 0.5, 0.75, 1, 1.25, 1.5]
labels = {'t': r'$t$ in s $\rightarrow$',
'x_0': r'$\uparrow$' + '\n' + r'$x_0$ in m',
'x_1': r'$\uparrow$' + '\n' + r'$x_1$ in '
+ r'$\mathrm{\frac{m}{s}}$',
'u_0': r'$\uparrow$' + '\n' + r'$u$ in '
+ r'$\mathrm{\frac{m}{s^2}}$'}
x_array = sol['tgrid_sim']
x_label = labels['t']
y_arrays = []
y_labels = []
for i in range(len(to_plot)):
pltr = Plotter(x_label, x_array)
for k in range(len(to_plot[i])):
for j in range(len(to_plot[i][k])):
key = to_plot[i][k][j]
if 'x' in key:
y_arrays[i][k].append(sol['x_sim'][:, int(key[-1])])
elif 'u' in key:
y_arrays[i][k].append(sol['u_sim'][:, int(key[-1])])
y_labels[-1] += key
y_labels[-1] = labels[y_labels[-1]]
pltr.add_subplot(y_arrays[i][k], y_labels[-1], colors=colors[i][k],
pltr.create_plot(fig_size, fig_name, font_size=font_size, show=show,
x_min=x_min, x_max=x_max, x_ticks=x_ticks, y_lpad=30)
if __name__ == '__main__':
# Create system, optimal control problem (ocp) and solver.
sys = create_system()
ocp = create_ocp()
solver = create_solver()
# Solve the ocp in the specified manner. See docstring of solve() function
# for details. Solution (sol) will be cached to "sols/sol_cache.dat".
sol = solver.solve()
# Show results.
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment