def test_static_nonlinear_call(satsys): # Make sure that the saturation system is a static nonlinearity assert satsys._isstatic() # Make sure the saturation function is doing the right computation input = [-2, -1, -0.5, 0, 0.5, 1, 2] desired = [-1, -1, -0.5, 0, 0.5, 1, 1] for x, y in zip(input, desired): assert satsys(x) == y # Test squeeze properties assert satsys(0.) == 0. assert satsys([0.], squeeze=True) == 0. np.testing.assert_array_equal(satsys([0.]), [0.]) # Test SIMO nonlinearity def _simofcn(t, x, u, params): return np.array([np.cos(u), np.sin(u)]) simo_sys = ct.NonlinearIOSystem(None, outfcn=_simofcn, input=1, output=2) np.testing.assert_array_equal(simo_sys([0.]), [1, 0]) np.testing.assert_array_equal(simo_sys([0.], squeeze=True), [1, 0]) # Test MISO nonlinearity def _misofcn(t, x, u, params={}): return np.array([np.sin(u[0]) * np.cos(u[1])]) miso_sys = ct.NonlinearIOSystem(None, outfcn=_misofcn, input=2, output=1) np.testing.assert_array_equal(miso_sys([0, 0]), [0]) np.testing.assert_array_equal(miso_sys([0, 0], squeeze=True), [0])
def test_sys_naming_convention(self): """Enforce generic system names 'sys[i]' to be present when systems are created without explicit names.""" ct.InputOutputSystem.idCounter = 0 sys = ct.LinearIOSystem(self.mimo_linsys1) self.assertEquals(sys.name, "sys[0]") self.assertEquals(sys.copy().name, "copy of sys[0]") namedsys = ios.NonlinearIOSystem( updfcn = lambda t, x, u, params: x, outfcn = lambda t, x, u, params: u, inputs = ('u[0]', 'u[1]'), outputs = ('y[0]', 'y[1]'), states = self.mimo_linsys1.states, name = 'namedsys') unnamedsys1 = ct.NonlinearIOSystem( lambda t,x,u,params: x, inputs=2, outputs=2, states=2 ) unnamedsys2 = ct.NonlinearIOSystem( None, lambda t,x,u,params: u, inputs=2, outputs=2 ) self.assertEquals(unnamedsys2.name, "sys[2]") # Unnamed/unnamed connections uu_series = unnamedsys1 * unnamedsys2 uu_parallel = unnamedsys1 + unnamedsys2 u_neg = - unnamedsys1 uu_feedback = unnamedsys2.feedback(unnamedsys1) uu_dup = unnamedsys1 * unnamedsys1.copy() uu_hierarchical = uu_series*unnamedsys1 self.assertEquals(uu_series.name, "sys[3]") self.assertEquals(uu_parallel.name, "sys[4]") self.assertEquals(u_neg.name, "sys[5]") self.assertEquals(uu_feedback.name, "sys[6]") self.assertEquals(uu_dup.name, "sys[7]") self.assertEquals(uu_hierarchical.name, "sys[8]") # Unnamed/named connections un_series = unnamedsys1 * namedsys un_parallel = unnamedsys1 + namedsys un_feedback = unnamedsys2.feedback(namedsys) un_dup = unnamedsys1 * namedsys.copy() un_hierarchical = uu_series*unnamedsys1 self.assertEquals(un_series.name, "sys[9]") self.assertEquals(un_parallel.name, "sys[10]") self.assertEquals(un_feedback.name, "sys[11]") self.assertEquals(un_dup.name, "sys[12]") self.assertEquals(un_hierarchical.name, "sys[13]") # Same system conflict with warnings.catch_warnings(record=True) as warnval: unnamedsys1 * unnamedsys1 self.assertEqual(len(warnval), 1)
def satsys(): satfcn = saturation_class() def _satfcn(t, x, u, params): return satfcn(u) return ct.NonlinearIOSystem(None, outfcn=_satfcn, input=1, output=1)
def __init__(self, T, Ts, l_r=0.5, L=1.0): """Constructor. Parameters ========== T : int Timesteps. Ts : float Step size in seconds. l_r : float Length of hind wheel to bicycle center of gravity. L : float Length of bicycle longitudinal dimension from hind to front wheel. """ self.T = T self.Ts = Ts self.timestamps = np.linspace(0, Ts * T, T + 1) self.l_r = l_r self.L = L self.io_dynamical_system = control.NonlinearIOSystem( bicycle_kinematics, None, inputs=('u_1', 'u_2'), outputs=('x', 'y', 'psi', 'v'), states=('x', 'y', 'psi', 'v'), params={ 'l_r': l_r, 'L': L }, name='bicycle_kinematics')
def test_interconnect_exceptions(): # First make sure the docstring example works P = ct.tf2io(ct.tf(1, [1, 0]), input='u', output='y') C = ct.tf2io(ct.tf(10, [1, 1]), input='e', output='u') sumblk = ct.summing_junction(inputs=['r', '-y'], output='e') T = ct.interconnect((P, C, sumblk), input='r', output='y') assert (T.ninputs, T.noutputs, T.nstates) == (1, 1, 2) # Unrecognized arguments # LinearIOSystem with pytest.raises(TypeError, match="unknown parameter"): P = ct.LinearIOSystem(ct.rss(2, 1, 1), output_name='y') # Interconnect with pytest.raises(TypeError, match="unknown parameter"): T = ct.interconnect((P, C, sumblk), input_name='r', output='y') # Interconnected system with pytest.raises(TypeError, match="unknown parameter"): T = ct.InterconnectedSystem((P, C, sumblk), input_name='r', output='y') # NonlinearIOSytem with pytest.raises(TypeError, match="unknown parameter"): nlios = ct.NonlinearIOSystem( None, lambda t, x, u, params: u*u, input_count=1, output_count=1) # Summing junction with pytest.raises(TypeError, match="input specification is required"): sumblk = ct.summing_junction() with pytest.raises(TypeError, match="unknown parameter"): sumblk = ct.summing_junction(input_count=2, output_count=2)
def test_signals_naming_convention(self): """Enforce generic names to be present when systems are created without explicit signal names: input: 'u[i]' state: 'x[i]' output: 'y[i]' """ ct.InputOutputSystem.idCounter = 0 sys = ct.LinearIOSystem(self.mimo_linsys1) for statename in ["x[0]", "x[1]"]: self.assertTrue(statename in sys.state_index) for inputname in ["u[0]", "u[1]"]: self.assertTrue(inputname in sys.input_index) for outputname in ["y[0]", "y[1]"]: self.assertTrue(outputname in sys.output_index) self.assertEqual(len(sys.state_index), sys.nstates) self.assertEqual(len(sys.input_index), sys.ninputs) self.assertEqual(len(sys.output_index), sys.noutputs) namedsys = ios.NonlinearIOSystem( updfcn = lambda t, x, u, params: x, outfcn = lambda t, x, u, params: u, inputs = ('u0'), outputs = ('y0'), states = ('x0'), name = 'namedsys') unnamedsys = ct.NonlinearIOSystem( lambda t,x,u,params: x, inputs=1, outputs=1, states=1 ) self.assertTrue('u0' in namedsys.input_index) self.assertTrue('y0' in namedsys.output_index) self.assertTrue('x0' in namedsys.state_index) # Unnamed/named connections un_series = unnamedsys * namedsys un_parallel = unnamedsys + namedsys un_feedback = unnamedsys.feedback(namedsys) un_dup = unnamedsys * namedsys.copy() un_hierarchical = un_series*unnamedsys u_neg = - unnamedsys self.assertTrue("sys[1].x[0]" in un_series.state_index) self.assertTrue("namedsys.x0" in un_series.state_index) self.assertTrue("sys[1].x[0]" in un_parallel.state_index) self.assertTrue("namedsys.x0" in un_series.state_index) self.assertTrue("sys[1].x[0]" in un_feedback.state_index) self.assertTrue("namedsys.x0" in un_feedback.state_index) self.assertTrue("sys[1].x[0]" in un_dup.state_index) self.assertTrue("copy of namedsys.x0" in un_dup.state_index) self.assertTrue("sys[1].x[0]" in un_hierarchical.state_index) self.assertTrue("sys[2].sys[1].x[0]" in un_hierarchical.state_index) self.assertTrue("sys[1].x[0]" in u_neg.state_index) # Same system conflict with warnings.catch_warnings(record=True) as warnval: same_name_series = unnamedsys * unnamedsys self.assertEquals(len(warnval), 1) self.assertTrue("sys[1].x[0]" in same_name_series.state_index) self.assertTrue("copy of sys[1].x[0]" in same_name_series.state_index)
def __init__(self, params={}): self.sys = ctrl.NonlinearIOSystem(updfcn=self.pi_update, outfcn=self.pi_split_accel_brake, name='driver', inputs=['v', 'vref'], outputs=['ya'], states=['z'], params=params) self.sys.name = 'driver'
def sys_dict(): sdict = {} sdict['ss'] = ct.ss([[-1]], [[1]], [[1]], [[0]]) sdict['tf'] = ct.tf([1], [0.5, 1]) sdict['tfx'] = ct.tf([1, 1], [1]) # non-proper transfer function sdict['frd'] = ct.frd([10 + 0j, 9 + 1j, 8 + 2j], [1, 2, 3]) sdict['lio'] = ct.LinearIOSystem(ct.ss([[-1]], [[5]], [[5]], [[0]])) sdict['ios'] = ct.NonlinearIOSystem(sdict['lio']._rhs, sdict['lio']._out, 1, 1, 1) sdict['arr'] = np.array([[2.0]]) sdict['flt'] = 3. return sdict
def sys_dict(): sdict = {} sdict['ss'] = ct.StateSpace([[-1]], [[1]], [[1]], [[0]]) sdict['tf'] = ct.TransferFunction([1], [0.5, 1]) sdict['tfx'] = ct.TransferFunction([1, 1], [1]) # non-proper TF sdict['frd'] = ct.frd([10 + 0j, 9 + 1j, 8 + 2j, 7 + 3j], [1, 2, 3, 4]) sdict['lio'] = ct.LinearIOSystem(ct.ss([[-1]], [[5]], [[5]], [[0]])) sdict['ios'] = ct.NonlinearIOSystem(sdict['lio']._rhs, sdict['lio']._out, inputs=1, outputs=1, states=1) sdict['arr'] = np.array([[2.0]]) sdict['flt'] = 3. return sdict
def compute_nonlinear_dynamical_states(initial_state, T, Ts, U, l_r=0.5, L=1.0): """Compute non-linear dynamical states from bicycle kinematic model. Parameters ========== initial_state : ndarray Initial state [x_0, y_0, psi_0, v_0]. T : int Timesteps. Ts : float Step size in seconds. U : ndarray Control inputs of shape (T, 2) [u_1, u_2] l_r : float Length of hind wheel to bicycle center of gravity. L : float Length of bicycle longitudinal dimension from hind to front wheel. Returns ======= ndarray Trajectory from control of shape (T + 1, 4) with rows [x, y, psi, v]. """ timestamps = np.linspace(0, Ts * T, T + 1) mock_inputs = np.concatenate((U, np.array([0, 0])[None]), axis=0).T io_bicycle_kinematics = control.NonlinearIOSystem( bicycle_kinematics, None, inputs=('u_1', 'u_2'), outputs=('x', 'y', 'psi', 'v'), states=('x', 'y', 'psi', 'v'), params={ 'l_r': l_r, 'L': L }, name='bicycle_kinematics') _, states = control.input_output_response(io_bicycle_kinematics, timestamps, mock_inputs, initial_state) return states.T
def compute_nonlinear_dynamical_states(initial_state, T, Ts, U, l_r=0.5, L=1.0): """ Parameters ========== initial_state : ndarray Initial state [x_0, y_0, psi_0, v_0, delta_0]. T : int Timesteps. Ts : float Step size in seconds. U : ndarray Control inputs of shape (T, 2) [u_1, u_2] Returns ======= ndarray Trajectory from control of shape (T + 1, 5) with rows [x, y, psi, v, delta]. """ timestamps = np.linspace(0, Ts * T, T + 1) mock_inputs = np.concatenate((U, np.array([0, 0])[None]), axis=0).T io_bicycle_kinematics = control.NonlinearIOSystem( bicycle_kinematics, None, inputs=('u_1', 'u_2'), outputs=('x', 'y', 'psi', 'v', 'delta'), states=('x', 'y', 'psi', 'v', 'delta'), params={ 'l_r': l_r, 'L': L }, name='bicycle_kinematics') _, states = control.input_output_response(io_bicycle_kinematics, timestamps, mock_inputs, initial_state) return states.T
'L': L, # pendulum length 'g': g, # gravity 'd': d, # dampping term } tspan = [0, 10] # time for simulation dt = 0.04 yInit = [0.0, 0.0, 0.0, 0.1] # initial state xeq = [0.3, 0.0, 0.0, 0.0] # final stable state # Build non-linear standard system sysFullNonLin = control.NonlinearIOSystem(cartpendSys_pyCtl, None, inputs=('u'), outputs=('x', 'xdot', 'theta', 'thetadot'), states=('x', 'xdot', 'theta', 'thetadot'), params=sysParams, name='sysFullNonLin') sysLin = sysFullNonLin.linearize([0.0, 0.0, 0.0, 0.0], 0.0, params=sysParams) # linearize on up position A = sysLin.A B = sysLin.B BF = np.concatenate([B, Vd, 0.0 * B], axis=1) # augment inputs to include disturbance and noise C_full = sysLin.C C_part = np.array([1, 0, 0, 0], dtype=np.float) ssFull = control.StateSpace(A, BF, C_full, np.zeros(
# Algebraic relationships e = x_d - x e_1 = x_d_dot + LAMBDA * e e_2 = e + LAMBDA * e_i # Control law u = j_hat * e_1 + b_hat * x + K * e_2 return [u, x_state[0], x_state[1]] IO_ADAPTIVE_PI = control.NonlinearIOSystem( adaptive_pi_state, adaptive_pi_output, inputs=3, outputs=('u', 'j_hat', 'b_hat'), states=3, name='control', dt=0 ) # Define the closed-loop system # x_cl = [plant.x, control.x[0], control.x[1], control.x[2]] # = [motorx, j_hat, b_hat, e_i] # u_cl = [xd, xddot] # y_cl = [motorx, Jhat, Bhat] IO_CLOSED = control.InterconnectedSystem( (IO_DC_MOTOR, IO_ADAPTIVE_PI), connections=( ('plant.u', 'control.u'), ('control.u[2]', 'plant.x')
# 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')) # # Gain scheduled controller # # For this system we use a simple schedule on the forward vehicle velocity and # place the poles of the system at fixed values. The controller takes the # current vehicle position and orientation plus the velocity velocity as # inputs, and returns the velocity and steering commands. # # System state: none # System input: ex, ey, etheta, vd, phid # System output: v, phi
def __init__(self, state, lane_width, dt=0.1): """ Function to initiate the lateral Model, it will set the parameters. :param state: dict of system state :param lane_width: float, width of initial lane in meters [m] :param dt: timestep increment of the simulation """ # Initial lane position should be lane center self.lane_width = lane_width self.dt = dt # Initial steering angle is 0 self.steering_angle = 0 # In lane position is the distance from the right tangent line of the lane # Initial lane position is middle of the lane self.in_lane_pos = self.lane_width / 2 # Initial ego vehicle state and parameters self.lateral_state = { 'heading': np.radians(state['heading']), 'steering_angle': self.steering_angle, 'x_position': state['x_position'], 'y_position': state['y_position'], 'speed': state['speed'], 'lane_id': state['lane_id'], # Vehicle parameters that are not supposed to update: # Vehicle dimensions for collision detection 'length': state['length'], 'width': state['width'], } self.vehicle_params = { # Rear wheel offset in meters 'refoffset': 1.5, # Wheelbase in meters 'wheelbase': 3, # Maximum steering angle in radians 'maxsteer': 0.5, } # Define the vehicle steering dynamics as an input/output system self.vehicle = ct.NonlinearIOSystem( self.vehicle_update, self.vehicle_output, states=3, name='vehicle', inputs=('v', 'delta'), outputs=('x', 'y'), params=self.vehicle_params, ) self.pos_log = { 'x_position': [self.lateral_state['x_position']], 'y_position': [self.lateral_state['y_position']], 'heading': [self.lateral_state['heading']], 'lane_id': [self.lateral_state['lane_id']] }
omega_2 = [ u_input[1], u_input[3], u_input[5], u_input[7], u_input[9], u_input[11] ] # Control law # u = Theta * omega return [ sum([a * b for a, b in zip(theta_1, omega_1)]), sum([a * b for a, b in zip(theta_2, omega_2)]) ] IO_ADAPTIVE = control.NonlinearIOSystem(adaptive_state, adaptive_output, inputs=16, outputs=2, states=12, name='control', dt=0) # Define the closed-loop system # x_cl = [plant[5], reference[2], input_filter[4], output_filter[4], controller[12]] IO_CLOSED = control.InterconnectedSystem( (IO_PLANT, IO_REF_MODEL, IO_INPUT_FILTER, IO_OUTPUT_FILTER, IO_ADAPTIVE), connections=(('plant.u[0]', 'control.y[0]'), ('plant.u[1]', 'control.y[1]'), ('input_filter.u[0]', 'control.y[0]'), ('input_filter.u[1]', 'control.y[1]'), ('output_filter.u[0]', 'plant.y[0]'), ('output_filter.u[1]', 'plant.y[1]'), ('control.u[2]',
def test_optimal_doc(): """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 = [0., -2., 0.] u0 = [10., 0.] xf = [100., 2., 0.] uf = [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])] # Solve the optimal control problem horizon = np.linspace(0, Tf, 3, endpoint=True) result = opt.solve_ocp(vehicle, horizon, x0, traj_cost, constraints, terminal_cost=term_cost, initial_guess=u0) # Make sure the resulting trajectory generate a good solution resp = ct.input_output_response(vehicle, horizon, result.inputs, x0, t_eval=np.linspace(0, Tf, 10)) t, y = resp assert (y[0, -1] - xf[0]) / xf[0] < 0.01 assert (y[1, -1] - xf[1]) / xf[1] < 0.01 assert y[2, -1] < 0.1
e = x_p - x_m e_u = e - e_delta # Control law u_c = theta * x_p + k * r u = u_c if abs(u_c) <= U_MAX else U_MAX * np.sign(u_c) delta_u = u - u_c return [u, theta, k, e, delta_u, u_c, e_delta, e_u] IO_ADAPTIVE = control.NonlinearIOSystem(adaptive_state, adaptive_output, inputs=3, outputs=('u', 'theta', 'k', 'e', 'delta_u', 'u_c', 'e_delta', 'e_u'), states=4, name='control', dt=0) # Define the closed-loop system # x_cl = [plant.x_p, ref_model.x_m, control.theta, control.k, control.beta_delta, control.e_delta] IO_CLOSED = control.InterconnectedSystem( (IO_PLANT, IO_REF_MODEL, IO_ADAPTIVE), connections=(('plant.u', 'control.u'), ('control.u[1]', 'plant.x_p'), ('control.u[2]', 'ref_model.x_m')), inplist=('ref_model.r', 'control.u[0]'), outlist=('plant.x_p', 'ref_model.x_m', 'control.u', 'control.theta', 'control.k', 'control.e', 'control.u_c', 'control.e_delta', 'control.e_u'),
# u = controller power signal # returns heater power after saturation power = x[0] if u < 0.0: u = 0 if u > Pmax: #300Watt heating element u = Pmax dP = u - power return dP # https://python-control.readthedocs.io/en/0.8.3/iosys.html # Heater system heater_IO = ctl.NonlinearIOSystem(hupdate, None, inputs='p', outputs='ph', states='p') ctl_PID_IO = ctl.iosys.tf2io(conpid) # includes Kd plant_IO = ctl.LinearIOSystem(ss_mod) ufb_IO = ctl.iosys.tf2io(ufb) # unity feedback clsysSat = ctl.feedback( ctl.series(ctl_PID_IO, heater_IO, plant_IO), ufb_IO, sign=-1) # negative fb closed loop system w/ Saturation # Saturation test: #input = Texp # simple ramp #for i,iN in enumerate(input): #input[i] = 2*iN - 250 # expose the saturation
# Return the derivative of the state return np.array([ math.cos(x[2]) * u[0], # xdot = cos(theta) v math.sin(x[2]) * u[0], # ydot = sin(theta) v (u[0] / l) * math.tan(phi) # thdot = v/l tan(phi) ]) def vehicle_output(t, x, u, params): return x # return x, y, theta (full state) vehicle = ct.NonlinearIOSystem(vehicle_update, vehicle_output, states=3, name='vehicle', inputs=('v', 'phi'), outputs=('x', 'y', 'theta')) # Initial and final conditions x0 = [0., -2., 0.] u0 = [10., 0.] xf = [100., 2., 0.] uf = [10., 0.] Tf = 10 # Define the time horizon (and spacing) for the optimization horizon = np.linspace(0, Tf, 10, endpoint=True) # Provide an intial guess (will be extended to entire horizon) bend_left = [10, 0.01] # slight left veer
# The force F is generated by the engine, whose torque is proportional to # the rate of fuel injection, which is itself proportional to a control # signal 0 <= u <= 1 that controls the throttle position. The torque also # depends on engine speed omega. def motor_torque(omega, params={}): # Set up the system parameters Tm = params.get('Tm', 190.) # engine torque constant omega_m = params.get('omega_m', 420.) # peak engine angular speed beta = params.get('beta', 0.4) # peak engine rolloff return np.clip(Tm * (1 - beta * (omega/omega_m - 1)**2), 0, None) # Define the input/output system for the vehicle vehicle = ct.NonlinearIOSystem( vehicle_update, None, name='vehicle', inputs=('u', 'gear', 'theta'), outputs=('v'), states=('v')) # Figure 1.11: A feedback system for controlling the speed of a vehicle. In # this example, the speed of the vehicle is measured and compared to the # desired speed. The controller is a PI controller represented as a transfer # function. In the textbook, the simulations are done for LTI systems, but # here we simulate the full nonlinear system. # Construct a PI controller with rolloff, as a transfer function Kp = 0.5 # proportional gain Ki = 0.1 # integral gain control_tf = ct.tf2io( ct.TransferFunction([Kp, Ki], [1, 0.01*Ki/Kp]), name='control', inputs='u', outputs='y')
# Algebraic relationships e = x_p - x_m # Control law u = theta * x_p + r # Constant bias v = V return [u, v, theta, e] IO_ADAPTIVE = control.NonlinearIOSystem(adaptive_state, adaptive_output, inputs=3, outputs=('u', 'v', 'theta', 'e'), states=1, name='control', dt=0) # Define the closed-loop system # x_cl = [plant.x_p, ref_model.x_m, control.theta] IO_CLOSED = control.InterconnectedSystem( (IO_PLANT, IO_REF_MODEL, IO_ADAPTIVE), connections=(('plant.u', 'control.u'), ('plant.v', 'control.v'), ('control.u[1]', 'plant.x_p'), ('control.u[2]', 'ref_model.x_m')), inplist=('control.u[0]'), outlist=('plant.x_p', 'ref_model.x_m', 'control.u', 'control.v', 'control.theta', 'control.e'), dt=0)
return x[0:2] # Default vehicle parameters (including nominal velocity) vehicle_params = { 'refoffset': 1.5, 'wheelbase': 3, 'velocity': 15, 'maxsteer': 0.5 } # Define the vehicle steering dynamics as an input/output system vehicle = ct.NonlinearIOSystem(vehicle_update, vehicle_output, states=3, name='vehicle', inputs=('v', 'delta'), outputs=('x', 'y'), params=vehicle_params) # # # # ## Vehicle driving on a curvy road (Figure 8.6a) # ## # System parameters wheelbase = vehicle_params['wheelbase'] # coming from part1 v0 = vehicle_params['velocity'] # Dictionary parameters membership key # Control inputs
return np.array([ np.cos(x[2]) * u[0], # xdot = cos(psi) v np.sin(x[2]) * u[0], # ydot = sin(psi) v (u[0] / l) * np.tan(x[3]), # psi_dot = v/l tan(delta) delta_rate # delta_dot = u ]) def vehicle_output(t, x, u, params): return x # return x, y, psi (full state) # Define the vehicle steering dynamics as an input/output system vehicle = ct.NonlinearIOSystem(vehicle_update, vehicle_output, states=4, name='vehicle', inputs=('v', 'delta_rate'), outputs=('x', 'y', 'psi', 'delta')) ############################################################################### # Control ############################################################################### def Sigmoid(x): return x / (np.fabs(x) + eps) def Sat(x): val = 0 if x >= Delta: val = 1
# def target_output(t, x, u, params): ex = [u[1] - icx for icx in target_curvature_sets.x] ey = [u[2] - icy for icy in target_curvature_sets.y] d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(ex, ey)] mind = min(d) index = d.index(mind) return np.array([target_curvature_sets.x[index],target_curvature_sets.y[index],target_curvature_sets.yaw[index],target_curvature_sets.k[index],u[0]]) # Define the trajectory generator as an input/output system target = ct.NonlinearIOSystem( None, target_output, name='target', inputs=('v_ref', 'x', 'y'), outputs=('x_r', 'y_r', 'yaw_r', 'k_r' , 'v_r')) ############################################################################### # Control ############################################################################### # angle to -pi-pi def pi_2_pi(angle): return (angle + np.pi) % (2 * np.pi) - np.pi def solve_DARE(A, B, Q, R): """ solve a discrete time_Algebraic Riccati equation (DARE) """ X = Q maxiter = 150
# Get the input vector F1, F2, d = u # Get the system response from the original dynamics xdot, ydot, thetadot, xddot, yddot, thddot = \ pvtol_update(t, x, u[0:2], params) # Now add the wind term m = params.get('m', 4.) # mass of aircraft xddot += d / m return np.array([xdot, ydot, thetadot, xddot, yddot, thddot]) windy_pvtol = ct.NonlinearIOSystem( windy_update, pvtol_output, name="windy_pvtol", states = [f'x{i}' for i in range(6)], inputs = ['F1', 'F2', 'd'], outputs = [f'x{i}' for i in range(6)] ) # # PVTOL dynamics with noise and disturbances # def noisy_update(t, x, u, params): # Get the inputs F1, F2, Dx, Dy, Nx, Ny, Nth = u # Get the system response from the original dynamics xdot, ydot, thetadot, xddot, yddot, thddot = \ pvtol_update(t, x, u[0:2], params)