|
24 | 24 | atol = 1e-4 |
25 | 25 | rtol = 1e-4 |
26 | 26 |
|
| 27 | +# Define the kinematic car system |
| 28 | +def vehicle_flat_forward(x, u, params={}): |
| 29 | + b = params.get('wheelbase', 3.) # get parameter values |
| 30 | + zflag = [np.zeros(3), np.zeros(3)] # list for flag arrays |
| 31 | + zflag[0][0] = x[0] # flat outputs |
| 32 | + zflag[1][0] = x[1] |
| 33 | + zflag[0][1] = u[0] * np.cos(x[2]) # first derivatives |
| 34 | + zflag[1][1] = u[0] * np.sin(x[2]) |
| 35 | + thdot = (u[0]/b) * np.tan(u[1]) # dtheta/dt |
| 36 | + zflag[0][2] = -u[0] * thdot * np.sin(x[2]) # second derivatives |
| 37 | + zflag[1][2] = u[0] * thdot * np.cos(x[2]) |
| 38 | + return zflag |
| 39 | + |
| 40 | +def vehicle_flat_reverse(zflag, params={}): |
| 41 | + b = params.get('wheelbase', 3.) # get parameter values |
| 42 | + x = np.zeros(3); u = np.zeros(2) # vectors to store x, u |
| 43 | + x[0] = zflag[0][0] # x position |
| 44 | + x[1] = zflag[1][0] # y position |
| 45 | + x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # angle |
| 46 | + u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2]) |
| 47 | + thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2]) |
| 48 | + u[1] = np.arctan2(thdot_v, u[0]**2 / b) |
| 49 | + return x, u |
| 50 | + |
| 51 | +def vehicle_update(t, x, u, params): |
| 52 | + b = params.get('wheelbase', 3.) # get parameter values |
| 53 | + dx = np.array([ |
| 54 | + np.cos(x[2]) * u[0], |
| 55 | + np.sin(x[2]) * u[0], |
| 56 | + (u[0]/b) * np.tan(u[1]) |
| 57 | + ]) |
| 58 | + return dx |
| 59 | + |
| 60 | +def vehicle_output(t, x, u, params): return x |
| 61 | + |
| 62 | +# Create differentially flat input/output system |
| 63 | +vehicle_flat = fs.FlatSystem( |
| 64 | + vehicle_flat_forward, vehicle_flat_reverse, vehicle_update, |
| 65 | + vehicle_output, inputs=('v', 'delta'), outputs=('x', 'y', 'theta'), |
| 66 | + states=('x', 'y', 'theta'), name='vehicle_flat') |
| 67 | + |
| 68 | + |
27 | 69 | class TestFlatSys: |
28 | 70 | """Test differential flat systems""" |
29 | 71 |
|
@@ -58,48 +100,9 @@ def test_double_integrator(self, xf, uf, Tf, basis): |
58 | 100 | t, y, x = ct.forced_response(sys, T, ud, x1, return_x=True) |
59 | 101 | np.testing.assert_array_almost_equal(x, xd, decimal=3) |
60 | 102 |
|
61 | | - @pytest.fixture |
62 | | - def vehicle_flat(self): |
63 | | - """Differential flatness for a kinematic car""" |
64 | | - def vehicle_flat_forward(x, u, params={}): |
65 | | - b = params.get('wheelbase', 3.) # get parameter values |
66 | | - zflag = [np.zeros(3), np.zeros(3)] # list for flag arrays |
67 | | - zflag[0][0] = x[0] # flat outputs |
68 | | - zflag[1][0] = x[1] |
69 | | - zflag[0][1] = u[0] * np.cos(x[2]) # first derivatives |
70 | | - zflag[1][1] = u[0] * np.sin(x[2]) |
71 | | - thdot = (u[0]/b) * np.tan(u[1]) # dtheta/dt |
72 | | - zflag[0][2] = -u[0] * thdot * np.sin(x[2]) # second derivatives |
73 | | - zflag[1][2] = u[0] * thdot * np.cos(x[2]) |
74 | | - return zflag |
75 | | - |
76 | | - def vehicle_flat_reverse(zflag, params={}): |
77 | | - b = params.get('wheelbase', 3.) # get parameter values |
78 | | - x = np.zeros(3); u = np.zeros(2) # vectors to store x, u |
79 | | - x[0] = zflag[0][0] # x position |
80 | | - x[1] = zflag[1][0] # y position |
81 | | - x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # angle |
82 | | - u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2]) |
83 | | - thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2]) |
84 | | - u[1] = np.arctan2(thdot_v, u[0]**2 / b) |
85 | | - return x, u |
86 | | - |
87 | | - def vehicle_update(t, x, u, params): |
88 | | - b = params.get('wheelbase', 3.) # get parameter values |
89 | | - dx = np.array([ |
90 | | - np.cos(x[2]) * u[0], |
91 | | - np.sin(x[2]) * u[0], |
92 | | - (u[0]/b) * np.tan(u[1]) |
93 | | - ]) |
94 | | - return dx |
95 | | - |
96 | | - def vehicle_output(t, x, u, params): return x |
97 | | - |
98 | | - # Create differentially flat input/output system |
99 | | - return fs.FlatSystem( |
100 | | - vehicle_flat_forward, vehicle_flat_reverse, vehicle_update, |
101 | | - vehicle_output, inputs=('v', 'delta'), outputs=('x', 'y', 'theta'), |
102 | | - states=('x', 'y', 'theta')) |
| 103 | + @pytest.fixture(name='vehicle_flat') |
| 104 | + def vehicle_flat_fixture(self): |
| 105 | + return vehicle_flat |
103 | 106 |
|
104 | 107 | @pytest.mark.parametrize("basis", [ |
105 | 108 | fs.PolyFamily(6), fs.PolyFamily(8), fs.BezierFamily(6), |
@@ -811,3 +814,61 @@ def test_flatsys_factory_function(self, vehicle_flat): |
811 | 814 |
|
812 | 815 | with pytest.raises(TypeError, match="incorrect number or type"): |
813 | 816 | flatsys = fs.flatsys(1, 2, 3, 4, 5) |
| 817 | + |
| 818 | + |
| 819 | +if __name__ == '__main__': |
| 820 | + # Generate images for User Guide |
| 821 | + import matplotlib.pyplot as plt |
| 822 | + |
| 823 | + # |
| 824 | + # Point to point |
| 825 | + # |
| 826 | + # Define the endpoints of the trajectory |
| 827 | + x0 = [0., -2., 0.]; u0 = [10., 0.] |
| 828 | + xf = [100., 2., 0.]; uf = [10., 0.] |
| 829 | + Tf = 10 |
| 830 | + |
| 831 | + # Define a set of basis functions to use for the trajectories |
| 832 | + poly = fs.PolyFamily(6) |
| 833 | + |
| 834 | + # Find a trajectory between the initial condition and the final condition |
| 835 | + traj = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly) |
| 836 | + |
| 837 | + # Create the trajectory |
| 838 | + timepts = np.linspace(0, Tf, 100) |
| 839 | + xd, ud = traj.eval(timepts) |
| 840 | + resp_p2p = ct.input_output_response(vehicle_flat, timepts, ud, X0=xd[:, 0]) |
| 841 | + |
| 842 | + # |
| 843 | + # Solve OCP |
| 844 | + # |
| 845 | + # Define the cost along the trajectory: penalize steering angle |
| 846 | + traj_cost = ct.optimal.quadratic_cost( |
| 847 | + vehicle_flat, None, np.diag([0.1, 10]), u0=uf) |
| 848 | + |
| 849 | + # Define the terminal cost: penalize distance from the end point |
| 850 | + term_cost = ct.optimal.quadratic_cost( |
| 851 | + vehicle_flat, np.diag([1e3, 1e3, 1e3]), None, x0=xf) |
| 852 | + |
| 853 | + # Use a straight line as the initial guess |
| 854 | + evalpts = np.linspace(0, Tf, 10) |
| 855 | + initial_guess = np.array( |
| 856 | + [x0[i] + (xf[i] - x0[i]) * evalpts/Tf for i in (0, 1)]) |
| 857 | + |
| 858 | + # Solve the optimal control problem, evaluating cost at timepts |
| 859 | + bspline = fs.BSplineFamily([0, Tf/2, Tf], 4) |
| 860 | + traj = fs.solve_flat_ocp( |
| 861 | + vehicle_flat, evalpts, x0, u0, traj_cost, |
| 862 | + terminal_cost=term_cost, initial_guess=initial_guess, basis=bspline) |
| 863 | + |
| 864 | + xd, ud = traj.eval(timepts) |
| 865 | + resp_ocp = ct.input_output_response(vehicle_flat, timepts, ud, X0=xd[:, 0]) |
| 866 | + |
| 867 | + # |
| 868 | + # Plot the results |
| 869 | + # |
| 870 | + cplt = ct.time_response_plot( |
| 871 | + ct.combine_time_responses([resp_p2p, resp_ocp]), |
| 872 | + overlay_traces=True, trace_labels=['point_to_point', 'solve_ocp']) |
| 873 | + |
| 874 | + plt.savefig('flatsys-steering-compare.png') |
0 commit comments