-
Notifications
You must be signed in to change notification settings - Fork 458
Expand file tree
/
Copy pathflatsys_test.py
More file actions
131 lines (107 loc) · 5.1 KB
/
Copy pathflatsys_test.py
File metadata and controls
131 lines (107 loc) · 5.1 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
#!/usr/bin/env python
#
# flatsys_test.py - test flat system module
# RMM, 29 Jun 2019
#
# This test suite checks to make sure that the basic functions supporting
# differential flat systetms are functioning. It doesn't do exhaustive
# testing of operations on flat systems. Separate unit tests should be
# created for that purpose.
import unittest
import numpy as np
import scipy as sp
import control as ct
import control.flatsys as fs
from distutils.version import StrictVersion
class TestFlatSys(unittest.TestCase):
def setUp(self):
ct.use_numpy_matrix(False)
def test_double_integrator(self):
# Define a second order integrator
sys = ct.StateSpace([[-1, 1], [0, -2]], [[0], [1]], [[1, 0]], 0)
flatsys = fs.LinearFlatSystem(sys)
# Define the endpoints of a trajectory
x1 = [0, 0]; u1 = [0]; T1 = 1
x2 = [1, 0]; u2 = [0]; T2 = 2
x3 = [0, 1]; u3 = [0]; T3 = 3
x4 = [1, 1]; u4 = [1]; T4 = 4
# Define the basis set
poly = fs.PolyFamily(6)
# Plan trajectories for various combinations
for x0, u0, xf, uf, Tf in [
(x1, u1, x2, u2, T2), (x1, u1, x3, u3, T3), (x1, u1, x4, u4, T4)]:
traj = fs.point_to_point(flatsys, x0, u0, xf, uf, Tf, basis=poly)
# 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])
# Simulate the system and make sure we stay close to desired traj
T = np.linspace(0, Tf, 100)
xd, ud = traj.eval(T)
t, y, x = ct.forced_response(sys, T, ud, x0)
np.testing.assert_array_almost_equal(x, xd, decimal=3)
def test_kinematic_car(self):
"""Differential flatness for a kinematic car"""
def vehicle_flat_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_flat_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
def vehicle_update(t, x, u, params):
b = params.get('wheelbase', 3.) # get parameter values
dx = np.array([
np.cos(x[2]) * u[0],
np.sin(x[2]) * u[0],
(u[0]/b) * np.tan(u[1])
])
return dx
def vehicle_output(t, x, u, params): return x
# Create differentially flat input/output system
vehicle_flat = fs.FlatSystem(
vehicle_flat_forward, vehicle_flat_reverse, vehicle_update,
vehicle_output, inputs=('v', 'delta'), outputs=('x', 'y', 'theta'),
states=('x', 'y', 'theta'))
# Define the endpoints of the trajectory
x0 = [0., -2., 0.]; u0 = [10., 0.]
xf = [100., 2., 0.]; uf = [10., 0.]
Tf = 10
# Define a set of basis functions to use for the trajectories
poly = fs.PolyFamily(6)
# Find trajectory between initial and final conditions
traj = fs.point_to_point(vehicle_flat, x0, u0, xf, uf, Tf, basis=poly)
# 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])
# Simulate the system and make sure we stay close to desired traj
T = np.linspace(0, Tf, 500)
xd, ud = traj.eval(T)
# For SciPy 1.0+, integrate equations and compare to desired
if StrictVersion(sp.__version__) >= "1.0":
t, y, x = ct.input_output_response(
vehicle_flat, T, ud, x0, return_x=True)
np.testing.assert_allclose(x, xd, atol=0.01, rtol=0.01)
def tearDown(self):
ct.reset_defaults()
if __name__ == '__main__':
unittest.main()