def test_flat_default_output(self, vehicle_flat): # Construct a flat system with the default outputs flatsys = fs.FlatSystem(vehicle_flat.forward, vehicle_flat.reverse, vehicle_flat.updfcn, inputs=vehicle_flat.ninputs, outputs=vehicle_flat.ninputs, states=vehicle_flat.nstates) # Define the endpoints of the trajectory x0 = [0., -2., 0.] u0 = [10., 0.] xf = [100., 2., 0.] uf = [10., 0.] Tf = 10 # Find trajectory between initial and final conditions poly = fs.PolyFamily(6) traj1 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly) traj2 = fs.point_to_point(flatsys, Tf, x0, u0, xf, uf, basis=poly) # Verify that the trajectory computation is correct T = np.linspace(0, Tf, 10) x1, u1 = traj1.eval(T) x2, u2 = traj2.eval(T) np.testing.assert_array_almost_equal(x1, x2) np.testing.assert_array_almost_equal(u1, u2) # Run a simulation and verify that the outputs are correct resp1 = ct.input_output_response(vehicle_flat, T, u1, x0) resp2 = ct.input_output_response(flatsys, T, u1, x0) np.testing.assert_array_almost_equal(resp1.outputs[0:2], resp2.outputs)
def test_kinematic_car(self, vehicle_flat, poly): # Define the endpoints of the trajectory x0 = [0., -2., 0.] u0 = [10., 0.] xf = [100., 2., 0.] uf = [10., 0.] Tf = 10 # Find trajectory between initial and final conditions traj = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly) # Verify that the trajectory computation is correct x, u = traj.eval([0, Tf]) np.testing.assert_array_almost_equal(x0, x[:, 0]) np.testing.assert_array_almost_equal(u0, u[:, 0]) np.testing.assert_array_almost_equal(xf, x[:, 1]) np.testing.assert_array_almost_equal(uf, u[:, 1]) # Simulate the system and make sure we stay close to desired traj T = np.linspace(0, Tf, 100) xd, ud = traj.eval(T) resp = ct.input_output_response(vehicle_flat, T, ud, x0) np.testing.assert_array_almost_equal(resp.states, xd, decimal=2) # For SciPy 1.0+, integrate equations and compare to desired if StrictVersion(sp.__version__) >= "1.0": t, y, x = ct.input_output_response(vehicle_flat, T, ud, x0, return_x=True) np.testing.assert_allclose(x, xd, atol=0.01, rtol=0.01)
def test_discrete_lqr(): # oscillator model defined in 2D # Source: https://www.mpt3.org/UI/RegulationProblem A = [[0.5403, -0.8415], [0.8415, 0.5403]] B = [[-0.4597], [0.8415]] C = [[1, 0]] D = [[0]] # Linear discrete-time model with sample time 1 sys = ct.ss2io(ct.ss(A, B, C, D, 1)) # Include weights on states/inputs Q = np.eye(2) R = 1 K, S, E = ct.dlqr(A, B, Q, R) # Compute the integral and terminal cost integral_cost = opt.quadratic_cost(sys, Q, R) terminal_cost = opt.quadratic_cost(sys, S, None) # Solve the LQR problem lqr_sys = ct.ss2io(ct.ss(A - B @ K, B, C, D, 1)) # Generate a simulation of the LQR controller time = np.arange(0, 5, 1) x0 = np.array([1, 1]) _, _, lqr_x = ct.input_output_response(lqr_sys, time, 0, x0, return_x=True) # Use LQR input as initial guess to avoid convergence/precision issues lqr_u = np.array(-K @ lqr_x[0:time.size]) # convert from matrix # Formulate the optimal control problem and compute optimal trajectory optctrl = opt.OptimalControlProblem(sys, time, integral_cost, terminal_cost=terminal_cost, initial_guess=lqr_u) res1 = optctrl.compute_trajectory(x0, return_states=True) # Compare to make sure results are the same np.testing.assert_almost_equal(res1.inputs, lqr_u[0]) np.testing.assert_almost_equal(res1.states, lqr_x) # Add state and input constraints trajectory_constraints = [ (sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -.5], [5, 5, 0.5]), ] # Re-solve res2 = opt.solve_ocp(sys, time, x0, integral_cost, trajectory_constraints, terminal_cost=terminal_cost, initial_guess=lqr_u) # Make sure we got a different solution assert np.any(np.abs(res1.inputs - res2.inputs) > 0.1)
def compareKalmanAndSys(): # test kalman estimator & compare non-linear system and linear system T = np.arange(tspan[0], tspan[1], dt) uDIST = np.random.multivariate_normal(np.zeros(4), Vd, size=T.shape[0]) uDIST = np.transpose(uDIST) uNOISE = np.random.normal(0.0, Vn, size=(1, T.shape[0])) u0 = np.zeros([1, T.shape[0]], dtype=np.float) u0[:, 100:110] = 20 # impulse u0[:, 400:420] = -20 # impulse uAUG = np.concatenate([u0, uDIST, uNOISE, u0]) y0 = [] y0.extend(yInit) y0.extend(yInit) t0, yout0 = control.input_output_response(sysPartKF, T, U=uAUG, X0=y0) t1, yout1 = control.input_output_response(sysFullNonLin, T, U=u0, X0=yInit) t2, yout2 = control.input_output_response(sysFull, T, U=np.concatenate( [u0, uDIST, uNOISE]), X0=yInit) plt.plot(T, yout0[0, :], "k-", label="noise observation of x") plt.plot(T, yout0[1, :], "r-", label="kalman estimation of x") plt.plot(T, yout0[2, :], "g-", label="kalman estimation of x_dot") plt.plot(T, yout0[3, :], "b-", label="kalman estimation of theta") plt.plot(T, yout0[4, :], "y-", label="kalman estimation of theta_dot") plt.plot(T, yout1[0, :], "y--", label="non-linear sys state of x") plt.plot(T, yout1[1, :], "r--", label="non-linear sys state of x_dot") plt.plot(T, yout1[2, :], "g--", label="non-linear sys state of theta") plt.plot(T, yout1[3, :], "b--", label="non-linear sys state of theta_dot") plt.plot(T, yout2[0, :], "y:", label="linear sys state of x") plt.plot(T, yout2[1, :], "r:", label="linear sys state of x_dot") plt.plot(T, yout2[2, :], "g:", label="linear sys state of theta") plt.plot(T, yout2[3, :], "b:", label="linear sys state of theta_dot") plt.title( 'Compare non-linear system, linear system and kalman estimator on noise linear system' ) plt.legend(bbox_to_anchor=(-0.05, -0.5, 1.05, 0.25), loc='lower left', ncol=3, mode="expand", borderaxespad=0.) plt.subplots_adjust(bottom=0.3) plt.show()
def test_discrete_lqr(): # oscillator model defined in 2D # Source: https://www.mpt3.org/UI/RegulationProblem A = [[0.5403, -0.8415], [0.8415, 0.5403]] B = [[-0.4597], [0.8415]] C = [[1, 0]] D = [[0]] # Linear discrete-time model with sample time 1 sys = ct.ss2io(ct.ss(A, B, C, D, 1)) # Include weights on states/inputs Q = np.eye(2) R = 1 K, S, E = ct.lqr(A, B, Q, R) # note: *continuous* time LQR # Compute the integral and terminal cost integral_cost = opt.quadratic_cost(sys, Q, R) terminal_cost = opt.quadratic_cost(sys, S, None) # Formulate finite horizon MPC problem time = np.arange(0, 5, 1) x0 = np.array([1, 1]) optctrl = opt.OptimalControlProblem(sys, time, integral_cost, terminal_cost=terminal_cost) res1 = optctrl.compute_trajectory(x0, return_states=True) with pytest.xfail("discrete LQR not implemented"): # Result should match LQR K, S, E = ct.dlqr(A, B, Q, R) lqr_sys = ct.ss2io(ct.ss(A - B @ K, B, C, D, 1)) _, _, lqr_x = ct.input_output_response(lqr_sys, time, 0, x0, return_x=True) np.testing.assert_almost_equal(res1.states, lqr_x) # Add state and input constraints trajectory_constraints = [ (sp.optimize.LinearConstraint, np.eye(3), [-10, -10, -1], [10, 10, 1]), ] # Re-solve res2 = opt.solve_ocp(sys, time, x0, integral_cost, constraints, terminal_cost=terminal_cost) # Make sure we got a different solution assert np.any(np.abs(res1.inputs - res2.inputs) > 0.1)
def step(self, steering_angle, velocity_dif): """ Function responsible for the action transform into the continuous state space. :param dt: float, time step [sec] :param steering_angle: float, [rad] :param velocity_dif: float, difference to previous velocity [m/s] :return: new_state: containing the input vehicle's new position, orientation, speed, lane. """ self.lateral_state['steering_angle'] = steering_angle self.lateral_state['speed'] += velocity_dif # Simulate the system + estimator # Resolution of the trajectory timesteps = np.array([0, self.dt]) # Velocity array v_curvy = np.full(timesteps.shape, self.lateral_state['speed']) # Steering angle array delta_curvy = np.full(timesteps.shape, self.lateral_state['steering_angle']) # Input array u_curvy = [v_curvy, delta_curvy] # Initial condition (x, y, heading [rad]) x_0 = [ self.lateral_state['x_position'], self.lateral_state['y_position'], self.lateral_state['heading'] ] _, _, x_curvy = ct.input_output_response( self.vehicle, timesteps, u_curvy, x_0, params=self.vehicle_params, return_x=True, ) new_x, new_y, heading = x_curvy[:, -1] self.pos_log['x_position'].append(new_x) self.pos_log['y_position'].append(new_y) self.pos_log['heading'].append(heading) dif_x = new_x - self.lateral_state['x_position'] dif_y = new_y - self.lateral_state['y_position'] self.update_in_lane_position(dif_x, dif_y, road_curve=None) self.update_absolute_position(new_x, new_y) self.lateral_state['heading'] = heading return self.lateral_state
def test_mpc_iosystem(): # model of an aircraft discretized with 0.2s sampling time # Source: https://www.mpt3.org/UI/RegulationProblem A = [[0.99, 0.01, 0.18, -0.09, 0], [ 0, 0.94, 0, 0.29, 0], [ 0, 0.14, 0.81, -0.9, 0], [ 0, -0.2, 0, 0.95, 0], [ 0, 0.09, 0, 0, 0.9]] B = [[ 0.01, -0.02], [-0.14, 0], [ 0.05, -0.2], [ 0.02, 0], [-0.01, 0]] C = [[0, 1, 0, 0, -1], [0, 0, 1, 0, 0], [0, 0, 0, 1, 0], [1, 0, 0, 0, 0]] model = ct.ss2io(ct.ss(A, B, C, 0, 0.2)) # For the simulation we need the full state output sys = ct.ss2io(ct.ss(A, B, np.eye(5), 0, 0.2)) # compute the steady state values for a particular value of the input ud = np.array([0.8, -0.3]) xd = np.linalg.inv(np.eye(5) - A) @ B @ ud yd = C @ xd # provide constraints on the system signals constraints = [opt.input_range_constraint(sys, [-5, -6], [5, 6])] # provide penalties on the system signals Q = model.C.transpose() @ np.diag([10, 10, 10, 10]) @ model.C R = np.diag([3, 2]) cost = opt.quadratic_cost(model, Q, R, x0=xd, u0=ud) # online MPC controller object is constructed with a horizon 6 ctrl = opt.create_mpc_iosystem( model, np.arange(0, 6) * 0.2, cost, constraints) # Define an I/O system implementing model predictive control loop = ct.feedback(sys, ctrl, 1) # Choose a nearby initial condition to speed up computation X0 = np.hstack([xd, np.kron(ud, np.ones(6))]) * 0.99 Nsim = 12 tout, xout = ct.input_output_response( loop, np.arange(0, Nsim) * 0.2, 0, X0) # Make sure the system converged to the desired state np.testing.assert_allclose( xout[0:sys.nstates, -1], xd, atol=0.1, rtol=0.01)
def main(): # Initialize gain scheduled controller # Using two simple P-controllers, see also fcat.longitudinal_controller for more complicated examples # Note that this is just an example on how to build and simulate an aircraft system # The controllers are simple not tuned to get desired reference tracking controllers = [ SaturatedStateSpaceMatricesGS( A = -1*np.eye(1), B = np.zeros((1,1)), C = np.eye(1), D = np.zeros((1,1)), upper = 1, lower = -1, switch_signal = 0.3 ), SaturatedStateSpaceMatricesGS( A = -1*np.eye(1), B = np.zeros((1,1)), C = np.eye(1), D = np.zeros((1,1)), upper = 1, lower = -1, switch_signal = 0.6 ) ] # Using similar controllers for simplicity lat_gs_controller = init_gs_controller(controllers, Direction.LATERAL, 'icing') lon_gs_controller = init_gs_controller(controllers, Direction.LONGITUDINAL, 'icing') airspeed_pi_controller = init_airspeed_controller() control_input = ControlInput() control_input.throttle = 0.5 state = State() state.vx = 20 prop = IcedSkywalkerX8Properties(control_input) aircraft_model = build_nonlin_sys(prop, no_wind(), outputs=State.names+['icing', 'airspeed']) motor_time_constant = 0.001 elevon_time_constant = 0.001 actuator_model = build_flying_wing_actuator_system(elevon_time_constant, motor_time_constant) closed_loop = build_closed_loop(actuator_model, aircraft_model, lon_gs_controller, lat_gs_controller, airspeed_pi_controller) X0 = get_init_vector(closed_loop, control_input, state) constant_input = np.array([20, 0.04, -0.00033310605950459315]) sim_time = 15 t = np.linspace(0, sim_time, sim_time*5, endpoint=True) u = np.array([constant_input, ]*(len(t))).transpose() T, yout_non_lin, _ = input_output_response( closed_loop, U=u, T=t, X0=X0, return_x=True, method='BDF')
def evalClosedFeedback(): T = np.arange(tspan[0], tspan[1], dt) uDIST = np.random.multivariate_normal(np.zeros(4), Vd, size=T.shape[0]) uDIST = np.transpose(uDIST) uNOISE = np.random.normal(0.0, Vn, size=(1, T.shape[0])) y0 = [] y0.extend(yInit) y0.extend(yInit) y0.extend(yInit) t0, yout0 = control.input_output_response(io_closed, T, U=np.concatenate([ Vd @ Vd @ uDIST, Vn * uNOISE, Vd @ Vd @ uDIST, Vn * uNOISE ]), X0=y0) # plt.plot(T, yout0[0, :], "k-", label="noise observation of x") # plt.plot(T, yout0[1, :], "r-", label="kalman estimation of x") # plt.plot(T, yout0[2, :], "g-", label="kalman estimation of x_dot") # plt.plot(T, yout0[3, :], "b-", label="kalman estimation of theta") # plt.plot(T, yout0[4, :], "y-", label="kalman estimation of theta_dot") # plt.plot(T, yout0[5, :], "y--", label="non-linear sys state of x") # plt.plot(T, yout0[6, :], "r--", label="non-linear sys state of x_dot") # plt.plot(T, yout0[7, :], "g--", label="non-linear sys state of theta") # plt.plot(T, yout0[8, :], "b--", label="non-linear sys state of theta_dot") # plt.plot(T, yout0[9, :], "k:", label="LQR control u") # plt.title('Close loop evaluation') # plt.legend(bbox_to_anchor=(-0.05, -0.4, 1.05, 0.25), loc='lower left', # ncol=3, mode="expand", borderaxespad=0.) # plt.subplots_adjust(bottom=0.3) # plt.show() ############################################## # GUI simulated animation yout = yout0[:, :] gui = ti.GUI('Sim cartpend', res=(512, 512), background_color=0xdddddd) t_curr = 0 while gui.running: drawcartpend(gui, yout[:, t_curr], m, M, L) gui.show() t_curr += 1 if (t_curr > yout.shape[1] - 1): print(yout[:, t_curr - 1]) t_curr = 0
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 test_trdata_labels(): # Create an I/O system with labels sys = ct.rss(4, 3, 2) iosys = ct.LinearIOSystem(sys) T = np.linspace(1, 10, 10) U = [np.sin(T), np.cos(T)] # Create a response response = ct.input_output_response(iosys, T, U) # Make sure the labels got created np.testing.assert_equal( response.output_labels, ["y[%d]" % i for i in range(sys.noutputs)]) np.testing.assert_equal( response.state_labels, ["x[%d]" % i for i in range(sys.nstates)]) np.testing.assert_equal( response.input_labels, ["u[%d]" % i for i in range(sys.ninputs)])
def test_actuator_model(): control_input = ControlInput() control_input.elevon_right = 2 control_input.elevon_left = 3 control_input.throttle = 0.5 control_input.rudder = 0 initial_value_elevator = 3 initial_value_aileron = 2 initial_value_throttle = 2 initial_values = np.array([ initial_value_elevator, initial_value_aileron, 0, initial_value_throttle ]) initial_values_flying_wing = np.linalg.inv( flying_wing2ctrl_input_matrix()).dot(initial_values) initial_value_elevon_right = initial_values_flying_wing[0] initial_value_elevon_left = initial_values_flying_wing[1] initial_value_throttle = initial_values_flying_wing[3] elevon_time_constant = 0.3 motor_time_constant = 0.2 lin_model = build_flying_wing_actuator_system(elevon_time_constant, motor_time_constant) t = np.linspace(0.0, 10, 500, endpoint=True) u = np.array([ control_input.control_input, ] * len(t)).transpose() T, yout = input_output_response(lin_model, t, U=u, X0=initial_values) yout = np.linalg.inv(flying_wing2ctrl_input_matrix()).dot(yout) expect_elevon_r = control_input.elevon_right + \ (initial_value_elevon_right - control_input.elevon_right)*np.exp(-t/elevon_time_constant) expect_elevon_l = control_input.elevon_left + \ (initial_value_elevon_left - control_input.elevon_left)*np.exp(-t/elevon_time_constant) expect_motor = control_input.throttle + \ (initial_value_throttle - control_input.throttle)*np.exp(-t/motor_time_constant) assert np.allclose(expect_elevon_r, yout[0, :], atol=5e-3) assert np.allclose(expect_elevon_l, yout[1, :], atol=5e-3) assert np.allclose(expect_motor, yout[3, :], atol=5e-3)
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
def states_from_control(self, x_init, U): """Compute non-linear states from vehicle model given initial states and control variables over time. Parameters ========== x_init : ndarray Initial state [x_0, y_0, psi_0, v_0]. U : ndarray Control variables u[i] for i = 0..T - 1 with rows [u_1, u_2] Returns ======= ndarray State trajectory from control including origin of shape (T + 1, 4) with rows [x, y, psi, v]. """ U_pad = np.concatenate((U, U[-1][None])) _, states = control.input_output_response(self.io_dynamical_system, self.timestamps, U_pad.T, x_init) return states.T
def test_kinematic_car(self): """Differential flatness for a kinematic car""" def vehicle_flat_forward(x, u, params={}): b = params.get('wheelbase', 3.) # get parameter values zflag = [np.zeros(3), np.zeros(3)] # list for flag arrays zflag[0][0] = x[0] # flat outputs zflag[1][0] = x[1] zflag[0][1] = u[0] * np.cos(x[2]) # first derivatives zflag[1][1] = u[0] * np.sin(x[2]) thdot = (u[0] / b) * np.tan(u[1]) # dtheta/dt zflag[0][2] = -u[0] * thdot * np.sin(x[2]) # second derivatives zflag[1][2] = u[0] * thdot * np.cos(x[2]) return zflag def vehicle_flat_reverse(zflag, params={}): b = params.get('wheelbase', 3.) # get parameter values x = np.zeros(3) u = np.zeros(2) # vectors to store x, u x[0] = zflag[0][0] # x position x[1] = zflag[1][0] # y position x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # angle u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2]) thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2]) u[1] = np.arctan2(thdot_v, u[0]**2 / b) return x, u def vehicle_update(t, x, u, params): b = params.get('wheelbase', 3.) # get parameter values dx = np.array([ np.cos(x[2]) * u[0], np.sin(x[2]) * u[0], (u[0] / b) * np.tan(u[1]) ]) return dx def vehicle_output(t, x, u, params): return x # Create differentially flat input/output system vehicle_flat = fs.FlatSystem(vehicle_flat_forward, vehicle_flat_reverse, vehicle_update, vehicle_output, inputs=('v', 'delta'), outputs=('x', 'y', 'theta'), states=('x', 'y', 'theta')) # Define the endpoints of the trajectory x0 = [0., -2., 0.] u0 = [10., 0.] xf = [100., 2., 0.] uf = [10., 0.] Tf = 10 # Define a set of basis functions to use for the trajectories poly = fs.PolyFamily(6) # Find trajectory between initial and final conditions traj = fs.point_to_point(vehicle_flat, x0, u0, xf, uf, Tf, basis=poly) # Verify that the trajectory computation is correct x, u = traj.eval([0, Tf]) np.testing.assert_array_almost_equal(x0, x[:, 0]) np.testing.assert_array_almost_equal(u0, u[:, 0]) np.testing.assert_array_almost_equal(xf, x[:, 1]) np.testing.assert_array_almost_equal(uf, u[:, 1]) # Simulate the system and make sure we stay close to desired traj T = np.linspace(0, Tf, 500) xd, ud = traj.eval(T) # For SciPy 1.0+, integrate equations and compare to desired if StrictVersion(sp.__version__) >= "1.0": t, y, x = ct.input_output_response(vehicle_flat, T, ud, x0, return_x=True) np.testing.assert_allclose(x, xd, atol=0.01, rtol=0.01)
# System inputs inplist=['target.v_ref'], inputs=['v_ref'], # System outputs outlist=['vehicle.x', 'vehicle.y' , 'vehicle.yaw','controller.delta','target.yaw_r'], outputs=['x', 'y', 'psi', 'delta', 'psi_ref'] ) ############################################################################### # Input Output Response ############################################################################### # time of response T = np.linspace(0,simulation_time,simulation_sample) # the response tout, yout = ct.input_output_response(LatRearWheelFeedbackControl, T, [v_ref*np.ones(len(T))],X0=[init_x,init_y,init_yaw]) err_curvature = [] for i in range(len(tout)): ex = [yout[0][i] - icx for icx in target_curvature_sets.x] ey = [yout[1][i] - 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) err_curvature.append(np.sqrt(mind)) plt.figure() plt.title('Tracking')
def test_squeeze(self, fcn, nstate, nout, ninp, squeeze, shape1, shape2): # Figure out if we have SciPy 1+ scipy0 = StrictVersion(sp.__version__) < '1.0' # Define the system if fcn == ct.tf and (nout > 1 or ninp > 1) and not slycot_check(): pytest.skip("Conversion of MIMO systems to transfer functions " "requires slycot.") else: sys = fcn(ct.rss(nstate, nout, ninp, strictly_proper=True)) # Generate the time and input vectors tvec = np.linspace(0, 1, 8) uvec = np.ones((sys.ninputs, 1)) @ np.reshape(np.sin(tvec), (1, 8)) # # Pass squeeze argument and make sure the shape is correct # # For responses that are indexed by the input, check against shape1 # For responses that have no/fixed input, check against shape2 # # Impulse response if isinstance(sys, StateSpace): # Check the states as well _, yvec, xvec = ct.impulse_response( sys, tvec, squeeze=squeeze, return_x=True) if sys.issiso(): assert xvec.shape == (sys.nstates, 8) else: assert xvec.shape == (sys.nstates, sys.ninputs, 8) else: _, yvec = ct.impulse_response(sys, tvec, squeeze=squeeze) assert yvec.shape == shape1 # Step response if isinstance(sys, StateSpace): # Check the states as well _, yvec, xvec = ct.step_response( sys, tvec, squeeze=squeeze, return_x=True) if sys.issiso(): assert xvec.shape == (sys.nstates, 8) else: assert xvec.shape == (sys.nstates, sys.ninputs, 8) else: _, yvec = ct.step_response(sys, tvec, squeeze=squeeze) assert yvec.shape == shape1 # Initial response (only indexed by output) if isinstance(sys, StateSpace): # Check the states as well _, yvec, xvec = ct.initial_response( sys, tvec, 1, squeeze=squeeze, return_x=True) assert xvec.shape == (sys.nstates, 8) else: _, yvec = ct.initial_response(sys, tvec, 1, squeeze=squeeze) assert yvec.shape == shape2 # Forced response (only indexed by output) if isinstance(sys, StateSpace): # Check the states as well _, yvec, xvec = ct.forced_response( sys, tvec, uvec, 0, return_x=True, squeeze=squeeze) assert xvec.shape == (sys.nstates, 8) else: # Just check the input/output response _, yvec = ct.forced_response(sys, tvec, uvec, 0, squeeze=squeeze) assert yvec.shape == shape2 # Test cases where we choose a subset of inputs and outputs _, yvec = ct.step_response( sys, tvec, input=ninp-1, output=nout-1, squeeze=squeeze) if squeeze is False: # Shape should be unsqueezed assert yvec.shape == (1, 1, 8) else: # Shape should be squeezed assert yvec.shape == (8, ) # For InputOutputSystems, also test input/output response if isinstance(sys, ct.InputOutputSystem) and not scipy0: _, yvec = ct.input_output_response(sys, tvec, uvec, squeeze=squeeze) assert yvec.shape == shape2 # # Changing config.default to False should return 3D frequency response # ct.config.set_defaults('control', squeeze_time_response=False) _, yvec = ct.impulse_response(sys, tvec) if squeeze is not True or sys.ninputs > 1 or sys.noutputs > 1: assert yvec.shape == (sys.noutputs, sys.ninputs, 8) _, yvec = ct.step_response(sys, tvec) if squeeze is not True or sys.ninputs > 1 or sys.noutputs > 1: assert yvec.shape == (sys.noutputs, sys.ninputs, 8) _, yvec = ct.initial_response(sys, tvec, 1) if squeeze is not True or sys.noutputs > 1: assert yvec.shape == (sys.noutputs, 8) if isinstance(sys, ct.StateSpace): _, yvec, xvec = ct.forced_response( sys, tvec, uvec, 0, return_x=True) assert xvec.shape == (sys.nstates, 8) else: _, yvec = ct.forced_response(sys, tvec, uvec, 0) if squeeze is not True or sys.noutputs > 1: assert yvec.shape == (sys.noutputs, 8) # For InputOutputSystems, also test input_output_response if isinstance(sys, ct.InputOutputSystem) and not scipy0: _, yvec = ct.input_output_response(sys, tvec, uvec) if squeeze is not True or sys.noutputs > 1: assert yvec.shape == (sys.noutputs, 8)
# System parameters wheelbase = vehicle_params['wheelbase'] # coming from part1 v0 = vehicle_params['velocity'] # Dictionary parameters membership key # Control inputs T_curvy = np.linspace(0, 7, 500) v_curvy = v0 * np.ones(T_curvy.shape) delta_curvy = 0.1 * np.sin(T_curvy) * np.cos(4 * T_curvy) + 0.0025 * np.sin( T_curvy * np.pi / 7) u_curvy = [v_curvy, delta_curvy] X0_curvy = [0, 0.8, 0] # Simulate the system + estimator t_curvy, y_curvy, x_curvy = ct.input_output_response(vehicle, T_curvy, u_curvy, X0_curvy, params=vehicle_params, return_x=True) # Configure matplotlib plots to be a bit bigger and optimize layout plt.figure(figsize=[9, 4.5]) # Plot the resulting trajectory (and some road boundaries) plt.subplot( 1, 4, 2) # Add an Axes to the current figure or retrieve an existing Axes plt.plot(y_curvy[1], y_curvy[0]) # Plot y versus x as lines and/or markers. plt.plot(y_curvy[1] - 9 / np.cos(x_curvy[2]), y_curvy[0], 'k-', linewidth=1) plt.plot(y_curvy[1] - 3 / np.cos(x_curvy[2]), y_curvy[0], 'k--', linewidth=1) plt.plot(y_curvy[1] + 3 / np.cos(x_curvy[2]), y_curvy[0], 'k-', linewidth=1) plt.xlabel('y [m]') # Set the label for the x-axis.
if False: dt = 1 # min Tmod = np.arange(0, 24 * 60, dt) # 24 hours by minutes Rmod = np.zeros(len(Tmod)) Cmod = Rmod for i, t in enumerate(Tmod): if t <= 300: Rmod[i] = 188 - tamb # denature phase Cmod[i] = 188 else: Rmod[i] = 94 - tamb # fermentation Cmod[i] = 94 # closed loop non-linear response (model) t4, y4 = ctl.input_output_response(clsysSat, Tmod, [Rmod]) # closed loop non-linear control effort (model) clsysEFF = ctl.feedback( ctl.series(ctl_PID_IO, heater_IO), plant_IO, sign=-1) # negative fb closed loop system w/ Saturation t5, yCEF = ctl.input_output_response(clsysEFF, Tmod, [Rmod]) th = np.zeros(np.shape(t4)) for i in range(np.shape(t4)[0]): #y4[i] = tamb + y4[i] # add back in ambient th[i] = t4[i] / 60.00 # time to hours ax, fig = plt.subplots() #plt.plot(th,y4,th,yCEF,th,Cmod,th,Rmod) # plt.plot(th, y4, th, yCEF, th, Cmod) #,th,Rmod) #
def main(): # Script showing how to simulate icedskywalkerX8 from config_file using controllers saved in file config_file = "examples/skywalkerX8_linearize.yml" lat_controller_file = "examples/inner_loop_controllers/single_robust_roll_ctrl.json" lon_controller_file = "examples/inner_loop_controllers/single_robust_pitch_ctrl.json" K_lat = get_state_space_from_file(lat_controller_file) K_lon = get_state_space_from_file(lon_controller_file) K_lat = SaturatedStateSpaceController(A=np.array(K_lat.A), B=np.array(K_lat.B), C=np.array(K_lat.C), D=np.array(K_lat.D), lower=-0.4, upper=0.4) K_lon = SaturatedStateSpaceController(A=np.array(K_lon.A), B=np.array(K_lon.B), C=np.array(K_lon.C), D=np.array(K_lon.D), lower=-0.4, upper=0.4) # Using similar controllers for simplicity lat_controller = init_robust_controller(K_lat, Direction.LATERAL) lon_controller = init_robust_controller(K_lon, Direction.LONGITUDINAL) airspeed_pi_controller = init_airspeed_controller() with open(config_file, 'r') as infile: data = load(infile) aircraft = aircraft_property_from_dct(data['aircraft']) ctrl = ControlInput.from_dict(data['init_control']) state = State.from_dict(data['init_state']) updates = { 'icing': [PropUpdate(0.5, 0.5), PropUpdate(5.0, 1.0)], } updater = PropertyUpdater(updates) aircraft_model = build_nonlin_sys(aircraft, no_wind(), outputs=State.names + ['icing', 'airspeed'], prop_updater=updater) actuator = actuator_from_dct(data['actuator']) closed_loop = build_closed_loop(actuator, aircraft_model, lon_controller, lat_controller, airspeed_pi_controller) X0 = get_init_vector(closed_loop, ctrl, state) constant_input = np.array([20, 0.2, -0.2]) sim_time = 15 t = np.linspace(0, sim_time, sim_time * 5, endpoint=True) u = np.array([ constant_input, ] * (len(t))).transpose() T, yout_non_lin, _ = input_output_response(closed_loop, U=u, T=t, X0=X0, return_x=True, method='BDF') plot_respons(T, yout_non_lin) plt.show()
def fil_y(pi_a, time, y_val, fil_y_val): fil_y_val.put(control.input_output_response(pi_a, time, y_val)) return
#n = 3 #m = 1 # Pi CONTROLLER n = 4 m = 1 result = Queue() phi = np.zeros((n + m + 1, len(u))) phi_hat = np.zeros((n + m + 1, len(u))) A = np.array([1, 10, 100, 100, 10000]) # INITIAL ESTIMATE for i in range(n): p_i = np.zeros(n) p_i[i] = 1 F = control.tf(p_i, A) # P^n/A(p) F = control.tf2io(F) Yf = control.input_output_response(F, t, y) phi[i] = -Yf[1].T for i in range(m + 1): p_i = np.zeros(m + 1) p_i[i] = 1 Uf = control.tf(p_i, A) # P^m/A(p) Uf = control.tf2io(Uf) Uf = control.input_output_response(Uf, t, u) phi[n + i] = Uf[1].T Pn = np.zeros(n + 1) Pn[0] = 1 yfn = control.tf(Pn, A) yfn = control.tf2io(yfn) yfn = control.input_output_response(yfn, t, y) front = np.matmul(phi, phi.T)
# Set initial conditions X0 = np.zeros((4, 1)) X0[0] = 1 X0[1] = J_HAT_0 X0[2] = B_HAT_0 # Define simulation time span and control input T = np.linspace(0, T_F, N_POINTS) U = np.zeros((2, N_POINTS)) XD_IN = np.sin(0.2 * np.pi * T) XD_DOT_IN = np.cos(0.2 * np.pi * T) U[0, :] = XD_IN U[1, :] = XD_DOT_IN # Simulate the system T_OUT, Y_OUT = control.input_output_response(IO_CLOSED, T, U, X0) # Plot the response plt.rc('text', usetex=True) plt.rc('font', family='sans') FIG = plt.figure(1, figsize=(6, 6), dpi=300, facecolor='w', edgecolor='k') RED = '#f62d73' BLUE = '#1269d3' WHITE = '#ffffff' AX_1 = FIG.add_subplot(3, 1, 1) AX_1.plot(T, XD_IN, label=r'$x_{d}$', color=RED) AX_1.plot(T_OUT, Y_OUT[0], label=r'$x$', color=BLUE) AX_1.set_ylabel('Motor Velocity')
yref = 1 T = np.linspace(0, 5, 100) # Set up a figure for plotting the results mpl.figure() # Plot the reference trajectory for the y position mpl.plot([0, 5], [yref, yref], 'k--') # Find the signals we want to plot y_index = steering.find_output('y') v_index = steering.find_output('v') # Do an iteration through different speeds for vref in [8, 10, 12]: # Simulate the closed loop controller response tout, yout = ct.input_output_response( steering, T, [vref * np.ones(len(T)), yref * np.ones(len(T))]) # Plot the reference speed mpl.plot([0, 5], [vref, vref], 'k--') # Plot the system output y_line, = mpl.plot(tout, yout[y_index, :], 'r') # lateral position v_line, = mpl.plot(tout, yout[v_index, :], 'b') # vehicle velocity # Add axis labels mpl.xlabel('Time (s)') mpl.ylabel('x vel (m/s), y pos (m)') mpl.legend((v_line, y_line), ('v', 'y'), loc='center right', frameon=False)
def test_kinematics(): control_input = ControlInput() prop = FrictionlessBall(control_input) outputs = [ "x", "y", "z", "roll", "pitch", "yaw", "vx", "vy", "vz", "ang_rate_x", "ang_rate_y", "ang_rate_z" ] system = build_nonlin_sys(prop, no_wind(), outputs) t = np.linspace(0.0, 10, 500, endpoint=True) state = State() state.vx = 20.0 state.vy = 1 for i in range(3): state.roll = 0 state.pitch = 0 state.yaw = 0 if i == 0: state.ang_rate_x = 0.157079633 state.ang_rate_y = 0 state.ang_rate_z = 0 elif i == 1: state.ang_rate_x = 0 state.ang_rate_y = 0.157079633 state.ang_rate_z = 0 elif i == 2: state.ang_rate_x = 0 state.ang_rate_y = 0 state.ang_rate_z = 0.157079633 T, yout = input_output_response(system, t, U=0, X0=state.state) pos = np.array(yout[:3]) eul_ang = np.array(yout[3:6]) vel_inertial = np.array( [np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t))]) for j in range(len(vel_inertial[0])): state.roll = yout[3, j] state.pitch = yout[4, j] state.yaw = yout[5, j] vel = np.array([yout[6, j], yout[7, j], yout[8, j]]) vel_inertial_elem = body2inertial(vel, state) vel_inertial[0, j] = vel_inertial_elem[0] vel_inertial[1, j] = vel_inertial_elem[1] vel_inertial[2, j] = vel_inertial_elem[2] vx_inertial_expect = 20 * np.ones(len(t)) vy_inertial_expect = 1 * np.ones(len(t)) vz_inertial_expect = GRAVITY_CONST * t x_expect = 20 * t y_expect = 1 * t z_expect = 0.5 * GRAVITY_CONST * t**2 roll_expect = state.ang_rate_x * t pitch_expect = state.ang_rate_y * t yaw_expect = state.ang_rate_z * t assert np.allclose(vx_inertial_expect, vel_inertial[0], atol=7e-3) assert np.allclose(vy_inertial_expect, vel_inertial[1], atol=5e-3) assert np.allclose(vz_inertial_expect, vel_inertial[2], atol=8e-3) assert np.allclose(x_expect, pos[0], atol=8e-2) assert np.allclose(y_expect, pos[1], atol=5e-2) assert np.allclose(z_expect, pos[2], atol=7e-2) assert np.allclose(roll_expect, eul_ang[0], atol=1e-3) assert np.allclose(pitch_expect, eul_ang[1], atol=1e-3) assert np.allclose(yaw_expect, eul_ang[2], atol=1e-3)
from matplotlib import pyplot as plt print("Reading Data") y = np.loadtxt("KHU_KongBot2/SRIVC/RefinedY.txt", delimiter=",") # linux u = np.loadtxt("KHU_KongBot2/SRIVC/RefinedU.txt", delimiter=",") # linux t = u[0:15000, 0] y = y[0:15000, 1] u = u[0:15000, 1] theta=np.loadtxt("KHU_KongBot2/SRIVC/PD_control_theta.txt") """ PD_Control 452.2 s + 5781 -------------------------------- s^3 + 38.85 s^2 + 851.1 s + 5774 """ n=3 m=1 A = theta[0:n] A = np.insert(A, 0, 1) B = theta[n:] sys=control.tf(B,A) print(sys) sys=control.tf2io(sys) y_m=control.input_output_response(sys,t,u) fig = plt.figure() plt.plot(t, u, color="b") plt.plot(t, y, color='r') plt.plot(y_m[0], y_m[1], color="g") plt.show()
# System inputs inplist=['target.v_ref'], inputs=['vref'], # System outputs outlist=['vehicle.x', 'vehicle.y', 'vehicle.psi', 'vehicle.delta'], outputs=['x', 'y', 'psi', 'delta']) ############################################################################### # Input Output Response ############################################################################### # time of response T = np.linspace(0, 10, 1000) # the response tout, yout = ct.input_output_response(LatSlidingModeControl, T, [vref * np.ones(len(T))], X0=[0.1, 0.1, 0.1, 0.0, 5.0, 0.31, 0.0]) #tout, yout = ct.input_output_response(vehicle, T, [vref*np.ones(len(T)),target_delta*np.ones(len(T)),delta_rate*np.ones(len(T))],X0=[0,5.0,0,0]) #plt.figure() #plt.title('track') #plt.xlabel('x[m]') #plt.ylabel('y[m]') #plt.plot(yout[0],yout[1]) # # #plt.figure() #plt.title('delta') #plt.xlabel('t[s]') #plt.ylabel('delta[deg]')
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
xf = [40., 2., 0.] uf = [10., 0.] Tf = 4 # Define a set of basis functions to use for the trajectories poly = fs.PolyFamily(6) # Find a trajectory between the initial condition and the final condition traj = fs.point_to_point(vehicle_flat, x0, u0, xf, uf, Tf, basis=poly) # Create the desired trajectory between the initial and final condition T = np.linspace(0, Tf, 500) xd, ud = traj.eval(T) # Simulation the open system dynamics with the full input t, y, x = ct.input_output_response(vehicle_flat, T, ud, x0, return_x=True) # Plot the open loop system dynamics plt.figure() plt.suptitle("Open loop trajectory for kinematic car lane change") # Plot the trajectory in xy coordinates plt.subplot(4, 1, 2) plt.plot(x[0], x[1]) plt.xlabel('x [m]') plt.ylabel('y [m]') plt.axis([x0[0], xf[0], x0[1] - 1, xf[1] + 1]) # Time traces of the state and input plt.subplot(2, 4, 5) plt.plot(t, x[1])
# Open-loop: 5 * (dy^2/dt^2)(t) + 4 * (dy/dt)(t) + 3 * y(t) = 2 * u(t) # Avec y(t) la sortie au temps t et u(t) la commande au temps t # On modelise ce systeme en temps discret (dt = 50ms, comme pour le ball and beam) dt = 0.05 # On utilise la fonction de transfert (facile a calculer), puis on transforme en representation # d'etat, car c'est necessaire pour ct.iosys.LinearIOSystem. syst_open_tf = ct.tf(np.array([2]), np.array([5, 4, 3]), dt) syst_open_ss = ct.tf2ss(syst_open_tf) # Creation d'un objet input/output pour modeliser la boucle ouverte. syst_open_io = ct.iosys.LinearIOSystem(syst_open_ss) # Analyse de la reponse du systeme a un signal en entree pendant 10s. t_in = np.arange(0, 10, dt) u_in = np.sin(t_in) t_out, y_open = ct.input_output_response(syst_open_io, T=t_in, U=u_in, squeeze=True) plt.plot(t_in, u_in, label="Command u(t)") plt.plot(t_out, y_open, label="Output y(t)") plt.title("Response of the open-loop system to a sinewave") plt.grid() plt.xlabel("Time t[s]") plt.ylabel("e.g. Voltage u[V]") plt.legend() plt.show()