initial implementation of optimization for flatsystems

This commit allows a cost function and constraints to be passed to the flatsys point_to_point() function and when present will use sp.optimize to find a trajectory. Preliminary unit tests, benchmarks, docstrings and documentation also in place. Switched the order of arguments in point_to_point() so that Tf (or timepts) now comes before the initial and final states, consistent with ordering elsewhere in the package. Make some small updates to optimal.py docstrings and argument names to make things consistent with flatsys.
murrayrm · Mar 19, 2021 · Mar 2, 2021 · Mar 7, 2021 · Mar 8, 2021 · Mar 13, 2021
commit 8a9ab951d589b169f916d4527be906f2d8858804
diff --git a/.gitignore b/.gitignore
@@ -26,3 +26,6 @@ TAGS
 
 # Files created by or for asv (airspeed velocity)
 .asv/
+
+# Files created by Spyder
+.spyproject/
diff --git a/benchmarks/flatsys_bench.py b/benchmarks/flatsys_bench.py
@@ -0,0 +1,142 @@
+# flatsys_bench.py - benchmarks for flat systems package
+# RMM, 2 Mar 2021
+#
+# This benchmark tests the timing for the flat system module
+# (control.flatsys) and is intended to be used for helping tune the
+# performance of the functions used for optimization-based control.
+
+import numpy as np
+import math
+import control as ct
+import control.flatsys as flat
+import control.optimal as opt
+
+# Vehicle steering dynamics
+def vehicle_update(t, x, u, params):
+    # Get the parameters for the model
+    l = params.get('wheelbase', 3.)         # vehicle wheelbase
+    phimax = params.get('maxsteer', 0.5)    # max steering angle (rad)
+
+    # Saturate the steering input (use min/max instead of clip for speed)
+    phi = max(-phimax, min(u[1], phimax))
+
+    # Return the derivative of the state
+    return np.array([
+        math.cos(x[2]) * u[0],            # xdot = cos(theta) v
+        math.sin(x[2]) * u[0],            # ydot = sin(theta) v
+        (u[0] / l) * math.tan(phi)        # thdot = v/l tan(phi)
+    ])
+
+def vehicle_output(t, x, u, params):
+    return x                            # return x, y, theta (full state)
+
+# Flatness structure
+def vehicle_forward(x, u, params={}):
+    b = params.get('wheelbase', 3.)             # get parameter values
+    zflag = [np.zeros(3), np.zeros(3)]          # list for flag arrays
+    zflag[0][0] = x[0]                          # flat outputs
+    zflag[1][0] = x[1]
+    zflag[0][1] = u[0] * np.cos(x[2])           # first derivatives
+    zflag[1][1] = u[0] * np.sin(x[2])
+    thdot = (u[0]/b) * np.tan(u[1])             # dtheta/dt
+    zflag[0][2] = -u[0] * thdot * np.sin(x[2])  # second derivatives
+    zflag[1][2] =  u[0] * thdot * np.cos(x[2])
+    return zflag
+
+def vehicle_reverse(zflag, params={}):
+    b = params.get('wheelbase', 3.)             # get parameter values
+    x = np.zeros(3); u = np.zeros(2)            # vectors to store x, u
+    x[0] = zflag[0][0]                          # x position
+    x[1] = zflag[1][0]                          # y position
+    x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # angle
+    u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])
+    thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])
+    u[1] = np.arctan2(thdot_v, u[0]**2 / b)
+    return x, u
+
+vehicle = flat.FlatSystem(
+    vehicle_forward, vehicle_reverse, vehicle_update,
+    vehicle_output, inputs=('v', 'delta'), outputs=('x', 'y', 'theta'),
+    states=('x', 'y', 'theta'))
+
+# Initial and final conditions
+x0 = [0., -2., 0.]; u0 = [10., 0.]
+xf = [100., 2., 0.]; uf = [10., 0.]
+Tf = 10
+
+# Define the time points where the cost/constraints will be evaluated
+timepts = np.linspace(0, Tf, 10, endpoint=True)
+
+def time_steering_point_to_point(basis_name, basis_size):
+    if basis_name == 'poly':
+        basis = flat.PolyFamily(basis_size)
+    elif basis_name == 'bezier':
+        basis = flat.BezierFamily(basis_size)
+
+    # Find trajectory between initial and final conditions
+    traj = flat.point_to_point(vehicle, Tf, x0, u0, xf, uf, basis=basis)
+
+    # Verify that the trajectory computation is correct
+    x, u = traj.eval([0, Tf])
+    np.testing.assert_array_almost_equal(x0, x[:, 0])
+    np.testing.assert_array_almost_equal(u0, u[:, 0])
+    np.testing.assert_array_almost_equal(xf, x[:, 1])
+    np.testing.assert_array_almost_equal(uf, u[:, 1])
+
+time_steering_point_to_point.params = (['poly', 'bezier'], [6, 8])
+time_steering_point_to_point.param_names = ["basis", "size"]        
+
+def time_steering_cost():
+    # Define cost and constraints
+    traj_cost = opt.quadratic_cost(
+        vehicle, None, np.diag([0.1, 1]), u0=uf)
+    constraints = [
+        opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
+
+    traj = flat.point_to_point(
+        vehicle, timepts, x0, u0, xf, uf, cost=traj_cost
+    )
+
+    # Verify that the trajectory computation is correct
+    x, u = traj.eval([0, Tf])
+    np.testing.assert_array_almost_equal(x0, x[:, 0])
+    np.testing.assert_array_almost_equal(u0, u[:, 0])
+    np.testing.assert_array_almost_equal(xf, x[:, 1])
+    np.testing.assert_array_almost_equal(uf, u[:, 1])
+
+def skip_steering_bezier_basis(nbasis, ntimes):
+    # Set up costs and constriants
+    Q = np.diag([.1, 10, .1])           # keep lateral error low
+    R = np.diag([1, 1])                 # minimize applied inputs
+    cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
+    constraints = [ opt.input_range_constraint(vehicle, [0, -0.1], [20, 0.1]) ]
+    terminal = [ opt.state_range_constraint(vehicle, xf, xf) ]
+
+    # Set up horizon
+    horizon = np.linspace(0, Tf, ntimes, endpoint=True)
+
+    # Set up the optimal control problem
+    res = opt.solve_ocp(
+        vehicle, horizon, x0, cost,
+        constraints,
+        terminal_constraints=terminal,
+        initial_guess=bend_left,
+        basis=flat.BezierFamily(nbasis, T=Tf),
+        # solve_ivp_kwargs={'atol': 1e-4, 'rtol': 1e-2},
+        minimize_method='trust-constr',
+        minimize_options={'finite_diff_rel_step': 0.01},
+        # minimize_method='SLSQP', minimize_options={'eps': 0.01},
+        return_states=True, print_summary=False
+    )
+    t, u, x = res.time, res.inputs, res.states
+
+    # Make sure we found a valid solution
+    assert res.success
+
+# Reset the timeout value to allow for longer runs
+skip_steering_bezier_basis.timeout = 120
+
+# Set the parameter values for the number of times and basis vectors
+skip_steering_bezier_basis.param_names = ['nbasis', 'ntimes']
+skip_steering_bezier_basis.params = ([2, 4, 6], [5, 10, 20])
+
diff --git a/benchmarks/optimal_bench.py b/benchmarks/optimal_bench.py
@@ -1,5 +1,5 @@
 # optimal_bench.py - benchmarks for optimal control package
-# RMM, 27 Feb 2020
+# RMM, 27 Feb 2021
 #
 # This benchmark tests the timing for the optimal control module
 # (control.optimal) and is intended to be used for helping tune the
@@ -10,10 +10,6 @@
 import control as ct
 import control.flatsys as flat
 import control.optimal as opt
-import matplotlib.pyplot as plt
-import logging
-import time
-import os
 
 # Vehicle steering dynamics
 def vehicle_update(t, x, u, params):