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)