|
| 1 | +# flatsys_bench.py - benchmarks for flat systems package |
| 2 | +# RMM, 2 Mar 2021 |
| 3 | +# |
| 4 | +# This benchmark tests the timing for the flat system module |
| 5 | +# (control.flatsys) and is intended to be used for helping tune the |
| 6 | +# performance of the functions used for optimization-based control. |
| 7 | + |
| 8 | +import numpy as np |
| 9 | +import math |
| 10 | +import control as ct |
| 11 | +import control.flatsys as flat |
| 12 | +import control.optimal as opt |
| 13 | + |
| 14 | +# Vehicle steering dynamics |
| 15 | +def vehicle_update(t, x, u, params): |
| 16 | + # Get the parameters for the model |
| 17 | + l = params.get('wheelbase', 3.) # vehicle wheelbase |
| 18 | + phimax = params.get('maxsteer', 0.5) # max steering angle (rad) |
| 19 | + |
| 20 | + # Saturate the steering input (use min/max instead of clip for speed) |
| 21 | + phi = max(-phimax, min(u[1], phimax)) |
| 22 | + |
| 23 | + # Return the derivative of the state |
| 24 | + return np.array([ |
| 25 | + math.cos(x[2]) * u[0], # xdot = cos(theta) v |
| 26 | + math.sin(x[2]) * u[0], # ydot = sin(theta) v |
| 27 | + (u[0] / l) * math.tan(phi) # thdot = v/l tan(phi) |
| 28 | + ]) |
| 29 | + |
| 30 | +def vehicle_output(t, x, u, params): |
| 31 | + return x # return x, y, theta (full state) |
| 32 | + |
| 33 | +# Flatness structure |
| 34 | +def vehicle_forward(x, u, params={}): |
| 35 | + b = params.get('wheelbase', 3.) # get parameter values |
| 36 | + zflag = [np.zeros(3), np.zeros(3)] # list for flag arrays |
| 37 | + zflag[0][0] = x[0] # flat outputs |
| 38 | + zflag[1][0] = x[1] |
| 39 | + zflag[0][1] = u[0] * np.cos(x[2]) # first derivatives |
| 40 | + zflag[1][1] = u[0] * np.sin(x[2]) |
| 41 | + thdot = (u[0]/b) * np.tan(u[1]) # dtheta/dt |
| 42 | + zflag[0][2] = -u[0] * thdot * np.sin(x[2]) # second derivatives |
| 43 | + zflag[1][2] = u[0] * thdot * np.cos(x[2]) |
| 44 | + return zflag |
| 45 | + |
| 46 | +def vehicle_reverse(zflag, params={}): |
| 47 | + b = params.get('wheelbase', 3.) # get parameter values |
| 48 | + x = np.zeros(3); u = np.zeros(2) # vectors to store x, u |
| 49 | + x[0] = zflag[0][0] # x position |
| 50 | + x[1] = zflag[1][0] # y position |
| 51 | + x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # angle |
| 52 | + u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2]) |
| 53 | + thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2]) |
| 54 | + u[1] = np.arctan2(thdot_v, u[0]**2 / b) |
| 55 | + return x, u |
| 56 | + |
| 57 | +vehicle = flat.FlatSystem( |
| 58 | + vehicle_forward, vehicle_reverse, vehicle_update, |
| 59 | + vehicle_output, inputs=('v', 'delta'), outputs=('x', 'y', 'theta'), |
| 60 | + states=('x', 'y', 'theta')) |
| 61 | + |
| 62 | +# Initial and final conditions |
| 63 | +x0 = [0., -2., 0.]; u0 = [10., 0.] |
| 64 | +xf = [100., 2., 0.]; uf = [10., 0.] |
| 65 | +Tf = 10 |
| 66 | + |
| 67 | +# Define the time points where the cost/constraints will be evaluated |
| 68 | +timepts = np.linspace(0, Tf, 10, endpoint=True) |
| 69 | + |
| 70 | +def time_steering_point_to_point(basis_name, basis_size): |
| 71 | + if basis_name == 'poly': |
| 72 | + basis = flat.PolyFamily(basis_size) |
| 73 | + elif basis_name == 'bezier': |
| 74 | + basis = flat.BezierFamily(basis_size) |
| 75 | + |
| 76 | + # Find trajectory between initial and final conditions |
| 77 | + traj = flat.point_to_point(vehicle, Tf, x0, u0, xf, uf, basis=basis) |
| 78 | + |
| 79 | + # Verify that the trajectory computation is correct |
| 80 | + x, u = traj.eval([0, Tf]) |
| 81 | + np.testing.assert_array_almost_equal(x0, x[:, 0]) |
| 82 | + np.testing.assert_array_almost_equal(u0, u[:, 0]) |
| 83 | + np.testing.assert_array_almost_equal(xf, x[:, 1]) |
| 84 | + np.testing.assert_array_almost_equal(uf, u[:, 1]) |
| 85 | + |
| 86 | +time_steering_point_to_point.params = (['poly', 'bezier'], [6, 8]) |
| 87 | +time_steering_point_to_point.param_names = ["basis", "size"] |
| 88 | + |
| 89 | +def time_steering_cost(): |
| 90 | + # Define cost and constraints |
| 91 | + traj_cost = opt.quadratic_cost( |
| 92 | + vehicle, None, np.diag([0.1, 1]), u0=uf) |
| 93 | + constraints = [ |
| 94 | + opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ] |
| 95 | + |
| 96 | + traj = flat.point_to_point( |
| 97 | + vehicle, timepts, x0, u0, xf, uf, cost=traj_cost |
| 98 | + ) |
| 99 | + |
| 100 | + # Verify that the trajectory computation is correct |
| 101 | + x, u = traj.eval([0, Tf]) |
| 102 | + np.testing.assert_array_almost_equal(x0, x[:, 0]) |
| 103 | + np.testing.assert_array_almost_equal(u0, u[:, 0]) |
| 104 | + np.testing.assert_array_almost_equal(xf, x[:, 1]) |
| 105 | + np.testing.assert_array_almost_equal(uf, u[:, 1]) |
| 106 | + |
| 107 | +def skip_steering_bezier_basis(nbasis, ntimes): |
| 108 | + # Set up costs and constriants |
| 109 | + Q = np.diag([.1, 10, .1]) # keep lateral error low |
| 110 | + R = np.diag([1, 1]) # minimize applied inputs |
| 111 | + cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf) |
| 112 | + constraints = [ opt.input_range_constraint(vehicle, [0, -0.1], [20, 0.1]) ] |
| 113 | + terminal = [ opt.state_range_constraint(vehicle, xf, xf) ] |
| 114 | + |
| 115 | + # Set up horizon |
| 116 | + horizon = np.linspace(0, Tf, ntimes, endpoint=True) |
| 117 | + |
| 118 | + # Set up the optimal control problem |
| 119 | + res = opt.solve_ocp( |
| 120 | + vehicle, horizon, x0, cost, |
| 121 | + constraints, |
| 122 | + terminal_constraints=terminal, |
| 123 | + initial_guess=bend_left, |
| 124 | + basis=flat.BezierFamily(nbasis, T=Tf), |
| 125 | + # solve_ivp_kwargs={'atol': 1e-4, 'rtol': 1e-2}, |
| 126 | + minimize_method='trust-constr', |
| 127 | + minimize_options={'finite_diff_rel_step': 0.01}, |
| 128 | + # minimize_method='SLSQP', minimize_options={'eps': 0.01}, |
| 129 | + return_states=True, print_summary=False |
| 130 | + ) |
| 131 | + t, u, x = res.time, res.inputs, res.states |
| 132 | + |
| 133 | + # Make sure we found a valid solution |
| 134 | + assert res.success |
| 135 | + |
| 136 | +# Reset the timeout value to allow for longer runs |
| 137 | +skip_steering_bezier_basis.timeout = 120 |
| 138 | + |
| 139 | +# Set the parameter values for the number of times and basis vectors |
| 140 | +skip_steering_bezier_basis.param_names = ['nbasis', 'ntimes'] |
| 141 | +skip_steering_bezier_basis.params = ([2, 4, 6], [5, 10, 20]) |
| 142 | + |
0 commit comments