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()
