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.8525337Z2022-12-21T09:54:30.8525622Zself=<control.tests.flatsys_test.TestFlatSysobjectat0x7ffa1993a430>2022-12-21T09:54:30.8526148Zvehicle_flat=<FlatSystem:sys[3472]:['v','delta']->['x','y','theta']>2022-12-21T09:54:30.8526560Zbasis=<BSplineFamily:nvars=None,degree=[8],smoothness=[7]>,guess=None2022-12-21T09:54:30.8527014Zconstraints= ([8,-0.1], [12,0.1]),method=None2022-12-21T09:54:30.8527252Z2022-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.8531750Zdeftest_kinematic_car_ocp(2022-12-21T09:54:30.8532108Zself,vehicle_flat,basis,guess,constraints,method):2022-12-21T09:54:30.8532410Z2022-12-21T09:54:30.8532709Z# Define the endpoints of the trajectory2022-12-21T09:54:30.8533084Zx0= [0.,-2.,0.];u0= [10.,0.]2022-12-21T09:54:30.8533434Zxf= [40.,2.,0.];uf= [10.,0.]2022-12-21T09:54:30.8534029ZTf=42022-12-21T09:54:30.8534671Ztimepts=np.linspace(0,Tf,10)2022-12-21T09:54:30.8535730Z2022-12-21T09:54:30.8535966Z# Find trajectory between initial and final conditions2022-12-21T09:54:30.8536225Ztraj_p2p=fs.point_to_point(2022-12-21T09:54:30.8536495Zvehicle_flat,Tf,x0,u0,xf,uf,basis=basis)2022-12-21T09:54:30.8536704Z2022-12-21T09:54:30.8537049Z# Verify that the trajectory computation is correct2022-12-21T09:54:30.8537317Zx,u=traj_p2p.eval(timepts)2022-12-21T09:54:30.8537589Znp.testing.assert_array_almost_equal(x0,x[:,0])2022-12-21T09:54:30.8537872Znp.testing.assert_array_almost_equal(u0,u[:,0])2022-12-21T09:54:30.8538226Znp.testing.assert_array_almost_equal(xf,x[:,-1])2022-12-21T09:54:30.8538573Znp.testing.assert_array_almost_equal(uf,u[:,-1])2022-12-21T09:54:30.8538780Z2022-12-21T09:54:30.8538940Z#2022-12-21T09:54:30.8539210Z# Re-solve as optimal control problem2022-12-21T09:54:30.8539414Z#2022-12-21T09:54:30.8539569Z2022-12-21T09:54:30.8539811Z# Define the cost function (mainly penalize steering angle)2022-12-21T09:54:30.8540092Ztraj_cost=opt.quadratic_cost(2022-12-21T09:54:30.8540358Zvehicle_flat,None,np.diag([0.1,10]),x0=xf,u0=uf)2022-12-21T09:54:30.8540576Z2022-12-21T09:54:30.8540794Z# Set terminal cost to bring us close to xf2022-12-21T09:54:30.8541051Zterminal_cost=opt.quadratic_cost(2022-12-21T09:54:30.8541320Zvehicle_flat,1e3*np.eye(3),None,x0=xf)2022-12-21T09:54:30.8541535Z2022-12-21T09:54:30.8541749Z# Implement terminal constraints if specified2022-12-21T09:54:30.8541990Zifconstraints:2022-12-21T09:54:30.8542241Zinput_constraints=opt.input_range_constraint(2022-12-21T09:54:30.8542498Zvehicle_flat,*constraints)2022-12-21T09:54:30.8542706Zelse:2022-12-21T09:54:30.8542905Zinput_constraints=None2022-12-21T09:54:30.8543093Z2022-12-21T09:54:30.8543331Z# Use a straight line as an initial guess for the trajectory2022-12-21T09:54:30.8543624Zifguess=='prev':2022-12-21T09:54:30.8543934Zinitial_guess=traj_p2p.eval(timepts)[0][0:2]2022-12-21T09:54:30.8544221Zelifguess=='linear':2022-12-21T09:54:30.8544450Zinitial_guess=np.array(2022-12-21T09:54:30.8544774Z                 [x0[i]+ (xf[i]-x0[i])*timepts/Tfforiin (0,1)])2022-12-21T09:54:30.8545004Zelse:2022-12-21T09:54:30.8545194Zinitial_guess=None2022-12-21T09:54:30.8545381Z2022-12-21T09:54:30.8545564Z# Solve the optimal trajectory2022-12-21T09:54:30.8545801Ztraj_ocp=fs.solve_flat_ocp(2022-12-21T09:54:30.8546040Zvehicle_flat,timepts,x0,u0,2022-12-21T09:54:30.8546301Zcost=traj_cost,constraints=input_constraints,2022-12-21T09:54:30.8546584Zterminal_cost=terminal_cost,basis=basis,2022-12-21T09:54:30.8546840Zinitial_guess=initial_guess,2022-12-21T09:54:30.8547137Zminimize_kwargs={'method':method},2022-12-21T09:54:30.8547353Z         )2022-12-21T09:54:30.8547556Zxd,ud=traj_ocp.eval(timepts)2022-12-21T09:54:30.8547779Zifnottraj_ocp.success:2022-12-21T09:54:30.8548067Z# If unsuccessful, make sure the error is just about precision2022-12-21T09:54:30.8548428Z>assertre.match(".*precision loss.*",traj_ocp.message)isnotNone2022-12-21T09:54:30.8548730ZEAssertionError:assertNoneisnotNone2022-12-21T09:54:30.8549178ZE+whereNone=<functionmatchat0x7ffa22446dc0>('.*precision loss.*','Iteration limit reached')2022-12-21T09:54:30.8549525ZE+where<functionmatchat0x7ffa22446dc0>=re.match2022-12-21T09:54:30.8550032ZE+and'Iteration limit reached'=<control.flatsys.systraj.SystemTrajectoryobjectat0x7ffa1894ee80>.message2022-12-21T09:54:30.8550299Z2022-12-21T09:54:30.8550420Zcontrol/tests/flatsys_test.py:206:AssertionError