# Differentially flat system - kinematic car¶

This example demonstrates the use of the flatsys module for generating trajectories for differentially flat systems. The example is drawn from Chapter 8 of FBS2e.

## Code¶

  1# kincar-flatsys.py - differentially flat systems example
2# RMM, 3 Jul 2019
3#
4# This example demonstrates the use of the flatsys module for generating
5# trajectories for differnetially flat systems by computing a trajectory for a
6# kinematic (bicycle) model of a car changing lanes.
7
8import os
9import numpy as np
10import matplotlib.pyplot as plt
11import control as ct
12import control.flatsys as fs
13import control.optimal as opt
14
15#
16# System model and utility functions
17#
18
19# Function to take states, inputs and return the flat flag
20def vehicle_flat_forward(x, u, params={}):
21    # Get the parameter values
22    b = params.get('wheelbase', 3.)
23
24    # Create a list of arrays to store the flat output and its derivatives
25    zflag = [np.zeros(3), np.zeros(3)]
26
27    # Flat output is the x, y position of the rear wheels
28    zflag[0][0] = x[0]
29    zflag[1][0] = x[1]
30
31    # First derivatives of the flat output
32    zflag[0][1] = u[0] * np.cos(x[2])  # dx/dt
33    zflag[1][1] = u[0] * np.sin(x[2])  # dy/dt
34
35    # First derivative of the angle
36    thdot = (u[0]/b) * np.tan(u[1])
37
38    # Second derivatives of the flat output (setting vdot = 0)
39    zflag[0][2] = -u[0] * thdot * np.sin(x[2])
40    zflag[1][2] =  u[0] * thdot * np.cos(x[2])
41
42    return zflag
43
44
45# Function to take the flat flag and return states, inputs
46def vehicle_flat_reverse(zflag, params={}):
47    # Get the parameter values
48    b = params.get('wheelbase', 3.)
49
50    # Create a vector to store the state and inputs
51    x = np.zeros(3)
52    u = np.zeros(2)
53
54    # Given the flat variables, solve for the state
55    x[0] = zflag[0][0]  # x position
56    x[1] = zflag[1][0]  # y position
57    x[2] = np.arctan2(zflag[1][1], zflag[0][1])  # tan(theta) = ydot/xdot
58
59    # And next solve for the inputs
60    u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])
61    thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])
62    u[1] = np.arctan2(thdot_v, u[0]**2 / b)
63
64    return x, u
65
66# Function to compute the RHS of the system dynamics
67def vehicle_update(t, x, u, params):
68    b = params.get('wheelbase', 3.)             # get parameter values
69    dx = np.array([
70        np.cos(x[2]) * u[0],
71        np.sin(x[2]) * u[0],
72        (u[0]/b) * np.tan(u[1])
73    ])
74    return dx
75
76# Plot the trajectory in xy coordinates
77def plot_results(t, x, ud):
78    plt.subplot(4, 1, 2)
79    plt.plot(x[0], x[1])
80    plt.xlabel('x [m]')
81    plt.ylabel('y [m]')
82    plt.axis([x0[0], xf[0], x0[1]-1, xf[1]+1])
83
84    # Time traces of the state and input
85    plt.subplot(2, 4, 5)
86    plt.plot(t, x[1])
87    plt.ylabel('y [m]')
88
89    plt.subplot(2, 4, 6)
90    plt.plot(t, x[2])
92
93    plt.subplot(2, 4, 7)
94    plt.plot(t, ud[0])
95    plt.xlabel('Time t [sec]')
96    plt.ylabel('v [m/s]')
97    plt.axis([0, Tf, u0[0] - 1, uf[0] + 1])
98
99    plt.subplot(2, 4, 8)
100    plt.plot(t, ud[1])
101    plt.xlabel('Ttime t [sec]')
102    plt.ylabel('$\delta$ [rad]')
103    plt.tight_layout()
104
105#
106# Approach 1: point to point solution, no cost or constraints
107#
108
109# Create differentially flat input/output system
110vehicle_flat = fs.FlatSystem(
111    vehicle_flat_forward, vehicle_flat_reverse, vehicle_update,
112    inputs=('v', 'delta'), outputs=('x', 'y', 'theta'),
113    states=('x', 'y', 'theta'))
114
115# Define the endpoints of the trajectory
116x0 = [0., -2., 0.]; u0 = [10., 0.]
117xf = [40., 2., 0.]; uf = [10., 0.]
118Tf = 4
119
120# Define a set of basis functions to use for the trajectories
121poly = fs.PolyFamily(6)
122
123# Find a trajectory between the initial condition and the final condition
124traj = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly)
125
126# Create the desired trajectory between the initial and final condition
127T = np.linspace(0, Tf, 500)
128xd, ud = traj.eval(T)
129
130# Simulation the open system dynamics with the full input
131t, y, x = ct.input_output_response(
132    vehicle_flat, T, ud, x0, return_x=True)
133
134# Plot the open loop system dynamics
135plt.figure(1)
136plt.suptitle("Open loop trajectory for kinematic car lane change")
137plot_results(t, x, ud)
138
139#
140# Approach #2: add cost function to make lane change quicker
141#
142
143# Define timepoints for evaluation plus basis function to use
144timepts = np.linspace(0, Tf, 10)
145basis = fs.PolyFamily(8)
146
147# Define the cost function (penalize lateral error and steering)
149    vehicle_flat, np.diag([0, 0.1, 0]), np.diag([0.1, 1]), x0=xf, u0=uf)
150
151# Solve for an optimal solution
152traj = fs.point_to_point(
153    vehicle_flat, timepts, x0, u0, xf, uf, cost=traj_cost, basis=basis,
154)
155xd, ud = traj.eval(T)
156
157plt.figure(2)
158plt.suptitle("Lane change with lateral error + steering penalties")
159plot_results(T, xd, ud)
160
161#
162# Approach #3: optimal cost with trajectory constraints
163#
164# Resolve the problem with constraints on the inputs
165#
166
167constraints = [
168    opt.input_range_constraint(vehicle_flat, [8, -0.1], [12, 0.1]) ]
169
170# Solve for an optimal solution
171traj = fs.point_to_point(
172    vehicle_flat, timepts, x0, u0, xf, uf, cost=traj_cost,
173    constraints=constraints, basis=basis,
174)
175xd, ud = traj.eval(T)
176
177plt.figure(3)
178plt.suptitle("Lane change with penalty + steering constraints")
179plot_results(T, xd, ud)
180
181# Show the results unless we are running in batch mode
182if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
183    plt.show()


## Notes¶

1. The environment variable PYCONTROL_TEST_EXAMPLES is used for testing to turn off plotting of the outputs.