def test_double_integrator(self):
# Define a second order integrator
sys = ct.StateSpace([[-1, 1], [0, -2]], [[0], [1]], [[1, 0]], 0)
flatsys = fs.LinearFlatSystem(sys)
# Define the endpoints of a trajectory
x1 = [0, 0]; u1 = [0]; T1 = 1
x2 = [1, 0]; u2 = [0]; T2 = 2
x3 = [0, 1]; u3 = [0]; T3 = 3
x4 = [1, 1]; u4 = [1]; T4 = 4
# Define the basis set
poly = fs.PolyFamily(6)
# Plan trajectories for various combinations
for x0, u0, xf, uf, Tf in [
(x1, u1, x2, u2, T2), (x1, u1, x3, u3, T3), (x1, u1, x4, u4, T4)]:
traj = fs.point_to_point(flatsys, 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, 100)
xd, ud = traj.eval(T)
t, y, x = ct.forced_response(sys, T, ud, x0)
np.testing.assert_array_almost_equal(x, xd, decimal=3)
def test_response(self, xf, uf, Tf):
# Define a second order integrator
sys = ct.StateSpace([[-1, 1], [0, -2]], [[0], [1]], [[1, 0]], 0)
flatsys = fs.LinearFlatSystem(sys)
# Define the basis set
poly = fs.PolyFamily(6)
x1, u1, = [0, 0], [0]
traj = fs.point_to_point(flatsys, Tf, x1, u1, xf, uf, basis=poly)
# Compute the response the regular way
T = np.linspace(0, Tf, 10)
x, u = traj.eval(T)
# Recompute using response()
response = traj.response(T, squeeze=False)
np.testing.assert_equal(T, response.time)
np.testing.assert_equal(u, response.inputs)
np.testing.assert_equal(x, response.states)
def test_double_integrator(self, xf, uf, Tf):
# Define a second order integrator
sys = ct.StateSpace([[-1, 1], [0, -2]], [[0], [1]], [[1, 0]], 0)
flatsys = fs.LinearFlatSystem(sys)
# Define the basis set
poly = fs.PolyFamily(6)
x1, u1, = [0, 0], [0]
traj = fs.point_to_point(flatsys, Tf, x1, u1, xf, uf, basis=poly)
# Verify that the trajectory computation is correct
x, u = traj.eval([0, Tf])
np.testing.assert_array_almost_equal(x1, x[:, 0])
np.testing.assert_array_almost_equal(u1, 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, 100)
xd, ud = traj.eval(T)
t, y, x = ct.forced_response(sys, T, ud, x1, return_x=True)
np.testing.assert_array_almost_equal(x, xd, decimal=3)
def test_point_to_point_errors(self):
"""Test error and warning conditions in point_to_point()"""
# Double integrator system
sys = ct.ss([[0, 1], [0, 0]], [[0], [1]], [[1, 0]], 0)
flat_sys = fs.LinearFlatSystem(sys)
# Define the endpoints of the trajectory
x0 = [1, 0]
u0 = [0]
xf = [0, 0]
uf = [0]
Tf = 10
T = np.linspace(0, Tf, 500)
# Cost function
timepts = np.linspace(0, Tf, 10)
cost_fcn = opt.quadratic_cost(flat_sys,
np.diag([1, 1]),
1,
x0=xf,
u0=uf)
# Solving without basis specified should be OK
traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf, uf)
x, u = traj.eval(timepts)
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])
# Adding a cost function generates a warning
with pytest.warns(UserWarning, match="optimization not possible"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn)
# Make sure we still solved the problem
x, u = traj.eval(timepts)
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])
# Try to optimize with insufficient degrees of freedom
with pytest.warns(UserWarning, match="optimization not possible"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
basis=fs.PolyFamily(6))
# Make sure we still solved the problem
x, u = traj.eval(timepts)
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])
# Solve with the errors in the various input arguments
with pytest.raises(ValueError, match="Initial state: Wrong shape"):
traj = fs.point_to_point(flat_sys, timepts, np.zeros(3), u0, xf,
uf)
with pytest.raises(ValueError, match="Initial input: Wrong shape"):
traj = fs.point_to_point(flat_sys, timepts, x0, np.zeros(3), xf,
uf)
with pytest.raises(ValueError, match="Final state: Wrong shape"):
traj = fs.point_to_point(flat_sys, timepts, x0, u0, np.zeros(3),
uf)
with pytest.raises(ValueError, match="Final input: Wrong shape"):
traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf,
np.zeros(3))
# Different ways of describing constraints
constraint = opt.input_range_constraint(flat_sys, -100, 100)
with pytest.warns(UserWarning, match="optimization not possible"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
constraints=constraint,
basis=fs.PolyFamily(6))
x, u = traj.eval(timepts)
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])
# Constraint that isn't a constraint
with pytest.raises(TypeError, match="must be a list"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
constraints=np.eye(2),
basis=fs.PolyFamily(8))
# Unknown constraint type
with pytest.raises(TypeError, match="unknown constraint type"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
constraints=[(None, 0, 0, 0)],
basis=fs.PolyFamily(8))
# Unsolvable optimization
constraint = [opt.input_range_constraint(flat_sys, -0.01, 0.01)]
with pytest.raises(RuntimeError, match="Unable to solve optimal"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
constraints=constraint,
basis=fs.PolyFamily(8))
# Method arguments, parameters
traj_method = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
basis=fs.PolyFamily(8),
minimize_method='slsqp')
traj_kwarg = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
basis=fs.PolyFamily(8),
minimize_kwargs={'method': 'slsqp'})
np.testing.assert_allclose(traj_method.eval(timepts)[0],
traj_kwarg.eval(timepts)[0],
atol=1e-5)
# Unrecognized keywords
with pytest.raises(TypeError, match="unrecognized keyword"):
traj_method = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
solve_ivp_method=None)
def test_flat_cost_constr(self):
# Double integrator system
sys = ct.ss([[0, 1], [0, 0]], [[0], [1]], [[1, 0]], 0)
flat_sys = fs.LinearFlatSystem(sys)
# Define the endpoints of the trajectory
x0 = [1, 0]
u0 = [0]
xf = [0, 0]
uf = [0]
Tf = 10
T = np.linspace(0, Tf, 500)
# Find trajectory between initial and final conditions
traj = fs.point_to_point(flat_sys,
Tf,
x0,
u0,
xf,
uf,
basis=fs.PolyFamily(8))
x, u = traj.eval(T)
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])
# Solve with a cost function
timepts = np.linspace(0, Tf, 10)
cost_fcn = opt.quadratic_cost(flat_sys,
np.diag([0, 0]),
1,
x0=xf,
u0=uf)
traj_cost = fs.point_to_point(
flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
basis=fs.PolyFamily(8),
# initial_guess='lstsq',
# minimize_kwargs={'method': 'trust-constr'}
)
# Verify that the trajectory computation is correct
x_cost, u_cost = traj_cost.eval(T)
np.testing.assert_array_almost_equal(x0, x_cost[:, 0])
np.testing.assert_array_almost_equal(u0, u_cost[:, 0])
np.testing.assert_array_almost_equal(xf, x_cost[:, -1])
np.testing.assert_array_almost_equal(uf, u_cost[:, -1])
# Make sure that we got a different answer than before
assert np.any(np.abs(x - x_cost) > 0.1)
# Re-solve with constraint on the y deviation
lb, ub = [-2, -0.1], [2, 0]
lb, ub = [-2, np.min(x_cost[1]) * 0.95], [2, 1]
constraints = [opt.state_range_constraint(flat_sys, lb, ub)]
# Make sure that the previous solution violated at least one constraint
assert np.any(x_cost[0, :] < lb[0]) or np.any(x_cost[0, :] > ub[0]) \
or np.any(x_cost[1, :] < lb[1]) or np.any(x_cost[1, :] > ub[1])
traj_const = fs.point_to_point(
flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
constraints=constraints,
basis=fs.PolyFamily(8),
)
# Verify that the trajectory computation is correct
x_const, u_const = traj_const.eval(T)
np.testing.assert_array_almost_equal(x0, x_const[:, 0])
np.testing.assert_array_almost_equal(u0, u_const[:, 0])
np.testing.assert_array_almost_equal(xf, x_const[:, -1])
np.testing.assert_array_almost_equal(uf, u_const[:, -1])
# Make sure that the solution respects the bounds (with some slop)
for i in range(x_const.shape[0]):
assert np.all(x_const[i] >= lb[i] * 1.02)
assert np.all(x_const[i] <= ub[i] * 1.02)
# Solve the same problem with a nonlinear constraint type
nl_constraints = [(sp.optimize.NonlinearConstraint, lambda x, u: x, lb,
ub)]
traj_nlconst = fs.point_to_point(
flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
constraints=nl_constraints,
basis=fs.PolyFamily(8),
)
x_nlconst, u_nlconst = traj_nlconst.eval(T)
np.testing.assert_almost_equal(x_const, x_nlconst)
np.testing.assert_almost_equal(u_const, u_nlconst)
