test_optimal: More failures on exotic platforms #838

[  102s] =================================== FAILURES ===================================
[  102s] _____________________ test_optimal_doc[shooting-3-u0-None] _____________________
[  102s] 
[  102s] method = 'shooting', npts = 3, initial_guess = array([10.,  0.]), fail = None
[  102s] 
[  102s]     @pytest.mark.parametrize(
[  102s]         "method, npts, initial_guess, fail", [
[  102s]             ('shooting', 3, None, 'xfail'),         # doesn't converge
[  102s]             ('shooting', 3, 'zero', 'xfail'),       # doesn't converge
[  102s]             ('shooting', 3, 'u0', None),            # github issue #782
[  102s]             ('shooting', 3, 'input', 'endpoint'),   # doesn't converge to optimal
[  102s]             ('shooting', 5, 'input', 'endpoint'),   # doesn't converge to optimal
[  102s]             ('collocation', 3, 'u0', 'endpoint'),   # doesn't converge to optimal
[  102s]             ('collocation', 5, 'u0', 'endpoint'),
[  102s]             ('collocation', 5, 'input', 'openloop'),# open loop sim fails
[  102s]             ('collocation', 10, 'input', None),
[  102s]             ('collocation', 10, 'u0', None),        # from documentation
[  102s]             ('collocation', 10, 'state', None),
[  102s]             ('collocation', 20, 'state', None),
[  102s]         ])
[  102s]     def test_optimal_doc(method, npts, initial_guess, fail):
[  102s]         """Test optimal control problem from documentation"""
[  102s]         def vehicle_update(t, x, u, params):
[  102s]             # Get the parameters for the model
[  102s]             l = params.get('wheelbase', 3.)         # vehicle wheelbase
[  102s]             phimax = params.get('maxsteer', 0.5)    # max steering angle (rad)
[  102s]     
[  102s]             # Saturate the steering input
[  102s]             phi = np.clip(u[1], -phimax, phimax)
[  102s]     
[  102s]             # Return the derivative of the state
[  102s]             return np.array([
[  102s]                 np.cos(x[2]) * u[0],            # xdot = cos(theta) v
[  102s]                 np.sin(x[2]) * u[0],            # ydot = sin(theta) v
[  102s]                 (u[0] / l) * np.tan(phi)        # thdot = v/l tan(phi)
[  102s]             ])
[  102s]     
[  102s]         def vehicle_output(t, x, u, params):
[  102s]             return x                            # return x, y, theta (full state)
[  102s]     
[  102s]         # Define the vehicle steering dynamics as an input/output system
[  102s]         vehicle = ct.NonlinearIOSystem(
[  102s]             vehicle_update, vehicle_output, states=3, name='vehicle',
[  102s]             inputs=('v', 'phi'), outputs=('x', 'y', 'theta'))
[  102s]     
[  102s]         # Define the initial and final points and time interval
[  102s]         x0 = np.array([0., -2., 0.]); u0 = np.array([10., 0.])
[  102s]         xf = np.array([100., 2., 0.]); uf = np.array([10., 0.])
[  102s]         Tf = 10
[  102s]     
[  102s]         # Define the cost functions
[  102s]         Q = np.diag([0, 0, 0.1])          # don't turn too sharply
[  102s]         R = np.diag([1, 1])               # keep inputs small
[  102s]         P = np.diag([1000, 1000, 1000])   # get close to final point
[  102s]         traj_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
[  102s]         term_cost = opt.quadratic_cost(vehicle, P, 0, x0=xf)
[  102s]     
[  102s]         # Define the constraints
[  102s]         constraints = [ opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
[  102s]     
[  102s]         # Define an initial guess at the trajectory
[  102s]         timepts = np.linspace(0, Tf, npts, endpoint=True)
[  102s]         if initial_guess == 'zero':
[  102s]             initial_guess = 0
[  102s]     
[  102s]         elif initial_guess == 'u0':
[  102s]             initial_guess = u0
[  102s]     
[  102s]         elif initial_guess == 'input':
[  102s]             # Velocity = constant that gets us from start to end
[  102s]             initial_guess = np.zeros((vehicle.ninputs, timepts.size))
[  102s]             initial_guess[0, :] = (xf[0] - x0[0]) / Tf
[  102s]     
[  102s]             # Steering = rate required to turn to proper slope in first segment
[  102s]             straight_seg_length = timepts[-2] - timepts[1]
[  102s]             curved_seg_length = (Tf - straight_seg_length)/2
[  102s]             approximate_angle = math.atan2(xf[1] - x0[1], xf[0] - x0[0])
[  102s]             initial_guess[1, 0] = approximate_angle / (timepts[1] - timepts[0])
[  102s]             initial_guess[1, -1] = -approximate_angle / (timepts[-1] - timepts[-2])
[  102s]     
[  102s]         elif initial_guess == 'state':
[  102s]             input_guess = np.outer(u0, np.ones((1, npts)))
[  102s]             state_guess = np.array([
[  102s]                 x0 + (xf - x0) * time/Tf for time in timepts]).transpose()
[  102s]             initial_guess = (state_guess, input_guess)
[  102s]     
[  102s]         # Solve the optimal control problem
[  102s]         result = opt.solve_ocp(
[  102s]             vehicle, timepts, x0, traj_cost, constraints,
[  102s]             terminal_cost=term_cost, initial_guess=initial_guess,
[  102s]             trajectory_method=method,
[  102s]             # minimize_method='COBYLA', # SLSQP',
[  102s]         )
[  102s]     
[  102s]         if fail == 'xfail':
[  102s]             assert not result.success
[  102s]             pytest.xfail("optimization fails to converge")
[  102s]         elif fail == 'precision':
[  102s]             assert result.status == 2
[  102s]             pytest.xfail("optimization precision not achieved")
[  102s]         else:
[  102s]             # Make sure the optimization was successful
[  102s]             assert result.success
[  102s]     
[  102s]             # Make sure we started and stopped at the right spot
[  102s]             if fail == 'endpoint':
[  102s]                 assert not np.allclose(result.states[:, -1], xf, rtol=1e-4)
[  102s]                 pytest.xfail("optimization does not converge to endpoint")
[  102s]             else:
[  102s]                 np.testing.assert_almost_equal(result.states[:, 0], x0, decimal=4)
[  102s] >               np.testing.assert_almost_equal(result.states[:, -1], xf, decimal=2)
[  102s] 
[  102s] control/tests/optimal_test.py:723: 
[  102s] _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
[  102s] /usr/lib64/python3.8/contextlib.py:75: in inner
[  102s]     return func(*args, **kwds)
[  102s] /usr/lib64/python3.8/contextlib.py:75: in inner
[  102s]     return func(*args, **kwds)
[  102s] _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
[  102s] 
[  102s] args = (<function assert_array_almost_equal.<locals>.compare at 0x3ff61d50430>, array([100.,  -2.,   0.]), array([100.,   2.,   0.]))
[  102s] kwds = {'err_msg': '', 'header': 'Arrays are not almost equal to 2 decimals', 'precision': 2, 'verbose': True}
[  102s] 
[  102s]     @wraps(func)
[  102s]     def inner(*args, **kwds):
[  102s]         with self._recreate_cm():
[  102s] >           return func(*args, **kwds)
[  102s] E           AssertionError: 
[  102s] E           Arrays are not almost equal to 2 decimals
[  102s] E           
[  102s] E           Mismatched elements: 1 / 3 (33.3%)
[  102s] E           Max absolute difference: 4.
[  102s] E           Max relative difference: 2.
[  102s] E            x: array([100.,  -2.,   0.])
[  102s] E            y: array([100.,   2.,   0.])
[  102s] 
[  102s] /usr/lib64/python3.8/contextlib.py:75: AssertionError
[  102s] ----------------------------- Captured stdout call -----------------------------
[  102s] Summary statistics:
[  102s] * Cost function calls: 7
[  102s] * Constraint calls: 16
[  102s] * System simulations: 22
[  102s] * Final cost: 16000.0