def test_flat_default_output(self, vehicle_flat):
# Construct a flat system with the default outputs
flatsys = fs.FlatSystem(vehicle_flat.forward,
vehicle_flat.reverse,
vehicle_flat.updfcn,
inputs=vehicle_flat.ninputs,
outputs=vehicle_flat.ninputs,
states=vehicle_flat.nstates)
# Define the endpoints of the trajectory
x0 = [0., -2., 0.]
u0 = [10., 0.]
xf = [100., 2., 0.]
uf = [10., 0.]
Tf = 10
# Find trajectory between initial and final conditions
poly = fs.PolyFamily(6)
traj1 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly)
traj2 = fs.point_to_point(flatsys, Tf, x0, u0, xf, uf, basis=poly)
# Verify that the trajectory computation is correct
T = np.linspace(0, Tf, 10)
x1, u1 = traj1.eval(T)
x2, u2 = traj2.eval(T)
np.testing.assert_array_almost_equal(x1, x2)
np.testing.assert_array_almost_equal(u1, u2)
# Run a simulation and verify that the outputs are correct
resp1 = ct.input_output_response(vehicle_flat, T, u1, x0)
resp2 = ct.input_output_response(flatsys, T, u1, x0)
np.testing.assert_array_almost_equal(resp1.outputs[0:2], resp2.outputs)
def vehicle_flat(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
return fs.FlatSystem(vehicle_flat_forward,
vehicle_flat_reverse,
vehicle_update,
vehicle_output,
inputs=('v', 'delta'),
outputs=('x', 'y', 'theta'),
states=('x', 'y', 'theta'))
return x, u
# Function to compute the RHS of the system dynamics
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
# Create differentially flat input/output system
vehicle_flat = fs.FlatSystem(vehicle_flat_forward,
vehicle_flat_reverse,
vehicle_update,
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 = [40., 2., 0.]
uf = [10., 0.]
Tf = 4
# Define a set of basis functions to use for the trajectories
poly = fs.PolyFamily(6)
# Find a trajectory between the initial condition and the final condition
traj = fs.point_to_point(vehicle_flat, x0, u0, xf, uf, Tf, basis=poly)
# Given the flat variables, solve for the state
x[0] = zflag[0][0] # x position
x[1] = zflag[1][0] # y position
x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # tan(theta) = ydot/xdot
# And next solve for the inputs
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 = fs.FlatSystem(vehicle_flat_forward,
vehicle_flat_reverse,
inputs=2,
states=3)
# In[ ]:
# Utility function to plot lane change trajectory
def plot_vehicle_lanechange(traj):
# Create the trajectory
t = np.linspace(0, Tf, 100)
x, u = traj.eval(t)
# Configure matplotlib plots to be a bit bigger and optimize layout
plt.figure(figsize=[9, 4.5])
# Plot the trajectory in xy coordinate
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)
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 # return x, y, theta (full state)
# Create differentially flat input/output system
vehicle = fs.FlatSystem(_vehicle_flat_forward,
_vehicle_flat_reverse,
name="vehicle",
updfcn=_vehicle_update,
outfcn=_vehicle_output,
inputs=('v', 'delta'),
outputs=('x', 'y', 'theta'),
states=('x', 'y', 'theta'))
#
# Utility function to plot lane change manuever
#
def plot_lanechange(t, y, u, figure=None, yf=None):
# Plot the xy trajectory
plt.subplot(3, 1, 1, label='xy')
plt.plot(y[0], y[1])
plt.xlabel("x [m]")
plt.ylabel("y [m]")
+ 2 * zflag[1][3] * cos(theta) * thdot \
# - (zflag[0][2] * cos(theta)
# + (zflag[1][2] + g) * sin(theta)) * thdot**2) \
- zflag[0][2] * cos(theta) * thdot**2
- (zflag[1][2] + g) * sin(theta) * thdot**2) \
/ (zflag[0][2] * sin(theta) - (zflag[1][2] + g) * cos(theta))
return np.array([x, y, theta, xdot, ydot, thdot]), np.array([F1, F2])
pvtol = fs.FlatSystem(
pvtol_flat_forward, pvtol_flat_reverse, name='pvtol',
updfcn=pvtol_update, outfcn=pvtol_output,
states = [f'x{i}' for i in range(6)],
inputs = ['F1', 'F2'],
outputs = [f'x{i}' for i in range(6)],
params = {
'm': 4., # mass of aircraft
'J': 0.0475, # inertia around pitch axis
'r': 0.25, # distance to center of force
'g': 9.8, # gravitational constant
'c': 0.05, # damping factor (estimated)
}
)
#
# PVTOL dynamics with wind
#
def windy_update(t, x, u, params):
# Get the input vector
F1, F2, d = u