diff --git a/docker/action/model_predictive_control/model_predictive_control.Dockerfile b/docker/action/model_predictive_control/model_predictive_control.Dockerfile index a10d3c59..51d0d0ad 100644 --- a/docker/action/model_predictive_control/model_predictive_control.Dockerfile +++ b/docker/action/model_predictive_control/model_predictive_control.Dockerfile @@ -6,7 +6,7 @@ FROM ${BASE_IMAGE} as source WORKDIR ${AMENT_WS}/src # Copy in source code -COPY src/action/model_predictive_control model_predictive_control +COPY src/action/local_planning local_planning COPY src/wato_msgs/sample_msgs sample_msgs # Scan for rosdeps diff --git a/modules/dev_overrides/docker-compose.action.yaml b/modules/dev_overrides/docker-compose.action.yaml index 1984e6db..6cfbc0fd 100644 --- a/modules/dev_overrides/docker-compose.action.yaml +++ b/modules/dev_overrides/docker-compose.action.yaml @@ -36,11 +36,10 @@ services: - ${MONO_DIR}/src/action/local_planning:/home/ament_ws/src/local_planning model_predictive_control: - <<: *fixuid extends: file: ../docker-compose.action.yaml service: model_predictive_control image: "${ACTION_MPC_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/action/model_predictive_control:/home/ament_ws/src/model_predictive_control \ No newline at end of file + - ${MONO_DIR}/src/action/model_predictive_control:/home/bolty/model_predictive_control \ No newline at end of file diff --git a/modules/docker-compose.action.yaml b/modules/docker-compose.action.yaml index ab73667e..47efc9f7 100644 --- a/modules/docker-compose.action.yaml +++ b/modules/docker-compose.action.yaml @@ -41,6 +41,5 @@ services: cache_from: - "${ACTION_MPC_IMAGE}:build_${TAG}" - "${ACTION_MPC_IMAGE}:build_main" - target: deploy image: "${ACTION_MPC_IMAGE}:${TAG}" command: /bin/bash -c "ros2 launch model_predictive_control model_predictive_control.launch.py" diff --git a/modules/docker-compose.simulation.yaml b/modules/docker-compose.simulation.yaml index 241f4567..20b7fc3b 100644 --- a/modules/docker-compose.simulation.yaml +++ b/modules/docker-compose.simulation.yaml @@ -5,11 +5,16 @@ services: image: carlasim/carla:0.9.13 environment: - DISPLAY=1 - - CUDA_VISIBLE_DEVICES=0,1,2 - - NVIDIA_VISIBLE_DEVICES=0,1,2 - runtime: nvidia + - CUDA_VISIBLE_DEVICES=0 + - NVIDIA_VISIBLE_DEVICES=0 restart: always command: /bin/bash -c "./CarlaUE4.sh -nosound -carla-server -RenderOffscreen -world-port=2000 -quality-level=Low" + deploy: + resources: + reservations: + devices: + - driver: nvidia + capabilities: [gpu] carla_ros_bridge: build: @@ -70,7 +75,7 @@ services: - SCENARIO_RUNNER_ROOT=/home/bolty/scenario_runner - DISPLAY=1 - CUDA_VISIBLE_DEVICES=0 - - NVIDIA_VISIBLE_DEVICES=1 + - NVIDIA_VISIBLE_DEVICES=0 container_name: ${COMPOSE_PROJECT_NAME:?}_carla_notebooks volumes: - ${MONO_DIR}/src/simulation/carla_notebooks:/home/bolty/carla_notebooks diff --git a/src/action/model_predictive_control/config/params.yaml b/src/action/model_predictive_control/config/params.yaml new file mode 100644 index 00000000..e69de29b diff --git a/src/action/model_predictive_control/launch/mpc_launch.py b/src/action/model_predictive_control/launch/mpc_launch.py new file mode 100644 index 00000000..e69de29b diff --git a/src/action/model_predictive_control/model_predictive_control/boxconstraint.py b/src/action/model_predictive_control/model_predictive_control/boxconstraint.py new file mode 100644 index 00000000..9cfd33bb --- /dev/null +++ b/src/action/model_predictive_control/model_predictive_control/boxconstraint.py @@ -0,0 +1,70 @@ +import numpy as np +import torch + + +class BoxConstraint: + """ + Bounded constraints lb <= x <= ub as polytopic constraints -Ix <= -b and Ix <= b. np.vstack(-I, I) forms the H matrix from III-D-b of the paper + """ + def __init__(self, lb=None, ub=None, plot_idxs=None): + """ + :param lb: dimwise list of lower bounds. + :param ub: dimwise list of lower bounds. + :param plot_idxs: When plotting, the box itself might be defined in some dimension greater than 2 but we might only want to + plot the workspace variables and so plot_idxs allows us to limit the consideration of plot_constraint_set to those variables. + """ + self.lb = np.array(lb, ndmin=2).reshape(-1, 1) + self.ub = np.array(ub, ndmin=2).reshape(-1, 1) + self.plot_idxs = plot_idxs + self.dim = self.lb.shape[0] + assert (self.lb < self.ub).all(), "Lower bounds must be greater than corresponding upper bound for any given dimension" + self.setup_constraint_matrix() + + def __str__(self): return "Lower bound: %s, Upper bound: %s" % (self.lb, self.ub) + + def get_random_vectors(self, num_samples): + rand_samples = np.random.rand(self.dim, num_samples) + for i in range(self.dim): + scale_factor, shift_factor = (self.ub[i] - self.lb[i]), self.lb[i] + rand_samples[i, :] = (rand_samples[i, :] * scale_factor) + shift_factor + return rand_samples + + def setup_constraint_matrix(self): + dim = self.lb.shape[0] + # Casadi can't do matrix mult with Torch instances but only numpy instead. So have to use the np version of the H and b matrix/vector when + # defining constraints in the opti stack. + self.H_np = np.vstack((-np.eye(dim), np.eye(dim))) + self.H = torch.Tensor(self.H_np) + # self.b = torch.Tensor(np.hstack((-self.lb, self.ub))) + self.b_np = np.vstack((-self.lb, self.ub)) + self.b = torch.Tensor(self.b_np) + # print(self.b) + self.sym_func = lambda x: self.H @ np.array(x, ndmin=2).T - self.b + + def check_satisfaction(self, sample): + # If sample is within the polytope defined by the constraints return 1 else 0. + # print(sample, np.array(sample, ndmin=2).T, self.sym_func(sample), self.b) + return (self.sym_func(sample) <= 0).all() + + def generate_uniform_samples(self, num_samples): + n = int(np.round(num_samples**(1./self.lb.shape[0]))) + + # Generate a 1D array of n equally spaced values between the lower and upper bounds for each dimension + coords = [] + for i in range(self.lb.shape[0]): + coords.append(np.linspace(self.lb[i, 0], self.ub[i, 0], n)) + + # Create a meshgrid of all possible combinations of the n-dimensions + meshes = np.meshgrid(*coords, indexing='ij') + + # Flatten the meshgrid and stack the coordinates to create an array of size (K, n-dimensions) + samples = np.vstack([m.flatten() for m in meshes]) + + # Truncate the array to K samples + samples = samples[:num_samples, :] + + # Print the resulting array + return samples + + def clip_to_bounds(self, samples): + return np.clip(samples, self.lb, self.ub) \ No newline at end of file diff --git a/src/action/model_predictive_control/model_predictive_control/carla_core.py b/src/action/model_predictive_control/model_predictive_control/carla_core.py new file mode 100644 index 00000000..afb6fe20 --- /dev/null +++ b/src/action/model_predictive_control/model_predictive_control/carla_core.py @@ -0,0 +1,165 @@ +import carla +import numpy as np +import datetime +import os +import shutil + +SIM_DURATION = 500 # Simulation duration in time steps in Carla +TIME_STEP = 0.05 +PREDICTION_HORIZON = 2.0 + +class CarlaCore: + def __init__(self, publish_state): + self.publish_state = publish_state + + self.spectator = None + self.vehicles = None + self.spawn_point = None + + self.setup_carla() + + self.vehicle_state = { + 'x': 0.0, + 'y': 0.0, + 'theta': 0.0, + 'velocity': 0.0, + 'iteration': 0 + } + + self.waypoints = [] + + def setup_carla(self): + ## SETUP ## + # Connect to CARLA + client = carla.Client('localhost', 2000) + maps = [m.replace('/Game/Carla/Maps/', '') for m in client.get_available_maps()] + print('Available maps: ', maps) + world = client.get_world() + mymap = world.get_map() + print('Using map: ', mymap.name) + self.spectator = world.get_spectator() + + # CARLA Settings + settings = world.get_settings() + # Timing settings + settings.synchronous_mode = True # Enables synchronous mode + TIME_STEP = 0.05 # Time step for synchronous mode + settings.fixed_delta_seconds = TIME_STEP + # Physics substep settings + settings.substepping = True + settings.max_substep_delta_time = 0.01 + settings.max_substeps = 10 + + world.apply_settings(settings) + + # Output client and world objects to console + print(client) + print(world) + + # Use recommended spawn points + self.spawn_points = mymap.get_spawn_points() + self.spawn_point = spawn_points[0] + + # Spawn vehicle + self.vehicles = world.get_actors().filter('vehicle.*') + blueprint_library = world.get_blueprint_library() + vehicle_bp = blueprint_library.filter('model3')[0] + print("Vehicle blueprint attributes:") + for attr in vehicle_bp: + print(' - {}'.format(attr)) + + if len(vehicles) == 0: + vehicle = world.spawn_actor(vehicle_bp, spawn_point) + else: + # Reset world + for vehicle in vehicles: + vehicle.destroy() + vehicle = world.spawn_actor(vehicle_bp, spawn_point) + print(vehicle) + + + def get_waypoints(self): + """Generate and return the list of waypoints relative to the vehicle's spawn point.""" + + for i in range(SIM_DURATION): + self.waypoints.append(self.generate_waypoint_relative_to_spawn(-10, 0)) + + return self.waypoints + + def generate_waypoint_relative_to_spawn(self, forward_offset=0, sideways_offset=0): + waypoint_x = spawn_point.location.x + spawn_point.get_forward_vector().x * forward_offset + spawn_point.get_right_vector().x * sideways_offset + waypoint_y = spawn_point.location.y + spawn_point.get_forward_vector().y * forward_offset + spawn_point.get_right_vector().y * sideways_offset + return [waypoint_x, waypoint_y] + + def get_vehicle_state(self): + """ + Retrieves the current state of the vehicle in the CARLA world. + The state includes position, orientation (theta), velocity, and iteration count. + """ + + # Fetch initial state from CARLA + x0 = vehicle.get_transform().location.x + y0 = vehicle.get_transform().location.y + theta0 = vehicle.get_transform().rotation.yaw / 180 * ca.pi + velocity_vector = vehicle.get_velocity() + v0 = ca.sqrt(velocity_vector.x ** 2 + velocity_vector.y ** 2) + + # Update vehicle state + self.vehicle_state['x'] = x0 + self.vehicle_state['y'] = y0 + self.vehicle_state['theta'] = theta0 + self.vehicle_state['velocity'] = v0 + + return self.vehicle_state + + def apply_control(self, steering_angle, throttle): + """ + Applies the received control commands to the vehicle. + """ + if throttle < 0: + self.vehicle.apply_control(carla.VehicleControl(throttle=-throttle, steer=steering_angle, reverse=True)) + else: + self.vehicle.apply_control(carla.VehicleControl(throttle=throttle, steer=steering_angle)) + + def start_main_loop(self): + """ + Main loop to continuously publish vehicle states and waypoints. + """ + N = int(PREDICTION_HORIZON/TIME_STEP) + + for i in range(SIM_DURATION - N): # Subtract N since we need to be able to predict N steps into the future + print("Iteration: ", i) + + self.vehicle_state['iteration'] = i + + move_spectator_to_vehicle() + + # Draw current waypoints in CARLA + for waypoint in self.waypoints[i:i + N]: + waypoint_x = float(np.array(waypoint[0])) + waypoint_y = float(np.array(waypoint[1])) + + carla_waypoint = carla.Location(x=waypoint_x, y=waypoint_y, z=0.5) + + extent = carla.Location(x=0.5, y=0.5, z=0.5) + world.debug.draw_box(box=carla.BoundingBox(carla_waypoint, extent * 1e-2), + rotation=carla.Rotation(pitch=0, yaw=0, roll=0), life_time=TIME_STEP * 10, thickness=0.5, + color=carla.Color(255, 0, 0)) + + self.publish_state() # MPC Should Start after this !!!! + + # Should automatically apply control after publishing state to mpc + + print("") + world.tick() # Tick the CARLA world + + + def move_spectator_to_vehicle(distance=10): + vehicle_location = vehicle.get_location() + # Set viewing angle to slightly above the vehicle + spectator_transform = carla.Transform(vehicle_location + carla.Location(z=distance), carla.Rotation(pitch=-90)) + spectator.set_transform(spectator_transform) + + def publish_state(self): + """To be overridden by CarlaNode for publishing the vehicle state.""" + pass \ No newline at end of file diff --git a/src/action/model_predictive_control/model_predictive_control/carla_node.py b/src/action/model_predictive_control/model_predictive_control/carla_node.py new file mode 100644 index 00000000..6db285c5 --- /dev/null +++ b/src/action/model_predictive_control/model_predictive_control/carla_node.py @@ -0,0 +1,80 @@ +import rclpy +from rclpy.node import Node + +from action_msgs import ControlCommands, VehicleState, WaypointArray +from carla_core import CarlaCore # Import the CARLA core logic + + +class CarlaNode(Node): + def __init__(self): + super().__init__('CarlaNode') + + # Initialize CARLA core object (manages CARLA setup and running) + self.carla_core = CarlaCore(self.publish_state) + + # Publishers + self.vehicle_state_publisher = self.create_publisher(VehicleState, '/carla/vehicle_state', 10) + self.waypoints_publisher = self.create_publisher(WaypointArray, '/carla/waypoints', 10) + + # Subscribers + self.control_subscription = self.create_subscription( + ControlCommands, '/mpc/control_commands', self.control_callback, 10) + + + self.carla_core.start_main_loop() + + + def publish_state(self): + # Get the current vehicle state from carla_core + vehicle_state = self.carla_core.get_vehicle_state() + if vehicle_state: + # Publish the vehicle state to MPC node + state_msg = state_msgs() + state_msg.pos_x = vehicle_state['x'] + state_msg.pos_y = vehicle_state['y'] + state_msg.angle = vehicle_state['theta'] + state_msg.velocity = vehicle_state['velocity'] + self.vehicle_state_publisher.publish(state_msg) + + def publish_waypoints(self): + # Get the current waypoints from carla_core + waypoints = self.carla_core.get_waypoints() + if waypoints: + # Create an instance of WaypointArray message + waypoint_array_msg = WaypointArray() + + for wp in waypoints: + # Create a Waypoint message for each waypoint in the list + waypoint_msg = Waypoint() + waypoint_msg.x = wp[0] # x-coordinate + waypoint_msg.y = wp[1] # y-coordinate + + # Append each Waypoint message to the WaypointArray message + waypoint_array_msg.waypoints.append(waypoint_msg) + + # Publish the WaypointArray message + self.waypoints_publisher.publish(waypoint_array_msg) + + def control_callback(self, msg): + """ + This function will be called when a control message is received from the MPC Node. + It will forward the control commands to the carla_core. + """ + # Extract steering and throttle from the message + steering_angle = msg.steering_angle + throttle = msg.throttle + + # Send control commands to CARLA via carla_core + self.carla_core.apply_control(steering_angle, throttle) + + +def main(args=None): + rclpy.init(args=args) + carla_node = CarlaNode() + rclpy.spin(carla_node) + carla_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/action/model_predictive_control/model_predictive_control/mpc.py b/src/action/model_predictive_control/model_predictive_control/mpc.py new file mode 100644 index 00000000..837a9ac2 --- /dev/null +++ b/src/action/model_predictive_control/model_predictive_control/mpc.py @@ -0,0 +1,283 @@ +import carla +import casadi as ca +import numpy as np +import datetime +import os +import shutil + +from boxconstraint import BoxConstraint + +SIM_DURATION = 500 # Simulation duration in time steps + +## SETUP ## +# Connect to CARLA +client = carla.Client('localhost', 2000) +maps = [m.replace('/Game/Carla/Maps/', '') for m in client.get_available_maps()] +print('Available maps: ', maps) +world = client.get_world() +mymap = world.get_map() +print('Using map: ', mymap.name) +spectator = world.get_spectator() + +# CARLA Settings +settings = world.get_settings() +# Timing settings +settings.synchronous_mode = True # Enables synchronous mode +TIME_STEP = 0.05 # Time step for synchronous mode +settings.fixed_delta_seconds = TIME_STEP +# Physics substep settings +settings.substepping = True +settings.max_substep_delta_time = 0.01 +settings.max_substeps = 10 + +world.apply_settings(settings) + +# Output client and world objects to console +print(client) +print(world) + + +# Function to move the spectator camera +def move_spectator_to_vehicle(vehicle, spectator, distance=10): + vehicle_location = vehicle.get_location() + # Set viewing angle to slightly above the vehicle + spectator_transform = carla.Transform(vehicle_location + carla.Location(z=distance), carla.Rotation(pitch=-90)) + spectator.set_transform(spectator_transform) + + +# Use recommended spawn points +spawn_points = mymap.get_spawn_points() +spawn_point = spawn_points[0] + +# Spawn vehicle +vehicles = world.get_actors().filter('vehicle.*') +blueprint_library = world.get_blueprint_library() +vehicle_bp = blueprint_library.filter('model3')[0] +print("Vehicle blueprint attributes:") +for attr in vehicle_bp: + print(' - {}'.format(attr)) + +if len(vehicles) == 0: + vehicle = world.spawn_actor(vehicle_bp, spawn_point) +else: + # Reset world + for vehicle in vehicles: + vehicle.destroy() + vehicle = world.spawn_actor(vehicle_bp, spawn_point) +print(vehicle) + + +def generate_waypoint_relative_to_spawn(forward_offset=0, sideways_offset=0): + waypoint_x = spawn_point.location.x + spawn_point.get_forward_vector().x * forward_offset + spawn_point.get_right_vector().x * sideways_offset + waypoint_y = spawn_point.location.y + spawn_point.get_forward_vector().y * forward_offset + spawn_point.get_right_vector().y * sideways_offset + return ca.vertcat(waypoint_x, waypoint_y) + + +def generate_waypoint(x, y): + return ca.vertcat(x, y) + + +waypoints = [] + +for i in range(SIM_DURATION): + waypoints.append(generate_waypoint_relative_to_spawn(-10, 0)) + +# Parameters +params = { + 'L': 2.875 # Wheelbase of the vehicle. Source : https://www.tesla.com/ownersmanual/model3/en_us/GUID-56562137-FC31-4110-A13C-9A9FC6657BF0.html +} +T = 2.0 # Prediction horizon in seconds +N = int(T / TIME_STEP) # Prediction horizon in time steps +dt = TIME_STEP # Time step for discretization +state_dim = 4 # Dimension of the state [x, y, theta, v] +control_dim = 2 # Dimension of the control input [steering angle, acceleration] + +# Initialize Opti object +opti = ca.Opti() + +# Declare variables +X = opti.variable(state_dim, N + 1) # state trajectory variables over prediction horizon +U = opti.variable(control_dim, N) # control trajectory variables over prediction horizon +P = opti.parameter(state_dim) # initial state parameter +Q_base = ca.MX.eye(state_dim) # Base state penalty matrix (emphasizes position states) +weight_increase_factor = 1.00 # Increase factor for each step in the prediction horizon +R = ca.MX.eye(control_dim) # control penalty matrix for objective function +W = opti.parameter(2, N) # Reference trajectory parameter + +# Objective +obj = 0 +for k in range(N): + Q = Q_base * (weight_increase_factor ** k) # Increase weight for each step in the prediction horizon + + x_k = X[:, k] # Current state + u_k = U[:, k] # Current control input + x_next = X[:, k + 1] # Next state + + x_ref = ca.vertcat(W[:, k], + ca.MX.zeros(state_dim - 2, 1)) # Reference state with waypoint and zero for other states + + dx = x_k - x_ref # Deviation of state from reference state + du = u_k # Control input deviation (assuming a desired control input of zero) + + # Quadratic cost with reference state and control input + obj += ca.mtimes([ca.mtimes(dx.T, Q), dx]) + ca.mtimes( + [ca.mtimes(du.T, R), du]) # Minimize quadratic cost and deviation from reference state + +opti.minimize(obj) + +# Maximum steerin angle for dynamics +max_steering_angle_deg = max(wheel.max_steer_angle for wheel in + vehicle.get_physics_control().wheels) # Maximum steering angle in degrees (from vehicle physics control +max_steering_angle_rad = max_steering_angle_deg * (ca.pi / 180) # Maximum steering angle in radians + +# Dynamics (Euler discretization using bicycle model) +for k in range(N): + steering_angle_rad = U[0, k] * max_steering_angle_rad # Convert normalized steering angle to radians + + opti.subject_to(X[:, k + 1] == X[:, k] + dt * ca.vertcat( + X[3, k] * ca.cos(X[2, k]), + X[3, k] * ca.sin(X[2, k]), + (X[3, k] / params['L']) * ca.tan(steering_angle_rad), + U[1, k] + )) + +# Constraints +opti.subject_to(X[:, 0] == P) # Initial state constraint + +# Input constraints +steering_angle_bounds = [-1.0, 1.0] +acceleration_bounds = [-1.0, 1.0] +lb = np.array([steering_angle_bounds[0], acceleration_bounds[0]]).reshape(-1, 1) +ub = np.array([steering_angle_bounds[1], acceleration_bounds[1]]).reshape(-1, 1) +action_space = BoxConstraint(lb=lb, ub=ub) + +# State constraints +# x_bounds = [-10000, 1000] # x position bounds (effectively no bounds) +# y_bounds = [-1000, 1000] # y position bounds (effectively no bounds) +# theta_bounds = [0, 360] # theta bounds in degrees +# v_bounds = [-10, 10] # velocity bounds +# lb = np.array([x_bounds[0], y_bounds[0], theta_bounds[0], v_bounds[0]]).reshape(-1, 1) +# ub = np.array([x_bounds[1], y_bounds[1], theta_bounds[1], v_bounds[1]]).reshape(-1, 1) +# state_space = BoxConstraint(lb=lb, ub=ub) + +# Apply constraints to optimization problem +for i in range(N): + # Input constraints + opti.subject_to(action_space.H_np @ U[:, i] <= action_space.b_np) + + # State constraints + # opti.subject_to(state_space.H_np @ X[:, i] <= state_space.b_np) + +# Setup solver +acceptable_dual_inf_tol = 1e11 +acceptable_compl_inf_tol = 1e-3 +acceptable_iter = 15 +acceptable_constr_viol_tol = 1e-3 +acceptable_tol = 1e-6 + +opts = {"ipopt.acceptable_tol": acceptable_tol, + "ipopt.acceptable_constr_viol_tol": acceptable_constr_viol_tol, + "ipopt.acceptable_dual_inf_tol": acceptable_dual_inf_tol, + "ipopt.acceptable_iter": acceptable_iter, + "ipopt.acceptable_compl_inf_tol": acceptable_compl_inf_tol, + "ipopt.hessian_approximation": "limited-memory", + "ipopt.print_level": 0} +opti.solver('ipopt', opts) + +# Array to store closed-loop trajectory states (X and Y coordinates) +closed_loop_data = [] +open_loop_data = [] +residuals_data = [] + +# Initialize warm-start parameters +prev_sol_x = None +prev_sol_u = None + +# Main Loop +for i in range(SIM_DURATION - N): # Subtract N since we need to be able to predict N steps into the future + print("Iteration: ", i) + + move_spectator_to_vehicle(vehicle, spectator) + + # Draw current waypoints in CARLA + for waypoint in waypoints[i:i + N]: + waypoint_x = float(np.array(waypoint[0])) + waypoint_y = float(np.array(waypoint[1])) + + carla_waypoint = carla.Location(x=waypoint_x, y=waypoint_y, z=0.5) + + extent = carla.Location(x=0.5, y=0.5, z=0.5) + world.debug.draw_box(box=carla.BoundingBox(carla_waypoint, extent * 1e-2), + rotation=carla.Rotation(pitch=0, yaw=0, roll=0), life_time=TIME_STEP * 10, thickness=0.5, + color=carla.Color(255, 0, 0)) + + # Fetch initial state from CARLA + x0 = vehicle.get_transform().location.x + y0 = vehicle.get_transform().location.y + theta0 = vehicle.get_transform().rotation.yaw / 180 * ca.pi + velocity_vector = vehicle.get_velocity() + v0 = ca.sqrt(velocity_vector.x ** 2 + velocity_vector.y ** 2) + + print("Current x: ", x0) + print("Current y: ", y0) + print("Current yaw: ", vehicle.get_transform().rotation.yaw) + print("Current theta: ", theta0) + print("Current velocity: ", v0) + + # Store current state in the closed-loop trajectory data + if i > 0: + closed_loop_data.append([x0, y0, theta0, v0]) + + # Set initial state for optimization problem + initial_state = ca.vertcat(x0, y0, theta0, v0) + opti.set_value(P, initial_state) + + # Set the reference trajectory for the current iteration + opti.set_value(W, ca.horzcat(*waypoints[i:i + N])) # Concatenate waypoints + + if prev_sol_x is not None and prev_sol_u is not None: + # Warm-starting the solver with the previous solution + opti.set_initial(X, prev_sol_x) + opti.set_initial(U, prev_sol_u) + + # Solve the optimization problem + sol = opti.solve() + + # If the solver is successful, apply the first control input to the vehicle + if sol.stats()['success']: + u = sol.value(U[:, 0]) + + # Bound acceleration and steering angle to [-1, 1] + u[0] = np.clip(u[0], -1, 1) + u[1] = np.clip(u[1], -1, 1) + + print("Steering angle: ", u[0]) + print("Acceleration: ", u[1]) + + if u[1] < 0: + vehicle.apply_control(carla.VehicleControl(throttle=-u[1], steer=u[0], reverse=True)) + else: + vehicle.apply_control(carla.VehicleControl(throttle=u[1], steer=u[0])) + + # Store open-loop trajectory data with control input applied to vehicle + open_loop_trajectory = sol.value(X) + open_loop_trajectory = open_loop_trajectory.T.reshape(-1, state_dim) + open_loop_trajectory = np.hstack((open_loop_trajectory, np.tile(u, (N + 1, 1)))) + open_loop_data.append(open_loop_trajectory) + + # Compute and store residuals if i > 0 since we need a previous state to compare + if i > 0: + predicted_state = prev_sol_x[:, 1] # Predicted next state from the previous solution + actual_state = np.array([x0, y0, theta0, v0]) # Current actual state from CARLA + residual = actual_state - predicted_state + residuals_data.append(residual) + + # Update previous solution variables for warm-starting next iteration + prev_sol_x = sol.value(X) + prev_sol_u = sol.value(U) + + else: + print("Error in optimization problem.") + + print("") + world.tick() # Tick the CARLA world diff --git a/src/action/model_predictive_control/model_predictive_control/mpc_core.py b/src/action/model_predictive_control/model_predictive_control/mpc_core.py new file mode 100644 index 00000000..0508ceee --- /dev/null +++ b/src/action/model_predictive_control/model_predictive_control/mpc_core.py @@ -0,0 +1,227 @@ +import casadi as ca +import numpy as np +import datetime +import os +import shutil + +from boxconstraint import BoxConstraint + +TIME_STEP = 0.05 +PREDICTION_HORIZON = 2.0 + + +class MPCCore: + def __init__(self): + + self.params = { + 'L': 2.875 # Wheelbase of the vehicle. Source : https://www.tesla.com/ownersmanual/model3/en_us/GUID-56562137-FC31-4110-A13C-9A9FC6657BF0.html + } + self.T = PREDICTION_HORIZON # Prediction horizon in seconds + self.N = int(T / TIME_STEP) # Prediction horizon in time steps + self.dt = TIME_STEP # Time step for discretization + self.state_dim = 4 # Dimension of the state [x, y, theta, v] + self.control_dim = 2 # Dimension of the control input [steering angle, acceleration] + + # Initialize Opti object + self.opti = ca.Opti() + + # Declare variables + self.X = self.opti.variable(self.state_dim, self.N + 1) # state trajectory variables over prediction horizon + self.U = self.opti.variable(self.control_dim, self.N) # control trajectory variables over prediction horizon + self.P = self.opti.parameter(self.state_dim) # initial state parameter + self.Q_base = ca.MX.eye(self.state_dim) # Base state penalty matrix (emphasizes position states) + self.weight_increase_factor = 1.00 # Increase factor for each step in the prediction horizon + self.R = ca.MX.eye(self.control_dim) # control penalty matrix for objective function + self.W = self.opti.parameter(2, self.N) # Reference trajectory parameter + + # Objective + self.obj = 0 + + self.waypoints = [] + + self.closed_loop_data = [] + self.open_loop_data = [] + self.residuals_data = [] + + self.prev_sol_x = None + self.prev_sol_u = None + + # MPC Initial Setup + self.get_waypoints() + self.setup_mpc() + self.setup_constraints() + self.setup_solver() + + def update_waypoints(self, waypoints_msg): + raw_waypoints[] = waypoints_msg.waypoints + + # Process and convert each waypoint to CasADi format + for wp in raw_waypoints: + if hasattr(wp, 'x') and hasattr(wp, 'y'): # Ensure waypoint has x and y + # Convert to CasADi format and add to the waypoints list + self.waypoints.append(generate_waypoint(wp.x, wp.y)) + # else: + # # Handle missing or invalid waypoint coordinates + # print(f"Invalid waypoint: {wp}") + + + def generate_waypoint(x, y): # Convert to CasADi format and add to the waypoints list + return ca.vertcat(x, y) + + def setup_mpc(self): + """ + Setup the MPC problem with CasADi + """ + + for k in range(self.N): + Q = self.Q_base * (self.weight_increase_factor ** k) # Increase weight for each step in the prediction horizon + + x_k = self.X[:, k] # Current state + u_k = self.U[:, k] # Current control input + x_next = self.X[:, k + 1] # Next state + + x_ref = ca.vertcat(self.W[:, k], + ca.MX.zeros(self.state_dim - 2, 1)) # Reference state with waypoint and zero for other states + + dx = x_k - x_ref # Deviation of state from reference state + du = u_k # Control input deviation (assuming a desired control input of zero) + + # Quadratic cost with reference state and control input + self.obj += ca.mtimes([ca.mtimes(dx.T, Q), dx]) + ca.mtimes( + [ca.mtimes(du.T, self.R), du]) # Minimize quadratic cost and deviation from reference state + + self.opti.minimize(self.obj) + + # Maximum steerin angle for dynamics + self.max_steering_angle_deg = max(wheel.max_steer_angle for wheel in + vehicle.get_physics_control().wheels) # Maximum steering angle in degrees (from vehicle physics control + self.max_steering_angle_rad = max_steering_angle_deg * (ca.pi / 180) # Maximum steering angle in radians + + # Dynamics (Euler discretization using bicycle model) + for k in range(self.N): + steering_angle_rad = self.U[0, k] * self.max_steering_angle_rad # Convert normalized steering angle to radians + + self.opti.subject_to(self.X[:, k + 1] == self.X[:, k] + self.dt * ca.vertcat( + self.X[3, k] * ca.cos(self.X[2, k]), + self.X[3, k] * ca.sin(self.X[2, k]), + (self.X[3, k] / self.params['L']) * ca.tan(steering_angle_rad), + self.U[1, k] + )) + + + def setup_constraints(self): + + self.opti.subject_to(self.X[:, 0] == self.P) # Initial state constraint + + # Input constraints + steering_angle_bounds = [-1.0, 1.0] + acceleration_bounds = [-1.0, 1.0] + lb = np.array([steering_angle_bounds[0], acceleration_bounds[0]]).reshape(-1, 1) + ub = np.array([steering_angle_bounds[1], acceleration_bounds[1]]).reshape(-1, 1) + action_space = BoxConstraint(lb=lb, ub=ub) + + # State constraints + # x_bounds = [-10000, 1000] # x position bounds (effectively no bounds) + # y_bounds = [-1000, 1000] # y position bounds (effectively no bounds) + # theta_bounds = [0, 360] # theta bounds in degrees + # v_bounds = [-10, 10] # velocity bounds + # lb = np.array([x_bounds[0], y_bounds[0], theta_bounds[0], v_bounds[0]]).reshape(-1, 1) + # ub = np.array([x_bounds[1], y_bounds[1], theta_bounds[1], v_bounds[1]]).reshape(-1, 1) + # state_space = BoxConstraint(lb=lb, ub=ub) + + # Apply constraints to optimization problem + for i in range(self.N): + # Input constraints + self.opti.subject_to(action_space.H_np @ self.U[:, i] <= action_space.b_np) + + # State constraints + # opti.subject_to(state_space.H_np @ X[:, i] <= state_space.b_np) + + def setup_solver(self): + acceptable_dual_inf_tol = 1e11 + acceptable_compl_inf_tol = 1e-3 + acceptable_iter = 15 + acceptable_constr_viol_tol = 1e-3 + acceptable_tol = 1e-6 + + opts = {"ipopt.acceptable_tol": acceptable_tol, + "ipopt.acceptable_constr_viol_tol": acceptable_constr_viol_tol, + "ipopt.acceptable_dual_inf_tol": acceptable_dual_inf_tol, + "ipopt.acceptable_iter": acceptable_iter, + "ipopt.acceptable_compl_inf_tol": acceptable_compl_inf_tol, + "ipopt.hessian_approximation": "limited-memory", + "ipopt.print_level": 0} + self.opti.solver('ipopt', opts) + + def compute_control(self, vehicle_state): + """ + Update the vehicle state based on the incoming ROS message. + :param vehicle_state: VehicleState message with current position, velocity, and angle. + """ + # Update P (initial state) with the new vehicle state + self.opti.set_value(self.P, ca.vertcat(vehicle_state.pos_x, vehicle_state.pos_y, vehicle_state.angle, vehicle_state.velocity)) + + x0 = vehicle_state.pos_x + y0 = vehicle_state.pos_y + theta0 = vehicle_state.angle + v0 = vehicle_state.velocity # Assumes CARLA sends a velocity scalar not a velocity vector + i = vehicle_state.interation + + print("Current x: ", x0) + print("Current y: ", y0) + print("Current theta: ", theta0) + print("Current velocity: ", v0) + + if i > 0: + self.closed_loop_data.append([x0, y0, theta0, v0]) # Original Code need i > 0 + + initial_state = ca.vertcat(x0, y0, theta0, v0) + self.opti.set_value(self.P, initial_state) + + # Set the reference trajectory for the current iteration + self.opti.set_value(self.W, ca.horzcat(*self.waypoints[i:i + self.N])) # Concatenate waypoints + + if self.prev_sol_x is not None and self.prev_sol_u is not None: + # Warm-starting the solver with the previous solution + self.opti.set_initial(self.X, self.prev_sol_x) + self.opti.set_initial(self.U, self.prev_sol_u) + + """ + Solve the MPC optimization problem and return the control commands. + :return: Tuple of (steering angle, throttle). + """ + steering_angle = 0 + throttle = 0 + + # Solve the optimization problem + sol = self.opti.solve() + + if sol.stats()['success']: + # Extract control inputs (steering angle, throttle) + u = sol.value(self.U[:, 0]) + steering_angle = np.clip(u[0], -1.0, 1.0) + throttle = np.clip(u[1], -1.0, 1.0) + + print("Steering angle: ", u[0]) + print("Acceleration: ", u[1]) + + # Store open-loop trajectory data with control input applied to vehicle + open_loop_trajectory = sol.value(self.X) + open_loop_trajectory = open_loop_trajectory.T.reshape(-1, self.state_dim) + open_loop_trajectory = np.hstack((open_loop_trajectory, np.tile(u, (N + 1, 1)))) + self.open_loop_data.append(open_loop_trajectory) + + if i > 0: + predicted_state = self.prev_sol_x[:, 1] # Predicted next state from the previous solution + actual_state = np.array([x0, y0, theta0, v0]) # Current actual state from CARLA + residual = actual_state - predicted_state + self.residuals_data.append(residual) + + # Update previous solution variables for warm-starting next iteration + self.prev_sol_x = sol.value(self.X) + self.prev_sol_u = sol.value(self.U) + + else: + print("Error in optimization problem.") + + return steering_angle, throttle \ No newline at end of file diff --git a/src/action/model_predictive_control/model_predictive_control/mpc_node.py b/src/action/model_predictive_control/model_predictive_control/mpc_node.py new file mode 100644 index 00000000..26983697 --- /dev/null +++ b/src/action/model_predictive_control/model_predictive_control/mpc_node.py @@ -0,0 +1,59 @@ +# Copyright 2023 WATonomous +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node + +from action_msgs import ControlCommands, VehicleState, WaypointArray +from model_predictive_control.mpc_core import MPCCore + + +class MPCNode(Node): + def __init__(self): + super().__init__('MPCNode') + + self.mpc_core = MPCCore() + + # Subscribe to vehicle state + self.state_subscription = self.create_subscription( + VehicleState, '/carla/vehicle_state', self.vehicle_state_callback, 10) + + # Subscribe to waypoints from CARLA + self.waypoints_subscription = self.create_subscription( + WaypointArray, '/carla/waypoints', self.waypoints_callback, 10) + + self.control_publisher = self.create_publisher(ControlCommands, '/mpc/control_commands', 10) + + def vehicle_state_callback(self, msg): + steering_angle, throttle = self.mpc_core.compute_control(msg) + + control_msg = control_msgs() + control_msg.steering_angle = steering_angle + control_msg.throttle = throttle + self.control_publisher.publish(control_msg) + + def waypoints_callback(self, msg): + self.mpc_core.update_waypoints(msg) + + +def main(args=None): + rclpy.init(args=args) + mpc_node = MPCNode() + rclpy.spin(mpc_node) + mpc_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/action/model_predictive_control/package.xml b/src/action/model_predictive_control/package.xml index a855111e..90884e4c 100644 --- a/src/action/model_predictive_control/package.xml +++ b/src/action/model_predictive_control/package.xml @@ -8,7 +8,7 @@ TODO: License declaration ament_cmake - + geometry_msgs ament_cmake diff --git a/src/action/model_predictive_control/test/mpc_test.py b/src/action/model_predictive_control/test/mpc_test.py new file mode 100644 index 00000000..7f36d1fc --- /dev/null +++ b/src/action/model_predictive_control/test/mpc_test.py @@ -0,0 +1 @@ +# create some trajectory and init mpc and carla nodes \ No newline at end of file diff --git a/src/simulation/carla_notebooks/boxconstraint.py b/src/simulation/carla_notebooks/boxconstraint.py new file mode 100644 index 00000000..9cfd33bb --- /dev/null +++ b/src/simulation/carla_notebooks/boxconstraint.py @@ -0,0 +1,70 @@ +import numpy as np +import torch + + +class BoxConstraint: + """ + Bounded constraints lb <= x <= ub as polytopic constraints -Ix <= -b and Ix <= b. np.vstack(-I, I) forms the H matrix from III-D-b of the paper + """ + def __init__(self, lb=None, ub=None, plot_idxs=None): + """ + :param lb: dimwise list of lower bounds. + :param ub: dimwise list of lower bounds. + :param plot_idxs: When plotting, the box itself might be defined in some dimension greater than 2 but we might only want to + plot the workspace variables and so plot_idxs allows us to limit the consideration of plot_constraint_set to those variables. + """ + self.lb = np.array(lb, ndmin=2).reshape(-1, 1) + self.ub = np.array(ub, ndmin=2).reshape(-1, 1) + self.plot_idxs = plot_idxs + self.dim = self.lb.shape[0] + assert (self.lb < self.ub).all(), "Lower bounds must be greater than corresponding upper bound for any given dimension" + self.setup_constraint_matrix() + + def __str__(self): return "Lower bound: %s, Upper bound: %s" % (self.lb, self.ub) + + def get_random_vectors(self, num_samples): + rand_samples = np.random.rand(self.dim, num_samples) + for i in range(self.dim): + scale_factor, shift_factor = (self.ub[i] - self.lb[i]), self.lb[i] + rand_samples[i, :] = (rand_samples[i, :] * scale_factor) + shift_factor + return rand_samples + + def setup_constraint_matrix(self): + dim = self.lb.shape[0] + # Casadi can't do matrix mult with Torch instances but only numpy instead. So have to use the np version of the H and b matrix/vector when + # defining constraints in the opti stack. + self.H_np = np.vstack((-np.eye(dim), np.eye(dim))) + self.H = torch.Tensor(self.H_np) + # self.b = torch.Tensor(np.hstack((-self.lb, self.ub))) + self.b_np = np.vstack((-self.lb, self.ub)) + self.b = torch.Tensor(self.b_np) + # print(self.b) + self.sym_func = lambda x: self.H @ np.array(x, ndmin=2).T - self.b + + def check_satisfaction(self, sample): + # If sample is within the polytope defined by the constraints return 1 else 0. + # print(sample, np.array(sample, ndmin=2).T, self.sym_func(sample), self.b) + return (self.sym_func(sample) <= 0).all() + + def generate_uniform_samples(self, num_samples): + n = int(np.round(num_samples**(1./self.lb.shape[0]))) + + # Generate a 1D array of n equally spaced values between the lower and upper bounds for each dimension + coords = [] + for i in range(self.lb.shape[0]): + coords.append(np.linspace(self.lb[i, 0], self.ub[i, 0], n)) + + # Create a meshgrid of all possible combinations of the n-dimensions + meshes = np.meshgrid(*coords, indexing='ij') + + # Flatten the meshgrid and stack the coordinates to create an array of size (K, n-dimensions) + samples = np.vstack([m.flatten() for m in meshes]) + + # Truncate the array to K samples + samples = samples[:num_samples, :] + + # Print the resulting array + return samples + + def clip_to_bounds(self, samples): + return np.clip(samples, self.lb, self.ub) \ No newline at end of file diff --git a/src/simulation/carla_notebooks/mpc_script.py b/src/simulation/carla_notebooks/mpc_script.py new file mode 100644 index 00000000..837a9ac2 --- /dev/null +++ b/src/simulation/carla_notebooks/mpc_script.py @@ -0,0 +1,283 @@ +import carla +import casadi as ca +import numpy as np +import datetime +import os +import shutil + +from boxconstraint import BoxConstraint + +SIM_DURATION = 500 # Simulation duration in time steps + +## SETUP ## +# Connect to CARLA +client = carla.Client('localhost', 2000) +maps = [m.replace('/Game/Carla/Maps/', '') for m in client.get_available_maps()] +print('Available maps: ', maps) +world = client.get_world() +mymap = world.get_map() +print('Using map: ', mymap.name) +spectator = world.get_spectator() + +# CARLA Settings +settings = world.get_settings() +# Timing settings +settings.synchronous_mode = True # Enables synchronous mode +TIME_STEP = 0.05 # Time step for synchronous mode +settings.fixed_delta_seconds = TIME_STEP +# Physics substep settings +settings.substepping = True +settings.max_substep_delta_time = 0.01 +settings.max_substeps = 10 + +world.apply_settings(settings) + +# Output client and world objects to console +print(client) +print(world) + + +# Function to move the spectator camera +def move_spectator_to_vehicle(vehicle, spectator, distance=10): + vehicle_location = vehicle.get_location() + # Set viewing angle to slightly above the vehicle + spectator_transform = carla.Transform(vehicle_location + carla.Location(z=distance), carla.Rotation(pitch=-90)) + spectator.set_transform(spectator_transform) + + +# Use recommended spawn points +spawn_points = mymap.get_spawn_points() +spawn_point = spawn_points[0] + +# Spawn vehicle +vehicles = world.get_actors().filter('vehicle.*') +blueprint_library = world.get_blueprint_library() +vehicle_bp = blueprint_library.filter('model3')[0] +print("Vehicle blueprint attributes:") +for attr in vehicle_bp: + print(' - {}'.format(attr)) + +if len(vehicles) == 0: + vehicle = world.spawn_actor(vehicle_bp, spawn_point) +else: + # Reset world + for vehicle in vehicles: + vehicle.destroy() + vehicle = world.spawn_actor(vehicle_bp, spawn_point) +print(vehicle) + + +def generate_waypoint_relative_to_spawn(forward_offset=0, sideways_offset=0): + waypoint_x = spawn_point.location.x + spawn_point.get_forward_vector().x * forward_offset + spawn_point.get_right_vector().x * sideways_offset + waypoint_y = spawn_point.location.y + spawn_point.get_forward_vector().y * forward_offset + spawn_point.get_right_vector().y * sideways_offset + return ca.vertcat(waypoint_x, waypoint_y) + + +def generate_waypoint(x, y): + return ca.vertcat(x, y) + + +waypoints = [] + +for i in range(SIM_DURATION): + waypoints.append(generate_waypoint_relative_to_spawn(-10, 0)) + +# Parameters +params = { + 'L': 2.875 # Wheelbase of the vehicle. Source : https://www.tesla.com/ownersmanual/model3/en_us/GUID-56562137-FC31-4110-A13C-9A9FC6657BF0.html +} +T = 2.0 # Prediction horizon in seconds +N = int(T / TIME_STEP) # Prediction horizon in time steps +dt = TIME_STEP # Time step for discretization +state_dim = 4 # Dimension of the state [x, y, theta, v] +control_dim = 2 # Dimension of the control input [steering angle, acceleration] + +# Initialize Opti object +opti = ca.Opti() + +# Declare variables +X = opti.variable(state_dim, N + 1) # state trajectory variables over prediction horizon +U = opti.variable(control_dim, N) # control trajectory variables over prediction horizon +P = opti.parameter(state_dim) # initial state parameter +Q_base = ca.MX.eye(state_dim) # Base state penalty matrix (emphasizes position states) +weight_increase_factor = 1.00 # Increase factor for each step in the prediction horizon +R = ca.MX.eye(control_dim) # control penalty matrix for objective function +W = opti.parameter(2, N) # Reference trajectory parameter + +# Objective +obj = 0 +for k in range(N): + Q = Q_base * (weight_increase_factor ** k) # Increase weight for each step in the prediction horizon + + x_k = X[:, k] # Current state + u_k = U[:, k] # Current control input + x_next = X[:, k + 1] # Next state + + x_ref = ca.vertcat(W[:, k], + ca.MX.zeros(state_dim - 2, 1)) # Reference state with waypoint and zero for other states + + dx = x_k - x_ref # Deviation of state from reference state + du = u_k # Control input deviation (assuming a desired control input of zero) + + # Quadratic cost with reference state and control input + obj += ca.mtimes([ca.mtimes(dx.T, Q), dx]) + ca.mtimes( + [ca.mtimes(du.T, R), du]) # Minimize quadratic cost and deviation from reference state + +opti.minimize(obj) + +# Maximum steerin angle for dynamics +max_steering_angle_deg = max(wheel.max_steer_angle for wheel in + vehicle.get_physics_control().wheels) # Maximum steering angle in degrees (from vehicle physics control +max_steering_angle_rad = max_steering_angle_deg * (ca.pi / 180) # Maximum steering angle in radians + +# Dynamics (Euler discretization using bicycle model) +for k in range(N): + steering_angle_rad = U[0, k] * max_steering_angle_rad # Convert normalized steering angle to radians + + opti.subject_to(X[:, k + 1] == X[:, k] + dt * ca.vertcat( + X[3, k] * ca.cos(X[2, k]), + X[3, k] * ca.sin(X[2, k]), + (X[3, k] / params['L']) * ca.tan(steering_angle_rad), + U[1, k] + )) + +# Constraints +opti.subject_to(X[:, 0] == P) # Initial state constraint + +# Input constraints +steering_angle_bounds = [-1.0, 1.0] +acceleration_bounds = [-1.0, 1.0] +lb = np.array([steering_angle_bounds[0], acceleration_bounds[0]]).reshape(-1, 1) +ub = np.array([steering_angle_bounds[1], acceleration_bounds[1]]).reshape(-1, 1) +action_space = BoxConstraint(lb=lb, ub=ub) + +# State constraints +# x_bounds = [-10000, 1000] # x position bounds (effectively no bounds) +# y_bounds = [-1000, 1000] # y position bounds (effectively no bounds) +# theta_bounds = [0, 360] # theta bounds in degrees +# v_bounds = [-10, 10] # velocity bounds +# lb = np.array([x_bounds[0], y_bounds[0], theta_bounds[0], v_bounds[0]]).reshape(-1, 1) +# ub = np.array([x_bounds[1], y_bounds[1], theta_bounds[1], v_bounds[1]]).reshape(-1, 1) +# state_space = BoxConstraint(lb=lb, ub=ub) + +# Apply constraints to optimization problem +for i in range(N): + # Input constraints + opti.subject_to(action_space.H_np @ U[:, i] <= action_space.b_np) + + # State constraints + # opti.subject_to(state_space.H_np @ X[:, i] <= state_space.b_np) + +# Setup solver +acceptable_dual_inf_tol = 1e11 +acceptable_compl_inf_tol = 1e-3 +acceptable_iter = 15 +acceptable_constr_viol_tol = 1e-3 +acceptable_tol = 1e-6 + +opts = {"ipopt.acceptable_tol": acceptable_tol, + "ipopt.acceptable_constr_viol_tol": acceptable_constr_viol_tol, + "ipopt.acceptable_dual_inf_tol": acceptable_dual_inf_tol, + "ipopt.acceptable_iter": acceptable_iter, + "ipopt.acceptable_compl_inf_tol": acceptable_compl_inf_tol, + "ipopt.hessian_approximation": "limited-memory", + "ipopt.print_level": 0} +opti.solver('ipopt', opts) + +# Array to store closed-loop trajectory states (X and Y coordinates) +closed_loop_data = [] +open_loop_data = [] +residuals_data = [] + +# Initialize warm-start parameters +prev_sol_x = None +prev_sol_u = None + +# Main Loop +for i in range(SIM_DURATION - N): # Subtract N since we need to be able to predict N steps into the future + print("Iteration: ", i) + + move_spectator_to_vehicle(vehicle, spectator) + + # Draw current waypoints in CARLA + for waypoint in waypoints[i:i + N]: + waypoint_x = float(np.array(waypoint[0])) + waypoint_y = float(np.array(waypoint[1])) + + carla_waypoint = carla.Location(x=waypoint_x, y=waypoint_y, z=0.5) + + extent = carla.Location(x=0.5, y=0.5, z=0.5) + world.debug.draw_box(box=carla.BoundingBox(carla_waypoint, extent * 1e-2), + rotation=carla.Rotation(pitch=0, yaw=0, roll=0), life_time=TIME_STEP * 10, thickness=0.5, + color=carla.Color(255, 0, 0)) + + # Fetch initial state from CARLA + x0 = vehicle.get_transform().location.x + y0 = vehicle.get_transform().location.y + theta0 = vehicle.get_transform().rotation.yaw / 180 * ca.pi + velocity_vector = vehicle.get_velocity() + v0 = ca.sqrt(velocity_vector.x ** 2 + velocity_vector.y ** 2) + + print("Current x: ", x0) + print("Current y: ", y0) + print("Current yaw: ", vehicle.get_transform().rotation.yaw) + print("Current theta: ", theta0) + print("Current velocity: ", v0) + + # Store current state in the closed-loop trajectory data + if i > 0: + closed_loop_data.append([x0, y0, theta0, v0]) + + # Set initial state for optimization problem + initial_state = ca.vertcat(x0, y0, theta0, v0) + opti.set_value(P, initial_state) + + # Set the reference trajectory for the current iteration + opti.set_value(W, ca.horzcat(*waypoints[i:i + N])) # Concatenate waypoints + + if prev_sol_x is not None and prev_sol_u is not None: + # Warm-starting the solver with the previous solution + opti.set_initial(X, prev_sol_x) + opti.set_initial(U, prev_sol_u) + + # Solve the optimization problem + sol = opti.solve() + + # If the solver is successful, apply the first control input to the vehicle + if sol.stats()['success']: + u = sol.value(U[:, 0]) + + # Bound acceleration and steering angle to [-1, 1] + u[0] = np.clip(u[0], -1, 1) + u[1] = np.clip(u[1], -1, 1) + + print("Steering angle: ", u[0]) + print("Acceleration: ", u[1]) + + if u[1] < 0: + vehicle.apply_control(carla.VehicleControl(throttle=-u[1], steer=u[0], reverse=True)) + else: + vehicle.apply_control(carla.VehicleControl(throttle=u[1], steer=u[0])) + + # Store open-loop trajectory data with control input applied to vehicle + open_loop_trajectory = sol.value(X) + open_loop_trajectory = open_loop_trajectory.T.reshape(-1, state_dim) + open_loop_trajectory = np.hstack((open_loop_trajectory, np.tile(u, (N + 1, 1)))) + open_loop_data.append(open_loop_trajectory) + + # Compute and store residuals if i > 0 since we need a previous state to compare + if i > 0: + predicted_state = prev_sol_x[:, 1] # Predicted next state from the previous solution + actual_state = np.array([x0, y0, theta0, v0]) # Current actual state from CARLA + residual = actual_state - predicted_state + residuals_data.append(residual) + + # Update previous solution variables for warm-starting next iteration + prev_sol_x = sol.value(X) + prev_sol_u = sol.value(U) + + else: + print("Error in optimization problem.") + + print("") + world.tick() # Tick the CARLA world diff --git a/src/simulation/carla_notebooks/mpc_test.py b/src/simulation/carla_notebooks/mpc_test.py new file mode 100644 index 00000000..10d6a7b0 --- /dev/null +++ b/src/simulation/carla_notebooks/mpc_test.py @@ -0,0 +1,14 @@ +import carla +import os +import random + +## SETUP ## +client_name = os.environ.get("CLIENT_NAME", "DOES NOT EXIST") +if client_name == "DOES NOT EXIST": + raise Exception("The environment variable for the container name of the carla server has not been set") + +# Connect to the client and retrieve the world object +client = carla.Client(client_name, 2000) +world = client.get_world() +spectator = world.get_spectator() + diff --git a/src/simulation/carla_sim/carla_sim/carla_core.py b/src/simulation/carla_sim/carla_sim/carla_core.py new file mode 100644 index 00000000..afb6fe20 --- /dev/null +++ b/src/simulation/carla_sim/carla_sim/carla_core.py @@ -0,0 +1,165 @@ +import carla +import numpy as np +import datetime +import os +import shutil + +SIM_DURATION = 500 # Simulation duration in time steps in Carla +TIME_STEP = 0.05 +PREDICTION_HORIZON = 2.0 + +class CarlaCore: + def __init__(self, publish_state): + self.publish_state = publish_state + + self.spectator = None + self.vehicles = None + self.spawn_point = None + + self.setup_carla() + + self.vehicle_state = { + 'x': 0.0, + 'y': 0.0, + 'theta': 0.0, + 'velocity': 0.0, + 'iteration': 0 + } + + self.waypoints = [] + + def setup_carla(self): + ## SETUP ## + # Connect to CARLA + client = carla.Client('localhost', 2000) + maps = [m.replace('/Game/Carla/Maps/', '') for m in client.get_available_maps()] + print('Available maps: ', maps) + world = client.get_world() + mymap = world.get_map() + print('Using map: ', mymap.name) + self.spectator = world.get_spectator() + + # CARLA Settings + settings = world.get_settings() + # Timing settings + settings.synchronous_mode = True # Enables synchronous mode + TIME_STEP = 0.05 # Time step for synchronous mode + settings.fixed_delta_seconds = TIME_STEP + # Physics substep settings + settings.substepping = True + settings.max_substep_delta_time = 0.01 + settings.max_substeps = 10 + + world.apply_settings(settings) + + # Output client and world objects to console + print(client) + print(world) + + # Use recommended spawn points + self.spawn_points = mymap.get_spawn_points() + self.spawn_point = spawn_points[0] + + # Spawn vehicle + self.vehicles = world.get_actors().filter('vehicle.*') + blueprint_library = world.get_blueprint_library() + vehicle_bp = blueprint_library.filter('model3')[0] + print("Vehicle blueprint attributes:") + for attr in vehicle_bp: + print(' - {}'.format(attr)) + + if len(vehicles) == 0: + vehicle = world.spawn_actor(vehicle_bp, spawn_point) + else: + # Reset world + for vehicle in vehicles: + vehicle.destroy() + vehicle = world.spawn_actor(vehicle_bp, spawn_point) + print(vehicle) + + + def get_waypoints(self): + """Generate and return the list of waypoints relative to the vehicle's spawn point.""" + + for i in range(SIM_DURATION): + self.waypoints.append(self.generate_waypoint_relative_to_spawn(-10, 0)) + + return self.waypoints + + def generate_waypoint_relative_to_spawn(self, forward_offset=0, sideways_offset=0): + waypoint_x = spawn_point.location.x + spawn_point.get_forward_vector().x * forward_offset + spawn_point.get_right_vector().x * sideways_offset + waypoint_y = spawn_point.location.y + spawn_point.get_forward_vector().y * forward_offset + spawn_point.get_right_vector().y * sideways_offset + return [waypoint_x, waypoint_y] + + def get_vehicle_state(self): + """ + Retrieves the current state of the vehicle in the CARLA world. + The state includes position, orientation (theta), velocity, and iteration count. + """ + + # Fetch initial state from CARLA + x0 = vehicle.get_transform().location.x + y0 = vehicle.get_transform().location.y + theta0 = vehicle.get_transform().rotation.yaw / 180 * ca.pi + velocity_vector = vehicle.get_velocity() + v0 = ca.sqrt(velocity_vector.x ** 2 + velocity_vector.y ** 2) + + # Update vehicle state + self.vehicle_state['x'] = x0 + self.vehicle_state['y'] = y0 + self.vehicle_state['theta'] = theta0 + self.vehicle_state['velocity'] = v0 + + return self.vehicle_state + + def apply_control(self, steering_angle, throttle): + """ + Applies the received control commands to the vehicle. + """ + if throttle < 0: + self.vehicle.apply_control(carla.VehicleControl(throttle=-throttle, steer=steering_angle, reverse=True)) + else: + self.vehicle.apply_control(carla.VehicleControl(throttle=throttle, steer=steering_angle)) + + def start_main_loop(self): + """ + Main loop to continuously publish vehicle states and waypoints. + """ + N = int(PREDICTION_HORIZON/TIME_STEP) + + for i in range(SIM_DURATION - N): # Subtract N since we need to be able to predict N steps into the future + print("Iteration: ", i) + + self.vehicle_state['iteration'] = i + + move_spectator_to_vehicle() + + # Draw current waypoints in CARLA + for waypoint in self.waypoints[i:i + N]: + waypoint_x = float(np.array(waypoint[0])) + waypoint_y = float(np.array(waypoint[1])) + + carla_waypoint = carla.Location(x=waypoint_x, y=waypoint_y, z=0.5) + + extent = carla.Location(x=0.5, y=0.5, z=0.5) + world.debug.draw_box(box=carla.BoundingBox(carla_waypoint, extent * 1e-2), + rotation=carla.Rotation(pitch=0, yaw=0, roll=0), life_time=TIME_STEP * 10, thickness=0.5, + color=carla.Color(255, 0, 0)) + + self.publish_state() # MPC Should Start after this !!!! + + # Should automatically apply control after publishing state to mpc + + print("") + world.tick() # Tick the CARLA world + + + def move_spectator_to_vehicle(distance=10): + vehicle_location = vehicle.get_location() + # Set viewing angle to slightly above the vehicle + spectator_transform = carla.Transform(vehicle_location + carla.Location(z=distance), carla.Rotation(pitch=-90)) + spectator.set_transform(spectator_transform) + + def publish_state(self): + """To be overridden by CarlaNode for publishing the vehicle state.""" + pass \ No newline at end of file diff --git a/src/simulation/carla_sim/carla_sim/carla_node.py b/src/simulation/carla_sim/carla_sim/carla_node.py new file mode 100644 index 00000000..6db285c5 --- /dev/null +++ b/src/simulation/carla_sim/carla_sim/carla_node.py @@ -0,0 +1,80 @@ +import rclpy +from rclpy.node import Node + +from action_msgs import ControlCommands, VehicleState, WaypointArray +from carla_core import CarlaCore # Import the CARLA core logic + + +class CarlaNode(Node): + def __init__(self): + super().__init__('CarlaNode') + + # Initialize CARLA core object (manages CARLA setup and running) + self.carla_core = CarlaCore(self.publish_state) + + # Publishers + self.vehicle_state_publisher = self.create_publisher(VehicleState, '/carla/vehicle_state', 10) + self.waypoints_publisher = self.create_publisher(WaypointArray, '/carla/waypoints', 10) + + # Subscribers + self.control_subscription = self.create_subscription( + ControlCommands, '/mpc/control_commands', self.control_callback, 10) + + + self.carla_core.start_main_loop() + + + def publish_state(self): + # Get the current vehicle state from carla_core + vehicle_state = self.carla_core.get_vehicle_state() + if vehicle_state: + # Publish the vehicle state to MPC node + state_msg = state_msgs() + state_msg.pos_x = vehicle_state['x'] + state_msg.pos_y = vehicle_state['y'] + state_msg.angle = vehicle_state['theta'] + state_msg.velocity = vehicle_state['velocity'] + self.vehicle_state_publisher.publish(state_msg) + + def publish_waypoints(self): + # Get the current waypoints from carla_core + waypoints = self.carla_core.get_waypoints() + if waypoints: + # Create an instance of WaypointArray message + waypoint_array_msg = WaypointArray() + + for wp in waypoints: + # Create a Waypoint message for each waypoint in the list + waypoint_msg = Waypoint() + waypoint_msg.x = wp[0] # x-coordinate + waypoint_msg.y = wp[1] # y-coordinate + + # Append each Waypoint message to the WaypointArray message + waypoint_array_msg.waypoints.append(waypoint_msg) + + # Publish the WaypointArray message + self.waypoints_publisher.publish(waypoint_array_msg) + + def control_callback(self, msg): + """ + This function will be called when a control message is received from the MPC Node. + It will forward the control commands to the carla_core. + """ + # Extract steering and throttle from the message + steering_angle = msg.steering_angle + throttle = msg.throttle + + # Send control commands to CARLA via carla_core + self.carla_core.apply_control(steering_angle, throttle) + + +def main(args=None): + rclpy.init(args=args) + carla_node = CarlaNode() + rclpy.spin(carla_node) + carla_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/simulation/carla_sim/launch/carla_sim.launch.py b/src/simulation/carla_sim/launch/carla_sim.launch.py new file mode 100644 index 00000000..45a077bb --- /dev/null +++ b/src/simulation/carla_sim/launch/carla_sim.launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +import launch_ros.actions + +# For creating launch arguments +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + publish_autopilot_arg = DeclareLaunchArgument( + 'publish_autopilot', + default_value='False' + ) + + return LaunchDescription( + [ + launch_ros.actions.Node( + namespace="carla_sample_node", + package='carla_sample_node', + executable='carla_sample_node', + parameters=[{ + 'publish_autopilot': LaunchConfiguration('publish_autopilot') + }], + output='screen') + ]) diff --git a/src/simulation/carla_sim/package.xml b/src/simulation/carla_sim/package.xml new file mode 100644 index 00000000..7e3d0b25 --- /dev/null +++ b/src/simulation/carla_sim/package.xml @@ -0,0 +1,20 @@ + + + + carla_sample_node + 0.0.0 + Sample python node to read data from CARLA + Vishal Jayakumar + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + rclpy + + + ament_python + + \ No newline at end of file diff --git a/src/simulation/carla_sim/setup.cfg b/src/simulation/carla_sim/setup.cfg new file mode 100644 index 00000000..e9a26297 --- /dev/null +++ b/src/simulation/carla_sim/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/carla_sample_node +[install] +install_scripts=$base/lib/carla_sample_node \ No newline at end of file diff --git a/src/simulation/carla_sim/setup.py b/src/simulation/carla_sim/setup.py new file mode 100644 index 00000000..18127651 --- /dev/null +++ b/src/simulation/carla_sim/setup.py @@ -0,0 +1,29 @@ +from setuptools import setup +from glob import glob +import os + +package_name = 'carla_sample_node' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, glob(os.path.join('launch', '*.launch.py'))) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Vishal Jayakumar', + maintainer_email='v3jayaku@watonomous.ca', + description='Sample python node to read data from CARLA', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'carla_sample_node = carla_sample_node.carla_sample_node:main' + ], + }, +) diff --git a/src/simulation/carla_sim/temp.txt b/src/simulation/carla_sim/temp.txt deleted file mode 100644 index f41acb40..00000000 --- a/src/simulation/carla_sim/temp.txt +++ /dev/null @@ -1 +0,0 @@ -place holder for actual sim code \ No newline at end of file diff --git a/watod-config.sh b/watod-config.sh index 3255dd40..eafc5ed9 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -15,7 +15,7 @@ ## - simulation : starts simulation ## - samples : starts sample ROS2 pubsub nodes -# ACTIVE_MODULES="" +ACTIVE_MODULES="simulation action" ################################# MODE OF OPERATION ################################# ## Possible modes of operation when running watod. @@ -23,11 +23,11 @@ ## - deploy (default) : runs production-grade containers (non-editable) ## - develop : runs developer containers (editable) -# MODE_OF_OPERATION="" +MODE_OF_OPERATION="develop" ############################## ADVANCED CONFIGURATIONS ############################## ## Name to append to docker containers. DEFAULT = "" -# COMPOSE_PROJECT_NAME="" +COMPOSE_PROJECT_NAME="hasan3773" ## Tag to use. Images are formatted as : with forward slashes replaced with dashes. ## DEFAULT = ""