Skip to content

Commit c3c6596

Browse files
murrayrmmurrayrm
authored
Merge pull request #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)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy