def __init__(self, vehicles, Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, xmin=None, xmax=None, umin=None, umax=None, x0=None, runs=20, saved_h=2, startup_time=1.): self.vehicles = vehicles self.dt = delta_t self.h = horizon self.vopt = speed_profile.Speed() self.runs = runs self.truck_length = truck_length self.saved_h = saved_h self.time_average = 0 self.total_elapsed_time = 0 self.verbose = False self.timegap = timegap self.startup_time = startup_time self.positions = numpy.zeros((len(self.vehicles), runs)) self.velocities = numpy.zeros((len(self.vehicles), runs)) self.accelerations = numpy.zeros((len(self.vehicles), runs)) self.timegaps = numpy.zeros((len(self.vehicles), runs)) self.path_errors = numpy.zeros((len(self.vehicles), runs)) self.velocity_errors = numpy.zeros((len(self.vehicles), runs)) self.mpcs = [None for i in self.vehicles] for i, vehicle in enumerate(self.vehicles): if i == 0: leader = True else: leader = False self.mpcs[i] = old_mpc_solver.TruckMPC(Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, xmin=xmin, xmax=xmax, umin=umin, umax=umax, x0=x0, vehicle_id=vehicle.ID, saved_h=saved_h, is_leader=leader) print('\nSimulation initialized. ')
def main(args): # ID of the vehicle. if len(args) < 2: print('Need to enter at least one vehicle ID.') sys.exit() vehicle_id = args[1] # First argument is the ID of the vehicle. if len(args) > 2: preceding_id = args[2] # Second argument is, if entered, the ID of the preceding vehicle. is_leader = False else: preceding_id = 'None' # If no second argument, the vehicle is the leader. is_leader = True # Topic name for subscribing to truck positions. position_topic_name = 'mocap_state' # Topic name for publishing vehicle commands. control_topic_name = 'pwm_commands' # Name for starting and stopping recording. recording_service_name = vehicle_id + '/dmpc/set_measurement' # Filename prefix for recording data. recording_filename = 'dmpc' + '_' + vehicle_id + '_' # PID parameters for path tracking. k_p = 0.5 k_i = 0 k_d = 3 # MPC information. horizon = 20 delta_t = 0.1 Ad = numpy.matrix([[1., 0.], [delta_t, 1.]]) Bd = numpy.matrix([[delta_t], [0.]]) zeta = 0.90 s0 = 0. v0 = 0. Q_v = 1 # Part of Q matrix for velocity tracking. Q_s = 0.5 # Part of Q matrix for position tracking. Q = numpy.array([Q_v, 0, 0, Q_s]).reshape(2, 2) # State tracking. R_acc = 0.1 R = numpy.array([1]) * R_acc # Input tracking. velocity_min = 0. velocity_max = 2. position_min = -100000. position_max = 1000000. acceleration_min = -0.5 acceleration_max = 0.5 truck_length = 0.2 safety_distance = 0.1 timegap = 1. delay = 1. x0 = numpy.array([s0, v0]) xmin = numpy.array([velocity_min, position_min]) xmax = numpy.array([velocity_max, position_max]) umin = numpy.array([acceleration_min]) umax = numpy.array([acceleration_max]) # Reference speed profile. opt_v_pts = 1000 # How many points. opt_v_max = 1.2 opt_v_min = 0.8 opt_v_period_length = 60 # Period in meters. vopt = speed_profile.Speed() vopt.generate_sin(opt_v_min, opt_v_max, opt_v_period_length, opt_v_pts) vopt.repeating = True # Controller reference path. x_radius = 1.4 y_radius = 1.2 center = [0.2, -y_radius/2] pts = 400 pt = path.Path() pt.gen_circle_path([x_radius, y_radius], points=pts, center=center) # Initialize controller. controller = Controller( position_topic_name, control_topic_name, vehicle_id, preceding_id, is_leader, pt, Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, speed_ref=vopt, xmin=xmin, xmax=xmax, umin=umin, umax=umax, x0=x0, k_p=k_p, k_i=k_i, k_d=k_d, recording_service_name=recording_service_name, recording_filename=recording_filename, delay=delay ) # Start controller. controller.run()
def _brake(self): """Sets the speed profile to 0 m/s everywhere. """ self.speed_profile = speed_profile.Speed([1], [0])
def main(args): vehicle_amount = 1 if len(args) > 1: vehicle_amount = int(args[1]) vehicle_ids = [] for i in range(vehicle_amount): vehicle_ids.append('v{}'.format(i + 1)) # PID parameters for path tracking. k_p = 0.5 k_i = -0.02 k_d = 3 horizon = 15 delta_t = 0.1 Ad = numpy.matrix([[1., 0.], [delta_t, 1.]]) Bd = numpy.matrix([[delta_t], [0.]]) zeta = 0.5 # z = 1 -> full timegap tracking. Q_v = 1 # Part of Q matrix for velocity tracking. Q_s = 1 # Part of Q matrix for position tracking. Q = numpy.array([Q_v, 0, 0, Q_s]).reshape(2, 2) # State tracking. R_acc = 0.1 R = numpy.array([1]) * R_acc # Input tracking. velocity_min = 0. velocity_max = 2. position_min = -100000. position_max = 1000000. acceleration_min = -1.5 acceleration_max = 1.5 truck_length = 0.3 safety_distance = 0.2 timegap = 1. delay = 0.0 simulation_length = 40 # How many seconds to simulate. xmin = numpy.array([velocity_min, position_min]) xmax = numpy.array([velocity_max, position_max]) umin = numpy.array([acceleration_min]) umax = numpy.array([acceleration_max]) # Reference speed profile. opt_v_pts = 400 # How many points. opt_v_max = 1.2 opt_v_min = 0.8 opt_v_period_length = 60 # Period in meters. speed_ref = speed_profile.Speed() speed_ref.generate_sin(opt_v_min, opt_v_max, opt_v_period_length, opt_v_pts) speed_ref.repeating = True # vopt = speed_profile.Speed([1], [1]) # Controller reference path. x_radius = 1.4 y_radius = 1.2 center = [0.2, -y_radius / 2] pts = 400 variance = 0. plot_data = True save_data = True filename = 'measurements/sim_dmpc' + '_' + '_'.join(vehicle_ids) + '_' pt = path.Path() pt.gen_circle_path([x_radius, y_radius], points=pts, center=center) start_distance = 0.5 path_len = pt.get_path_length() vehicles = [] for i, vehicle_id in enumerate(vehicle_ids): theta = (len(vehicle_ids) - i - 1)*2*math.pi*start_distance/path_len + 0.1 xcoord = center[0] + x_radius*math.cos(theta) ycoord = center[1] + y_radius*math.sin(theta) x = [xcoord, ycoord, theta + math.pi/2, 0] # x = [center[0], center[1] + y_radius, math.pi, 0] # # x = [0, 0, math.pi, 0] vehicles.append(trxmodel.Trx(x=x, ID=vehicle_id)) mpc = DistributedMPC(vehicles, pt, Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, k_p, k_i, k_d, simulation_length, xmin=xmin, xmax=xmax, umin=umin, umax=umax, speed_ref=speed_ref, delay=delay, variance=variance) mpc.run() if save_data: mpc.save_data_as_rosbag(filename) if plot_data: mpc.plot_stuff()
def _unbrake(self, vehicle_index): """Undoes the braking of the vehicle. """ self.speed_profiles[vehicle_index] = speed_profile.Speed([1], [0])
def _brake(self, vehicle_index): """Brakes the vehicle. """ self.speed_profiles[vehicle_index] = speed_profile.Speed([1], [0])
def __init__(self, vehicles, vehicle_path, Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, k_p, k_i, k_d, simulation_length, xmin=None, xmax=None, umin=None, umax=None, speed_ref=None, delay=0., variance=0.): self.pwm_min = 1500 self.pwm_max = 1990 self.dt = delta_t self.iterations = int(simulation_length/self.dt) self.pt = vehicle_path self.timegap = timegap self.variance = variance # Optimal speed profile in space. If none given, optimal speed is 1 m/s everywhere. if speed_ref is None: speed_ref = speed_profile.Speed([1], [1]) self.original_speed_profile = speed_ref self.vehicles = vehicles self.n = len(vehicles) self.h = horizon # Matrices for storing control errors etc. self.timestamps = numpy.zeros((self.n, self.iterations)) self.xx = numpy.zeros((self.n, self.iterations)) self.yy = numpy.zeros((self.n, self.iterations)) self.yaws = numpy.zeros((self.n, self.iterations)) self.positions = numpy.zeros((self.n, self.iterations)) self.velocities = numpy.zeros((self.n, self.iterations)) self.velocity_references = numpy.zeros((self.n, self.iterations)) self.accelerations = numpy.zeros((self.n, self.iterations)) self.timegaps = numpy.zeros((self.n, self.iterations)) self.path_errors = numpy.zeros((self.n, self.iterations)) self.velocity_errors = numpy.zeros((self.n, self.iterations)) self.speed_inputs = numpy.zeros((self.n, self.iterations)) self.steering_inputs = numpy.zeros((self.n, self.iterations)) assumed_state_length = horizon + int(2.*timegap/delta_t) self.assumed_timestamps = numpy.zeros((self.n, assumed_state_length)) self.assumed_velocities = numpy.zeros((self.n, assumed_state_length)) self.assumed_positions = numpy.zeros((self.n, assumed_state_length)) # Safe past assumed states that will be used by follower vehicles, simulating a time delay. self.saved_num = int(math.ceil(delay/self.dt)) + 1 self.current_saved = 0 self.saved_assumed_t = numpy.zeros((self.saved_num, self.n, assumed_state_length)) self.saved_assumed_v = numpy.zeros((self.saved_num, self.n, assumed_state_length)) self.saved_assumed_s = numpy.zeros((self.saved_num, self.n, assumed_state_length)) self.frenets = [] # Path tracking. self.path_positions = [] # Longitudinal path positions. self.mpcs = [] self.speed_pwms = [] # Speed control signals. self.angle_pwms = [] # Wheel angle control signals. self.speed_profiles = [] # Initialize lists. for i, vehicle in enumerate(self.vehicles): x = vehicle.get_x() self.frenets.append(frenetpid.FrenetPID(vehicle_path, k_p, k_i, k_d)) self.path_positions.append(path.PathPosition(vehicle_path, [x[0], x[1]])) if i == 0: is_leader = True else: is_leader = False self.mpcs.append(solver_distributed_mpc.MPC(Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, xmin=xmin, xmax=xmax, umin=umin, umax=umax, is_leader=is_leader)) self.speed_pwms.append(1500) self.angle_pwms.append(1500) self.speed_profiles.append(speed_ref) self._order_follower_path_positions() for i in range(len(self.vehicles)): self.assumed_positions[i, :] = self.path_positions[i].get_position() self.saved_assumed_s[:] = self.assumed_positions self.k = 0 self.mpctime = 0
def main(args): if len(args) > 1: vehicle_ids = args[1:] else: print('Need to enter at least one vehicle ID. ') sys.exit() # Topic name for subscribing to truck positions. position_topic_name = 'mocap_state' # Topic name for publishing vehicle commands. control_topic_name = 'pwm_commands' # Name for starting and stopping recording. recording_service_name = 'cmpc/set_measurement' # Filename prefix for recording data. recording_filename = 'cmpc' + '_' + '_'.join(vehicle_ids) + '_' # PID parameters for path tracking. k_p = 0.5 k_i = 0 k_d = 3 horizon = 20 delta_t = 0.1 Ad = numpy.matrix([[1., 0.], [delta_t, 1.]]) Bd = numpy.matrix([[delta_t], [0.]]) zeta = 0.5 Q_v = 1 # Part of Q matrix for velocity tracking. Q_s = 1 # Part of Q matrix for position tracking. Q = numpy.array([Q_v, 0, 0, Q_s]).reshape(2, 2) # State tracking. R_acc = 0.1 R = numpy.array([1]) * R_acc # Input tracking. velocity_min = 0. velocity_max = 2. position_min = -100000. position_max = 1000000. acceleration_min = -0.5 acceleration_max = 0.5 truck_length = 0.3 safety_distance = 0.2 timegap = 1. delay = 0.2 xmin = numpy.array([velocity_min, position_min]) xmax = numpy.array([velocity_max, position_max]) umin = numpy.array([acceleration_min]) umax = numpy.array([acceleration_max]) # Reference speed profile. opt_v_pts = 400 # How many points. opt_v_max = 1.2 opt_v_min = 0.8 opt_v_period_length = 60 # Period in meters. vopt = speed_profile.Speed() vopt.generate_sin(opt_v_min, opt_v_max, opt_v_period_length, opt_v_pts) vopt.repeating = True # vopt = speed_profile.Speed([1], [1]) # Controller reference path. x_radius = 1.4 y_radius = 1.2 center = [0.2, -y_radius / 2] pts = 400 pt = path.Path() pt.gen_circle_path([x_radius, y_radius], points=pts, center=center) mpc = CentralizedMPC(position_topic_name, control_topic_name, vehicle_ids, pt, Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, k_p, k_i, k_d, xmin=xmin, xmax=xmax, umin=umin, umax=umax, speed_ref=vopt, recording_service_name=recording_service_name, recording_filename=recording_filename, delay=delay) mpc.run()
def _brake(self): """Brakes the vehicle specified by the index. """ self.speed_profile = speed_profile.Speed([1], [0])
def __init__(self, position_topic_name, control_topic_name, vehicle_ids, vehicle_path, Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, k_p, k_i, k_d, xmin=None, xmax=None, umin=None, umax=None, speed_ref=None, delay=0., recording_service_name='cmpc/set_measurement', recording_filename='cmpc'): rospy.init_node('centralized_mpc', anonymous=True) self.dt = delta_t self.vehicle_ids = vehicle_ids self.running = False # If controller is running or not. self.recording = False self.bag_filename_prefix = recording_filename self.recorder = helper.RosbagRecorder(recording_filename, '.bag') # Variables for starting phase. self.starting_phase_duration = 2. self.starting_phase = True self.starting_phase_start_time = 0 self.pwm_max = 1900 self.pwm_min = 1500 # Variables for printing information in terminal. self.verbose = True self.k = 0 print_interval = 1. # How often to print things in terminal. self.print_interval_samples = int(print_interval/self.dt) self.control_iteration_time_sum = 0 # Update rate. self.rate = rospy.Rate(1./self.dt) # Optimal speed profile in space. If none given, optimal speed is 1 m/s everywhere. if speed_ref is None: speed_ref = speed_profile.Speed([1], [1]) self.speed_profile = speed_ref self.original_speed_profile = speed_ref # Save old initial positions used by MPC and path tracking to simulate communication delay. self.saved_num = int(math.ceil(delay/self.dt)) + 1 self.delay_counter = 0 self.delayed_states = numpy.zeros((self.saved_num, len(self.vehicle_ids), 4)) # Centralized MPC solver. self.mpc = solver_centralized_mpc.MPC(len(self.vehicle_ids), Ad, Bd, self.dt, horizon, zeta, Q, R, truck_length, safety_distance, timegap, xmin, xmax, umin, umax) self.frenets = dict() # Path tracking. self.path_positions = dict() # Longitudinal path positions. self.poses = dict() # [x, y, yaw, v] self.speed_pwms = dict() # Speed control signals. self.angle_pwms = dict() # Wheel angle control signals. self.gears = dict() self.first_callbacks = dict() # If having received first position data in initialization. # Initialize dictionaries. for vehicle_id in self.vehicle_ids: self.frenets[vehicle_id] = frenetpid.FrenetPID(vehicle_path, k_p, k_i, k_d) self.path_positions[vehicle_id] = path.PathPosition(vehicle_path) self.poses[vehicle_id] = [0, 0, 0, 0] self.speed_pwms[vehicle_id] = 1500 self.angle_pwms[vehicle_id] = 1500 self.gears[vehicle_id] = 0 self.first_callbacks[vehicle_id] = True # Publisher for controlling vehicles. self.pwm_publisher = rospy.Publisher(control_topic_name, PWM, queue_size=1) # Subscriber for vehicle positions. rospy.Subscriber(position_topic_name, MocapState, self._position_callback) # Subscriber for starting and stopping the controller. rospy.Subscriber('global/run', ControllerRun, self._start_stop_callback) # Service for starting or stopping recording. rospy.Service(recording_service_name, SetMeasurement, self.set_measurement) # Service for custom commands. rospy.Service('cmpc/command', Command, self._command_callback) print('\nCentralized MPC initialized. Vehicles: {}.\n'.format(self.vehicle_ids))
def main(args): # ID of the vehicle. if len(args) < 2: print('Need to enter at least one vehicle ID.') sys.exit() vehicle_ids = args[1:] # Topic information for subscribing to truck positions. position_topic_name = 'mocap_state' position_topic_type = MocapState # Topic information for publishing vehicle commands. control_topic_name = 'pwm_commands' control_topic_type = PWM scale = 1 # Data for controller reference path. x_radius = 1.7 * scale y_radius = 1.2 * scale center = [0, -y_radius] pts = 400 # PID parameters. # 0.0001 k_p = 0.00003 k_i = -0.02 * 0 k_d = 0.020 if scale == 1: k_p = 0.5 k_i = 0 k_d = 3 horizon = 5 delta_t = 0.1 Ad = numpy.matrix([[1., 0.], [delta_t, 1.]]) Bd = numpy.matrix([[delta_t], [0.]]) zeta = 0.75 s0 = 0. v0 = 1. Q_v = 1 # Part of Q matrix for velocity tracking. Q_s = 0.5 # Part of Q matrix for position tracking. Q = numpy.array([Q_v, 0, 0, Q_s]).reshape(2, 2) # State tracking. R_acc = 0.1 R = numpy.array([1]) * R_acc # Input tracking. v_min = 0. v_max = 2. s_min = 0. s_max = 1000000 acc_min = -0.5 acc_max = 0.5 truck_length = 0.2 safety_distance = 0.1 timegap = 1. saved_h = 2 x0 = numpy.array([s0, v0]) xmin = numpy.array([v_min, s_min]) xmax = numpy.array([v_max, s_max]) umin = numpy.array([acc_min]) umax = numpy.array([acc_max]) # Reference speed profile. opt_v_pts = 1000 opt_v_max = 1.2 opt_v_min = 0.8 opt_v_period_length = 40 vopt = speed_profile.Speed() vopt.generate_sin(opt_v_min, opt_v_max, opt_v_period_length, opt_v_pts) vopt.repeating = True pt = path.Path() pt.gen_circle_path([x_radius, y_radius], points=pts, center=center) # Initialize controller. controller = Controller(position_topic_type, position_topic_name, control_topic_type, control_topic_name, vehicle_ids, pt, Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, xmin=xmin, xmax=xmax, umin=umin, umax=umax, x0=x0, saved_h=saved_h, k_p=k_p, k_i=k_i, k_d=k_d) controller.set_vopt(vopt) # Start controller. controllerGUI.ControllerGUI(controller)
def __init__(self, position_topic_type, position_topic_name, control_topic_type, control_topic_name, vehicle_ids, vehicle_path, Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, xmin=None, xmax=None, umin=None, umax=None, x0=None, saved_h=2, k_p=0., k_i=0., k_d=0.): self.verbose = True self.do_startup = False self.vehicle_ids = vehicle_ids self.running = False # If controller is running or not. self.gear2_pwm = 120 self.dt = delta_t self.headstart_samples = int(1. / self.dt) self.vopt = speed_profile.Speed() self.truck_length = truck_length self.print_interval = round(1. / self.dt) self.k = 0 self.last_print_time = time.time() # Setup ROS node. rospy.init_node('platoon_controller', anonymous=True) # Publisher for controlling vehicle. self.pwm_publisher = rospy.Publisher(control_topic_name, control_topic_type, queue_size=1) # Subscriber for vehicle positions. rospy.Subscriber(position_topic_name, position_topic_type, self._callback) # Reference path. self.pt = vehicle_path self.frenets = dict( ) # Frenet path tracking controllers for each vehicle. self.positions = dict() # Stores vehicle positions [x, y, yaw, v]. self.mpcs = dict() # MPC controllers. self.path_positions = dict( ) # For keeping track of longitudinal path positions. self.speed_pwms = dict() # Stores speed PWM for each vehicle. for i, vehicle_id in enumerate(self.vehicle_ids): self.frenets[vehicle_id] = frenetpid.FrenetPID( self.pt, k_p, k_i, k_d) self.positions[vehicle_id] = [0, 0, 0, 0] # The first vehicle entered is set to be the leader. if i == 0: leader = True else: leader = False self.mpcs[vehicle_id] = old_mpc_solver.TruckMPC( Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, xmin=xmin, xmax=xmax, umin=umin, umax=umax, x0=x0, vehicle_id=vehicle_id, saved_h=saved_h, is_leader=leader) self.path_positions[vehicle_id] = path.PathPosition(self.pt) self.speed_pwms[vehicle_id] = 1500 print('\nController initialized. Vehicles {}.\n'.format( self.vehicle_ids))
def _brake(self): """Brakes all vehicles. """ self.speed_profile = speed_profile.Speed([1], [0])
def __init__(self, vehicles, vehicle_path, Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, k_p, k_i, k_d, simulation_length, xmin=None, xmax=None, umin=None, umax=None, speed_ref=None, delay=0., variance=0.): self.dt = delta_t self.h = horizon self.iterations = int(simulation_length / self.dt) self.pt = vehicle_path self.variance = variance self.timegap = timegap # Optimal speed profile in space. If none given, optimal speed is 1 m/s everywhere. if speed_ref is None: speed_ref = speed_profile.Speed([1], [1]) self.speed_profile = speed_ref self.original_speed_profile = speed_ref self.pwm_max = 1990 self.pwm_min = 1500 self.mpc = solver_centralized_mpc.MPC(len(vehicles), Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, xmin, xmax, umin, umax) self.vehicles = vehicles self.n = len(vehicles) # Matrices for storing control errors etc. self.timestamps = numpy.kron(numpy.ones((self.n, 1)), self.dt * numpy.arange(self.iterations)) self.xx = numpy.zeros((self.n, self.iterations)) self.yy = numpy.zeros((self.n, self.iterations)) self.yaws = numpy.zeros((self.n, self.iterations)) self.positions = numpy.zeros((self.n, self.iterations)) self.velocities = numpy.zeros((self.n, self.iterations)) self.velocity_references = numpy.zeros((self.n, self.iterations)) self.accelerations = numpy.zeros((self.n, self.iterations)) self.timegaps = numpy.zeros((self.n, self.iterations)) self.path_errors = numpy.zeros((self.n, self.iterations)) self.velocity_errors = numpy.zeros((self.n, self.iterations)) self.speed_inputs = numpy.zeros((self.n, self.iterations)) self.steering_inputs = numpy.zeros((self.n, self.iterations)) # Save old initial positions used by MPC and path tracking to simulate communication delay. self.saved_num = int(math.ceil(delay / self.dt)) + 1 self.delay_counter = 0 self.delayed_states = numpy.zeros( (self.saved_num, len(self.vehicles), 4)) self.frenets = [] # Path tracking. self.path_positions = [] # Longitudinal path positions. self.speed_pwms = [] # Speed control signals. self.angle_pwms = [] # Wheel angle control signals. # Initialize lists. for i, vehicle in enumerate(self.vehicles): x = vehicle.get_x() self.frenets.append( frenetpid.FrenetPID(vehicle_path, k_p, k_i, k_d)) self.path_positions.append( path.PathPosition(vehicle_path, [x[0], x[1]])) self.speed_pwms.append(1500) self.angle_pwms.append(1500) self.delayed_states[:, i, :] = x self._order_follower_path_positions() self.k = 0 self.mpctime = 0
def main(args): # ID of the vehicle. if len(args) < 2: print('Need to enter at least one vehicle ID.') sys.exit() vehicle_ids = ['/' + v_id for v_id in args[1:]] horizon = 10 delta_t = 0.1 Ad = numpy.array([1., 0., delta_t, 1.]).reshape(2, 2) Bd = numpy.array([delta_t, 0.]).reshape(2, 1) zeta = 0.75 Q_v = 1 # Part of Q matrix for velocity tracking. Q_s = 0.5 # Part of Q matrix for position tracking. Q = numpy.array([Q_v, 0., 0., Q_s]).reshape(2, 2) # State tracking. R_acc = 0.1 R = numpy.array([1]) * R_acc # Input tracking. v_min = 0. v_max = 2. s_min = 0. s_max = 1000000. acc_min = -0.5 acc_max = 0.5 truck_length = 0.2 safety_distance = 0.1 timegap = 1 saved_h = 2 runs = 200 xmin = numpy.array([v_min, s_min]) xmax = numpy.array([v_max, s_max]) umin = numpy.array([acc_min]) umax = numpy.array([acc_max]) x_radius = 1.7 y_radius = 1.2 center = [0, -y_radius] pts = 400 # Reference speed profile. opt_v_pts = 1000 opt_v_max = 1.3 opt_v_min = 0.7 opt_v_period_length = 20 vopt = speed_profile.Speed() vopt.generate_sin(opt_v_min, opt_v_max, opt_v_period_length, opt_v_pts) vopt.repeating = True pt = path.Path() pt.gen_circle_path([x_radius, y_radius], points=pts, center=center) kp = 0.5 ki = -0.02 kd = 3 verbose = False print_graphs = True start_distance = 0.75 path_len = pt.get_path_length() startup_time = 0.5 vehicles = [None for i in vehicle_ids] for i in range(len(vehicle_ids)): theta = (len(vehicle_ids) - i - 1)*2*math.pi*start_distance/path_len + 0.1 theta = 0.1 x = center[0] + x_radius*math.cos(theta) y = center[1] + y_radius*math.sin(theta) v = 0 vehicles[i] = FrenetUnicycle( pt, x=[x, y, theta + math.pi/2, v], u=[0, 0], kp=kp, ki=ki, kd=kd, ID=vehicle_ids[i]) sim = SimMPC(vehicles, Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, xmin=xmin, xmax=xmax, umin=umin, umax=umax, runs=runs, saved_h=saved_h, startup_time=startup_time) sim.set_vopt(vopt) sim.set_verbose(verbose) sim.run() if print_graphs: sim.print_graps()