___________________ test_optimal_doc[shooting-3-None-xfail] ____________________

method = 'shooting', npts = 3, initial_guess = None, fail = 'xfail'

    @pytest.mark.slow
    @pytest.mark.parametrize(
        "method, npts, initial_guess, fail", [
            ('shooting', 3, None, 'xfail'),         # doesn't converge
            ('shooting', 3, 'zero', 'xfail'),       # doesn't converge
            # ('shooting', 3, 'u0', None),          # github issue #782
            ('shooting', 3, 'input', 'endpoint'),   # doesn't converge to optimal
            ('shooting', 5, 'input', 'endpoint'),   # doesn't converge to optimal
            ('collocation', 3, 'u0', 'endpoint'),   # doesn't converge to optimal
            ('collocation', 5, 'u0', 'endpoint'),
            ('collocation', 5, 'input', 'openloop'),# open loop sim fails
            ('collocation', 10, 'input', None),
            ('collocation', 10, 'u0', None),        # from documentation
            ('collocation', 10, 'state', None),
            ('collocation', 20, 'state', None),
        ])
    def test_optimal_doc(method, npts, initial_guess, fail):
        """Test optimal control problem from documentation"""
        def vehicle_update(t, x, u, params):
            # Get the parameters for the model
            l = params.get('wheelbase', 3.)         # vehicle wheelbase
            phimax = params.get('maxsteer', 0.5)    # max steering angle (rad)
    
            # Saturate the steering input
            phi = np.clip(u[1], -phimax, phimax)
    
            # Return the derivative of the state
            return np.array([
                np.cos(x[2]) * u[0],            # xdot = cos(theta) v
                np.sin(x[2]) * u[0],            # ydot = sin(theta) v
                (u[0] / l) * np.tan(phi)        # thdot = v/l tan(phi)
            ])
    
        def vehicle_output(t, x, u, params):
            return x                            # return x, y, theta (full state)
    
        # Define the vehicle steering dynamics as an input/output system
        vehicle = ct.NonlinearIOSystem(
            vehicle_update, vehicle_output, states=3, name='vehicle',
            inputs=('v', 'phi'), outputs=('x', 'y', 'theta'))
    
        # Define the initial and final points and time interval
        x0 = np.array([0., -2., 0.]); u0 = np.array([10., 0.])
        xf = np.array([100., 2., 0.]); uf = np.array([10., 0.])
        Tf = 10
    
        # Define the cost functions
        Q = np.diag([0, 0, 0.1])          # don't turn too sharply
        R = np.diag([1, 1])               # keep inputs small
        P = np.diag([1000, 1000, 1000])   # get close to final point
        traj_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
        term_cost = opt.quadratic_cost(vehicle, P, 0, x0=xf)
    
        # Define the constraints
        constraints = [ opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
    
        # Define an initial guess at the trajectory
        timepts = np.linspace(0, Tf, npts, endpoint=True)
        if initial_guess == 'zero':
            initial_guess = 0
    
        elif initial_guess == 'u0':
            initial_guess = u0
    
        elif initial_guess == 'input':
            # Velocity = constant that gets us from start to end
            initial_guess = np.zeros((vehicle.ninputs, timepts.size))
            initial_guess[0, :] = (xf[0] - x0[0]) / Tf
    
            # Steering = rate required to turn to proper slope in first segment
            approximate_angle = math.atan2(xf[1] - x0[1], xf[0] - x0[0])
            initial_guess[1, 0] = approximate_angle / (timepts[1] - timepts[0])
            initial_guess[1, -1] = -approximate_angle / (timepts[-1] - timepts[-2])
    
        elif initial_guess == 'state':
            input_guess = np.outer(u0, np.ones((1, npts)))
            state_guess = np.array([
                x0 + (xf - x0) * time/Tf for time in timepts]).transpose()
            initial_guess = (state_guess, input_guess)
    
        # Solve the optimal control problem
        with warnings.catch_warnings():
            warnings.filterwarnings(
                'ignore', message="unable to solve", category=UserWarning)
            result = opt.solve_optimal_trajectory(
                vehicle, timepts, x0, traj_cost, constraints,
                terminal_cost=term_cost, initial_guess=initial_guess,
                trajectory_method=method,
                # minimize_method='COBYLA', # SLSQP',
            )
    
        if fail == 'xfail':
>           assert not result.success
E           assert not True
E            +  where True =      message: Optimization terminated successfully\n     success: True\n      status: 0\n         fun: 2.4136235872518714\n           x: [ 9.480e+00  1.025e+01  1.006e+01  3.297e-03  2.840e-03\n               -4.371e-03]\n         nit: 75\n         jac: [-2.905e+01 -5.013e+01 -2.577e+01  1.936e+07 -1.171e+03\n                1.935e+07]\n        nfev: 678\n        njev: 74\n multipliers: [ 0.000e+00  0.000e+00  0.000e+00  0.000e+00  0.000e+00\n                0.000e+00  0.000e+00  0.000e+00  0.000e+00  0.000e+00\n                0.000e+00  0.000e+00]\n     problem: <control.optimal.OptimalControlProblem object at 0x7f4fb7b074d0>\n        cost: 2.4136235872518714\n        time: [ 0.000e+00  5.000e+00  1.000e+01]\n      inputs: [[ 9.480e+00  1.025e+01  1.006e+01]\n               [ 3.297e-03  2.840e-03 -4.371e-03]]\n      states: [[ 0.000e+00  4.929e+01  9.999e+01]\n               [-2.000e+00 -7.256e-01  1.998e+00]\n               [ 0.000e+00  5.001e-02  3.711e-02]].success

control/tests/optimal_test.py:756: AssertionError
----------------------------- Captured stdout call -----------------------------
Summary statistics:
* Cost function calls: 678
* Constraint calls: 760
* System simulations: 1204
* Final cost: 2.4136235872518714
___________________ test_optimal_doc[shooting-3-zero-xfail] ____________________

method = 'shooting', npts = 3, initial_guess = 0, fail = 'xfail'

    @pytest.mark.slow
    @pytest.mark.parametrize(
        "method, npts, initial_guess, fail", [
            ('shooting', 3, None, 'xfail'),         # doesn't converge
            ('shooting', 3, 'zero', 'xfail'),       # doesn't converge
            # ('shooting', 3, 'u0', None),          # github issue #782
            ('shooting', 3, 'input', 'endpoint'),   # doesn't converge to optimal
            ('shooting', 5, 'input', 'endpoint'),   # doesn't converge to optimal
            ('collocation', 3, 'u0', 'endpoint'),   # doesn't converge to optimal
            ('collocation', 5, 'u0', 'endpoint'),
            ('collocation', 5, 'input', 'openloop'),# open loop sim fails
            ('collocation', 10, 'input', None),
            ('collocation', 10, 'u0', None),        # from documentation
            ('collocation', 10, 'state', None),
            ('collocation', 20, 'state', None),
        ])
    def test_optimal_doc(method, npts, initial_guess, fail):
        """Test optimal control problem from documentation"""
        def vehicle_update(t, x, u, params):
            # Get the parameters for the model
            l = params.get('wheelbase', 3.)         # vehicle wheelbase
            phimax = params.get('maxsteer', 0.5)    # max steering angle (rad)
    
            # Saturate the steering input
            phi = np.clip(u[1], -phimax, phimax)
    
            # Return the derivative of the state
            return np.array([
                np.cos(x[2]) * u[0],            # xdot = cos(theta) v
                np.sin(x[2]) * u[0],            # ydot = sin(theta) v
                (u[0] / l) * np.tan(phi)        # thdot = v/l tan(phi)
            ])
    
        def vehicle_output(t, x, u, params):
            return x                            # return x, y, theta (full state)
    
        # Define the vehicle steering dynamics as an input/output system
        vehicle = ct.NonlinearIOSystem(
            vehicle_update, vehicle_output, states=3, name='vehicle',
            inputs=('v', 'phi'), outputs=('x', 'y', 'theta'))
    
        # Define the initial and final points and time interval
        x0 = np.array([0., -2., 0.]); u0 = np.array([10., 0.])
        xf = np.array([100., 2., 0.]); uf = np.array([10., 0.])
        Tf = 10
    
        # Define the cost functions
        Q = np.diag([0, 0, 0.1])          # don't turn too sharply
        R = np.diag([1, 1])               # keep inputs small
        P = np.diag([1000, 1000, 1000])   # get close to final point
        traj_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
        term_cost = opt.quadratic_cost(vehicle, P, 0, x0=xf)
    
        # Define the constraints
        constraints = [ opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
    
        # Define an initial guess at the trajectory
        timepts = np.linspace(0, Tf, npts, endpoint=True)
        if initial_guess == 'zero':
            initial_guess = 0
    
        elif initial_guess == 'u0':
            initial_guess = u0
    
        elif initial_guess == 'input':
            # Velocity = constant that gets us from start to end
            initial_guess = np.zeros((vehicle.ninputs, timepts.size))
            initial_guess[0, :] = (xf[0] - x0[0]) / Tf
    
            # Steering = rate required to turn to proper slope in first segment
            approximate_angle = math.atan2(xf[1] - x0[1], xf[0] - x0[0])
            initial_guess[1, 0] = approximate_angle / (timepts[1] - timepts[0])
            initial_guess[1, -1] = -approximate_angle / (timepts[-1] - timepts[-2])
    
        elif initial_guess == 'state':
            input_guess = np.outer(u0, np.ones((1, npts)))
            state_guess = np.array([
                x0 + (xf - x0) * time/Tf for time in timepts]).transpose()
            initial_guess = (state_guess, input_guess)
    
        # Solve the optimal control problem
        with warnings.catch_warnings():
            warnings.filterwarnings(
                'ignore', message="unable to solve", category=UserWarning)
            result = opt.solve_optimal_trajectory(
                vehicle, timepts, x0, traj_cost, constraints,
                terminal_cost=term_cost, initial_guess=initial_guess,
                trajectory_method=method,
                # minimize_method='COBYLA', # SLSQP',
            )
    
        if fail == 'xfail':
>           assert not result.success
E           assert not True
E            +  where True =      message: Optimization terminated successfully\n     success: True\n      status: 0\n         fun: 2.4136235872518714\n           x: [ 9.480e+00  1.025e+01  1.006e+01  3.297e-03  2.840e-03\n               -4.371e-03]\n         nit: 75\n         jac: [-2.905e+01 -5.013e+01 -2.577e+01  1.936e+07 -1.171e+03\n                1.935e+07]\n        nfev: 678\n        njev: 74\n multipliers: [ 0.000e+00  0.000e+00  0.000e+00  0.000e+00  0.000e+00\n                0.000e+00  0.000e+00  0.000e+00  0.000e+00  0.000e+00\n                0.000e+00  0.000e+00]\n     problem: <control.optimal.OptimalControlProblem object at 0x7f4fb7b07750>\n        cost: 2.4136235872518714\n        time: [ 0.000e+00  5.000e+00  1.000e+01]\n      inputs: [[ 9.480e+00  1.025e+01  1.006e+01]\n               [ 3.297e-03  2.840e-03 -4.371e-03]]\n      states: [[ 0.000e+00  4.929e+01  9.999e+01]\n               [-2.000e+00 -7.256e-01  1.998e+00]\n               [ 0.000e+00  5.001e-02  3.711e-02]].success

control/tests/optimal_test.py:756: AssertionError
----------------------------- Captured stdout call -----------------------------
Summary statistics:
* Cost function calls: 678
* Constraint calls: 760
* System simulations: 1204
* Final cost: 2.4136235872518714