Closed
Description
From the Slycot CI on a few runs of the test matrix, but not all:
2022-12-21T09:54:30.8524100Z =================================== FAILURES ===================================
2022-12-21T09:54:30.8524781Z ______ TestFlatSys.test_kinematic_car_ocp[basis6-None-constraints6-None] _______
2022-12-21T09:54:30.8525337Z
2022-12-21T09:54:30.8525622Z self = <control.tests.flatsys_test.TestFlatSys object at 0x7ffa1993a430>
2022-12-21T09:54:30.8526148Z vehicle_flat = <FlatSystem:sys[3472]:['v', 'delta']->['x', 'y', 'theta']>
2022-12-21T09:54:30.8526560Z basis = <BSplineFamily: nvars=None, degree=[8], smoothness=[7]>, guess = None
2022-12-21T09:54:30.8527014Z constraints = ([8, -0.1], [12, 0.1]), method = None
2022-12-21T09:54:30.8527252Z
2022-12-21T09:54:30.8527434Z @pytest.mark.parametrize(
2022-12-21T09:54:30.8527773Z "basis, guess, constraints, method", [
2022-12-21T09:54:30.8528179Z (fs.PolyFamily(8, T=10), 'prev', None, None),
2022-12-21T09:54:30.8528603Z (fs.BezierFamily(8, T=10), 'linear', None, None),
2022-12-21T09:54:30.8528964Z (fs.BSplineFamily([0, 10], 8), None, None, None),
2022-12-21T09:54:30.8529417Z (fs.BSplineFamily([0, 10], 8), 'prev', None, 'trust-constr'),
2022-12-21T09:54:30.8529871Z (fs.BSplineFamily([0, 10], [6, 8], vars=2), 'prev', None, None),
2022-12-21T09:54:30.8530326Z (fs.BSplineFamily([0, 5, 10], 5), 'linear', None, 'slsqp'),
2022-12-21T09:54:30.8530807Z (fs.BSplineFamily([0, 10], 8), None, ([8, -0.1], [12, 0.1]), None),
2022-12-21T09:54:30.8531174Z (fs.BSplineFamily([0, 5, 10], 5, 3), None, None, None),
2022-12-21T09:54:30.8531474Z ])
2022-12-21T09:54:30.8531750Z def test_kinematic_car_ocp(
2022-12-21T09:54:30.8532108Z self, vehicle_flat, basis, guess, constraints, method):
2022-12-21T09:54:30.8532410Z
2022-12-21T09:54:30.8532709Z # Define the endpoints of the trajectory
2022-12-21T09:54:30.8533084Z x0 = [0., -2., 0.]; u0 = [10., 0.]
2022-12-21T09:54:30.8533434Z xf = [40., 2., 0.]; uf = [10., 0.]
2022-12-21T09:54:30.8534029Z Tf = 4
2022-12-21T09:54:30.8534671Z timepts = np.linspace(0, Tf, 10)
2022-12-21T09:54:30.8535730Z
2022-12-21T09:54:30.8535966Z # Find trajectory between initial and final conditions
2022-12-21T09:54:30.8536225Z traj_p2p = fs.point_to_point(
2022-12-21T09:54:30.8536495Z vehicle_flat, Tf, x0, u0, xf, uf, basis=basis)
2022-12-21T09:54:30.8536704Z
2022-12-21T09:54:30.8537049Z # Verify that the trajectory computation is correct
2022-12-21T09:54:30.8537317Z x, u = traj_p2p.eval(timepts)
2022-12-21T09:54:30.8537589Z np.testing.assert_array_almost_equal(x0, x[:, 0])
2022-12-21T09:54:30.8537872Z np.testing.assert_array_almost_equal(u0, u[:, 0])
2022-12-21T09:54:30.8538226Z np.testing.assert_array_almost_equal(xf, x[:, -1])
2022-12-21T09:54:30.8538573Z np.testing.assert_array_almost_equal(uf, u[:, -1])
2022-12-21T09:54:30.8538780Z
2022-12-21T09:54:30.8538940Z #
2022-12-21T09:54:30.8539210Z # Re-solve as optimal control problem
2022-12-21T09:54:30.8539414Z #
2022-12-21T09:54:30.8539569Z
2022-12-21T09:54:30.8539811Z # Define the cost function (mainly penalize steering angle)
2022-12-21T09:54:30.8540092Z traj_cost = opt.quadratic_cost(
2022-12-21T09:54:30.8540358Z vehicle_flat, None, np.diag([0.1, 10]), x0=xf, u0=uf)
2022-12-21T09:54:30.8540576Z
2022-12-21T09:54:30.8540794Z # Set terminal cost to bring us close to xf
2022-12-21T09:54:30.8541051Z terminal_cost = opt.quadratic_cost(
2022-12-21T09:54:30.8541320Z vehicle_flat, 1e3 * np.eye(3), None, x0=xf)
2022-12-21T09:54:30.8541535Z
2022-12-21T09:54:30.8541749Z # Implement terminal constraints if specified
2022-12-21T09:54:30.8541990Z if constraints:
2022-12-21T09:54:30.8542241Z input_constraints = opt.input_range_constraint(
2022-12-21T09:54:30.8542498Z vehicle_flat, *constraints)
2022-12-21T09:54:30.8542706Z else:
2022-12-21T09:54:30.8542905Z input_constraints = None
2022-12-21T09:54:30.8543093Z
2022-12-21T09:54:30.8543331Z # Use a straight line as an initial guess for the trajectory
2022-12-21T09:54:30.8543624Z if guess == 'prev':
2022-12-21T09:54:30.8543934Z initial_guess = traj_p2p.eval(timepts)[0][0:2]
2022-12-21T09:54:30.8544221Z elif guess == 'linear':
2022-12-21T09:54:30.8544450Z initial_guess = np.array(
2022-12-21T09:54:30.8544774Z [x0[i] + (xf[i] - x0[i]) * timepts/Tf for i in (0, 1)])
2022-12-21T09:54:30.8545004Z else:
2022-12-21T09:54:30.8545194Z initial_guess = None
2022-12-21T09:54:30.8545381Z
2022-12-21T09:54:30.8545564Z # Solve the optimal trajectory
2022-12-21T09:54:30.8545801Z traj_ocp = fs.solve_flat_ocp(
2022-12-21T09:54:30.8546040Z vehicle_flat, timepts, x0, u0,
2022-12-21T09:54:30.8546301Z cost=traj_cost, constraints=input_constraints,
2022-12-21T09:54:30.8546584Z terminal_cost=terminal_cost, basis=basis,
2022-12-21T09:54:30.8546840Z initial_guess=initial_guess,
2022-12-21T09:54:30.8547137Z minimize_kwargs={'method': method},
2022-12-21T09:54:30.8547353Z )
2022-12-21T09:54:30.8547556Z xd, ud = traj_ocp.eval(timepts)
2022-12-21T09:54:30.8547779Z if not traj_ocp.success:
2022-12-21T09:54:30.8548067Z # If unsuccessful, make sure the error is just about precision
2022-12-21T09:54:30.8548428Z > assert re.match(".*precision loss.*", traj_ocp.message) is not None
2022-12-21T09:54:30.8548730Z E AssertionError: assert None is not None
2022-12-21T09:54:30.8549178Z E + where None = <function match at 0x7ffa22446dc0>('.*precision loss.*', 'Iteration limit reached')
2022-12-21T09:54:30.8549525Z E + where <function match at 0x7ffa22446dc0> = re.match
2022-12-21T09:54:30.8550032Z E + and 'Iteration limit reached' = <control.flatsys.systraj.SystemTrajectory object at 0x7ffa1894ee80>.message
2022-12-21T09:54:30.8550299Z
2022-12-21T09:54:30.8550420Z control/tests/flatsys_test.py:206: AssertionErrorMetadata
Metadata
Assignees
Labels
No labels