Skip to content

Update benchmarks to help with optimal control tuning #800

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 5 commits into from
Nov 26, 2022
Merged

Update benchmarks to help with optimal control tuning #800

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
e14d7b3
move optimal_bench.py to steering_bench.py + small fixes
murrayrm Aug 27, 2022
08f6464
improved flat system benchmarking (+ docstring, unit test updates)
murrayrm Aug 27, 2022
efca9ca
new optimal benchmarks (replace steering with rss)
murrayrm Aug 28, 2022
2860d89
add error for using solve_ivp for discrete time + formatting tweaks
murrayrm Aug 29, 2022
54de2a3
allow print_summary to be set in solve_ocp
murrayrm Aug 30, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion benchmarks/README
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ you can use the following command from the root directory of the repository:

PYTHONPATH=`pwd` asv run --python=python

You can also run benchmarks against specific commits usuing
You can also run benchmarks against specific commits using

asv run <range>

Expand Down
84 changes: 71 additions & 13 deletions benchmarks/flatsys_bench.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,10 @@
import control.flatsys as flat
import control.optimal as opt

#
# System setup: vehicle steering (bicycle model)
#

# Vehicle steering dynamics
def vehicle_update(t, x, u, params):
# Get the parameters for the model
Expand Down Expand Up @@ -67,11 +71,28 @@ def vehicle_reverse(zflag, params={}):
# Define the time points where the cost/constraints will be evaluated
timepts = np.linspace(0, Tf, 10, endpoint=True)

def time_steering_point_to_point(basis_name, basis_size):
if basis_name == 'poly':
basis = flat.PolyFamily(basis_size)
elif basis_name == 'bezier':
basis = flat.BezierFamily(basis_size)
#
# Benchmark test parameters
#

basis_params = (['poly', 'bezier', 'bspline'], [8, 10, 12])
basis_param_names = ["basis", "size"]

def get_basis(name, size):
if name == 'poly':
basis = flat.PolyFamily(size, T=Tf)
elif name == 'bezier':
basis = flat.BezierFamily(size, T=Tf)
elif name == 'bspline':
basis = flat.BSplineFamily([0, Tf/2, Tf], size)
return basis

#
# Benchmarks
#

def time_point_to_point(basis_name, basis_size):
basis = get_basis(basis_name, basis_size)

# Find trajectory between initial and final conditions
traj = flat.point_to_point(vehicle, Tf, x0, u0, xf, uf, basis=basis)
Expand All @@ -80,13 +101,16 @@ def time_steering_point_to_point(basis_name, basis_size):
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])
np.testing.assert_array_almost_equal(xf, x[:, -1])
np.testing.assert_array_almost_equal(uf, u[:, -1])

time_point_to_point.params = basis_params
time_point_to_point.param_names = basis_param_names

time_steering_point_to_point.params = (['poly', 'bezier'], [6, 8])
time_steering_point_to_point.param_names = ["basis", "size"]

def time_steering_cost():
def time_point_to_point_with_cost(basis_name, basis_size):
basis = get_basis(basis_name, basis_size)

# Define cost and constraints
traj_cost = opt.quadratic_cost(
vehicle, None, np.diag([0.1, 1]), u0=uf)
Expand All @@ -95,13 +119,47 @@ def time_steering_cost():

traj = flat.point_to_point(
vehicle, timepts, x0, u0, xf, uf,
cost=traj_cost, constraints=constraints, basis=flat.PolyFamily(8)
cost=traj_cost, constraints=constraints, basis=basis,
)

# 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])
np.testing.assert_array_almost_equal(xf, x[:, -1])
np.testing.assert_array_almost_equal(uf, u[:, -1])

time_point_to_point_with_cost.params = basis_params
time_point_to_point_with_cost.param_names = basis_param_names


def time_solve_flat_ocp_terminal_cost(method, basis_name, basis_size):
basis = get_basis(basis_name, basis_size)

# Define cost and constraints
traj_cost = opt.quadratic_cost(
vehicle, None, np.diag([0.1, 1]), u0=uf)
term_cost = opt.quadratic_cost(
vehicle, np.diag([1e3, 1e3, 1e3]), None, x0=xf)
constraints = [
opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]

# Initial guess = straight line
initial_guess = np.array(
[x0[i] + (xf[i] - x0[i]) * timepts/Tf for i in (0, 1)])

traj = flat.solve_flat_ocp(
vehicle, timepts, x0, u0, basis=basis, initial_guess=initial_guess,
trajectory_cost=traj_cost, constraints=constraints,
terminal_cost=term_cost, minimize_method=method,
)

# 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(xf, x[:, -1], decimal=2)

time_solve_flat_ocp_terminal_cost.params = tuple(
[['slsqp', 'trust-constr']] + list(basis_params))
time_solve_flat_ocp_terminal_cost.param_names = tuple(
['method'] + basis_param_names)
Loading