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: AssertionError