Skip to content

Commit c3c6596

Browse files
murrayrmmurrayrm
authored
Merge pull request python-control#569 from murrayrm/optimal_flatsys
Add optimization to flat systems trajectory generation
2 parents 22b9953 + 0c1d638 commit c3c6596

File tree

13 files changed

+881
-407
lines changed

13 files changed

+881
-407
lines changed

‎.gitignore

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,3 +26,6 @@ TAGS
2626

2727
# Files created by or for asv (airspeed velocity)
2828
.asv/
29+
30+
# Files created by Spyder
31+
.spyproject/

‎benchmarks/flatsys_bench.py

Lines changed: 107 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,107 @@
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,
98+
cost=traj_cost, constraints=constraints, basis=flat.PolyFamily(8)
99+
)
100+
101+
# Verify that the trajectory computation is correct
102+
x, u = traj.eval([0, Tf])
103+
np.testing.assert_array_almost_equal(x0, x[:, 0])
104+
np.testing.assert_array_almost_equal(u0, u[:, 0])
105+
np.testing.assert_array_almost_equal(xf, x[:, 1])
106+
np.testing.assert_array_almost_equal(uf, u[:, 1])
107+

‎benchmarks/optimal_bench.py

Lines changed: 1 addition & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
# optimal_bench.py - benchmarks for optimal control package
2-
# RMM, 27 Feb 2020
2+
# RMM, 27 Feb 2021
33
#
44
# This benchmark tests the timing for the optimal control module
55
# (control.optimal) and is intended to be used for helping tune the
@@ -10,10 +10,6 @@
1010
import control as ct
1111
import control.flatsys as flat
1212
import control.optimal as opt
13-
import matplotlib.pyplot as plt
14-
import logging
15-
import time
16-
import os
1713

1814
# Vehicle steering dynamics
1915
def vehicle_update(t, x, u, params):

‎control/flatsys/bezier.py

Lines changed: 22 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@
3939
# SUCH DAMAGE.
4040

4141
import numpy as np
42-
from scipy.special import binom
42+
from scipy.special import binom, factorial
4343
from .basis import BasisFamily
4444

4545
class BezierFamily(BasisFamily):
@@ -48,7 +48,9 @@ class BezierFamily(BasisFamily):
4848
This class represents the family of polynomials of the form
4949
5050
.. math::
51-
\phi_i(t) = \sum_{i=0}^n {n \choose i} (T - t)^{n-i} t^i
51+
\phi_i(t) = \sum_{i=0}^n {n \choose i}
52+
\left( \frac{t}{T_\text{f}} - t \right)^{n-i}
53+
\left( \frac{t}{T_f} \right)^i
5254
5355
"""
5456
def __init__(self, N, T=1):
@@ -59,11 +61,23 @@ def __init__(self, N, T=1):
5961
# Compute the kth derivative of the ith basis function at time t
6062
def eval_deriv(self, i, k, t):
6163
"""Evaluate the kth derivative of the ith basis function at time t."""
62-
if k > 0:
63-
raise NotImplementedError("Bezier derivatives not yet available")
64-
elif i > self.N:
64+
if i >= self.N:
6565
raise ValueError("Basis function index too high")
66+
elif k >= self.N:
67+
# Higher order derivatives are zero
68+
return np.zeros(t.shape)
6669

67-
# Return the Bezier basis function (note N = # basis functions)
68-
return binom(self.N - 1, i) * \
69-
(t/self.T)**i * (1 - t/self.T)**(self.N - i - 1)
70+
# Compute the variables used in Bezier curve formulas
71+
n = self.N - 1
72+
u = t/self.T
73+
74+
if k == 0:
75+
# No derivative => avoid expansion for speed
76+
return binom(n, i) * u**i * (1-u)**(n-i)
77+
78+
# Return the kth derivative of the ith Bezier basis function
79+
return binom(n, i) * sum([
80+
(-1)**(j-i) *
81+
binom(n-i, j-i) * factorial(j)/factorial(j-k) * np.power(u, j-k)
82+
for j in range(max(i, k), n+1)
83+
])

0 commit comments

Comments
 (0)