Skip to content

Commit

Permalink
Draft
Browse files Browse the repository at this point in the history
  • Loading branch information
@Jackack
Jackack committed Mar 1, 2024
1 parent 3b927c2 commit 3bfc109
Show file tree
Hide file tree
Showing 3 changed files with 109 additions and 63 deletions.
2 changes: 1 addition & 1 deletion rb_ws/src/buggy/launch/sim_2d_2buggies.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@
<node name="sc_sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
args="$(arg sc_start_pos) $(arg sc_velocity) SC"/>

<arg name="sc_autonsystem_args" default="--controller stanley --dist 0.0 --traj buggycourse_safe_1.json --self_name SC --other_name NAND" />
<arg name="sc_autonsystem_args" default="--controller mpc --dist 0.0 --traj buggycourse_safe_1.json --self_name SC --other_name NAND" />
<node name="sc_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
args="$(arg sc_autonsystem_args)"/>

Expand Down
64 changes: 40 additions & 24 deletions rb_ws/src/buggy/scripts/auton/autonsystem.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,22 +141,25 @@ def tick_caller(self):
# planner. Make sure it is significantly (at least 2x) longer
# than 1 period of the planner when you change the planner frequency.

if not self.other_odom_msg is None and self.ticks == 0:
# for debugging, publish distance to other buggy
with self.lock:
self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg)
other_pose, _ = self.get_world_pose_and_speed(self.other_odom_msg)
distance = (self_pose.x - other_pose.x) ** 2 + (self_pose.y - other_pose.y) ** 2
distance = np.sqrt(distance)
self.distance_publisher.publish(Float64(distance))

# profiling
if self.profile:
cProfile.runctx('self.planner_tick()', globals(), locals(), sort="cumtime")
else:
self.planner_tick()

self.local_controller_tick()
# if not self.other_odom_msg is None and self.ticks == 0:
# # for debugging, publish distance to other buggy
# with self.lock:
# self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg)
# other_pose, _ = self.get_world_pose_and_speed(self.other_odom_msg)
# distance = (self_pose.x - other_pose.x) ** 2 + (self_pose.y - other_pose.y) ** 2
# distance = np.sqrt(distance)
# self.distance_publisher.publish(Float64(distance))

# # profiling
# if self.profile:
# cProfile.runctx('self.planner_tick()', globals(), locals(), sort="cumtime")
# else:
# self.planner_tick()

if self.has_other_buggy:
self.mpc_tick()
else:
self.local_controller_tick()

self.ticks += 1

Expand All @@ -182,23 +185,36 @@ def get_world_pose_and_speed(self, msg):
pose_gps = Pose.rospose_to_pose(current_rospose)
return World.gps_to_world_pose(pose_gps), current_speed

def local_controller_tick(self):
def mpc_tick(self):
with self.lock:
if self.other_odom_msg is None:
return

self_pose, self_speed = self.get_world_pose_and_speed(self.self_odom_msg)
other_pose, other_speed = self.get_world_pose_and_speed(self.other_odom_msg)

# Compute control output
steering_angle = self.local_controller.compute_control(
self_pose, self.cur_traj, self_speed)
self_pose, other_pose, self.cur_traj, self_speed)
steering_angle_deg = np.rad2deg(steering_angle)
self.steer_publisher.publish(Float64(steering_angle_deg))

def planner_tick(self):
def local_controller_tick(self):
with self.lock:
other_pose, other_speed = self.get_world_pose_and_speed(self.other_odom_msg)
# update local trajectory via path planner
self.cur_traj = self.path_planner.compute_traj(
other_pose,
other_speed)
self_pose, self_speed = self.get_world_pose_and_speed(self.self_odom_msg)
# Compute control output
steering_angle = self.local_controller.compute_control(
self_pose, self.cur_traj, self_speed)
steering_angle_deg = np.rad2deg(steering_angle)
self.steer_publisher.publish(Float64(steering_angle_deg))

# def planner_tick(self):
# with self.lock:
# other_pose, other_speed = self.get_world_pose_and_speed(self.other_odom_msg)
# # update local trajectory via path planner
# self.cur_traj = self.path_planner.compute_traj(
# other_pose,
# other_speed)
if __name__ == "__main__":
rospy.init_node("auton_system")
parser = argparse.ArgumentParser()
Expand Down
106 changes: 68 additions & 38 deletions rb_ws/src/buggy/scripts/auton/model_predictive_controller.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ class ModelPredictiveController(Controller):
Convex Model Predictive Controller (MPC)
"""

DEBUG = True
DEBUG = False
PLOT = False
TIME = False
ROS = True
Expand All @@ -42,6 +42,7 @@ class ModelPredictiveController(Controller):
LOOKAHEAD_TIME = 0.1 # seconds
N_STATES = 4
N_CONTROLS = 1
PASSING_MARGIN = 4

# MPC Cost Weights
state_cost = np.array([0.0001, 250, 5, 25]) # x, y, theta, steer
Expand Down Expand Up @@ -101,6 +102,16 @@ def __init__(self, buggy_name, start_index=0, ref_trajectory=None, ROS=False) ->
buggy_name + "/auton/debug/error", ROSPose, queue_size=1
)

self.debug_separation_publisher = rospy.Publisher(
buggy_name + "/auton/debug/obstacle_separation", Float64, queue_size=1
)

self.debug_obstacle_active_publisher = rospy.Publisher(
buggy_name + "/auton/debug/obstacle_active", Float64, queue_size=1
)



self.solver = osqp.OSQP()

self.C = sparse.kron(
Expand Down Expand Up @@ -348,12 +359,13 @@ def transform_trajectory(trajectory, transform_matrix):

first_iteration = True

def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current_speed: float):
def compute_trajectory(self, current_pose: Pose, other_pose: Pose, trajectory: Trajectory, current_speed: float):
"""
Computes the desired control output given the current state and reference trajectory
Args:
current_pose (Pose): current pose (x, y, theta) (UTM coordinates)
other_pose (Pose): pose of NAND (x, y, theta) (UTM coordinates)
trajectory (Trajectory): reference trajectory
current_speed (float): current speed of the buggy
Expand Down Expand Up @@ -407,12 +419,16 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current
if self.TIME:
t = time.time()

# Rotate reference trajectory to the frame of the current pose
# transform reference trajectory to the frame of the current pose
inverted_pose = current_pose.invert()
transform = inverted_pose.to_mat()
reference_trajectory = self.transform_trajectory(
reference_trajectory, inverted_pose.to_mat()
reference_trajectory, transform
)

# transform NAND's pose to the frame of the current pose
other_pose_local = current_pose.convert_pose_from_global_to_local_frame(other_pose)

if self.TIME:
ref_transform_time = 1000.0 * (time.time() - t)

Expand Down Expand Up @@ -442,25 +458,6 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current
if self.TIME:
t = time.time()

# H = sparse.block_diag(
# (
# self.control_cost_diag,
# *[
# m
# for k in range(0, self.MPC_HORIZON - 1)
# for m in [
# np.diag(self.rotate_state_cost(reference_trajectory[k, 2])),
# self.control_cost_diag,
# ]
# ],
# np.diag(
# self.rotate_final_state_cost(
# reference_trajectory[self.MPC_HORIZON - 1, 2]
# )
# ),
# ),
# format="csc",
# )
H = sparse.diags(
np.concatenate(
[
Expand Down Expand Up @@ -511,8 +508,12 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current
# c = [n 0 0], where n is the normal vector of the halfplane in x-y space
# p is the position of NAND in x-y space

n = np.array([100, 100])
p = np.array([0, 1])
n = trajectory.get_unit_normal_by_index(knot_point_indices[0].ravel())[0]
p = np.array([other_pose_local.x, other_pose_local.y]) # (x,y) of other buggy

# as a margin of error, shift p to the left along the ref-traj
p += (self.PASSING_MARGIN + n/np.linalg.norm(n))

c = np.concatenate((n, np.zeros((2, )))).reshape(1, self.N_STATES)

C2 = sparse.kron(
Expand Down Expand Up @@ -542,19 +543,39 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current
np.zeros(self.N_STATES * (self.MPC_HORIZON - 1)),
np.tile(self.state_lb, self.MPC_HORIZON) + reference_trajectory.ravel(),
np.tile(self.control_lb, self.MPC_HORIZON) + reference_control.ravel(),
np.tile(n.T @ p, self.MPC_HORIZON),
np.tile(-np.inf, self.MPC_HORIZON),
)
)
ub = np.hstack(
(
-self.state_jacobian(reference_trajectory[0, :])
@ (self.state(0, 0, 0, 0) - reference_trajectory[0, :]),
np.zeros(self.N_STATES * (self.MPC_HORIZON - 1)),
np.tile(self.state_ub, self.MPC_HORIZON) + reference_trajectory.ravel(),
np.tile(self.control_ub, self.MPC_HORIZON) + reference_control.ravel(),
np.tile(np.inf, self.MPC_HORIZON),

# only activate NAND constraint if we are near NAND
if other_pose_local.x ** 2 + other_pose_local.y ** 2 < 2 ** 2:
ub = np.hstack(
(
-self.state_jacobian(reference_trajectory[0, :])
@ (self.state(0, 0, 0, 0) - reference_trajectory[0, :]),
np.zeros(self.N_STATES * (self.MPC_HORIZON - 1)),
np.tile(self.state_ub, self.MPC_HORIZON) + reference_trajectory.ravel(),
np.tile(self.control_ub, self.MPC_HORIZON) + reference_control.ravel(),
np.tile(n.T@p, self.MPC_HORIZON),
)
)
)

self.debug_obstacle_active_publisher.publish(Float64(1.0))
else:
ub = np.hstack(
(
-self.state_jacobian(reference_trajectory[0, :])
@ (self.state(0, 0, 0, 0) - reference_trajectory[0, :]),
np.zeros(self.N_STATES * (self.MPC_HORIZON - 1)),
np.tile(self.state_ub, self.MPC_HORIZON) + reference_trajectory.ravel(),
np.tile(self.control_ub, self.MPC_HORIZON) + reference_control.ravel(),
np.tile(np.inf, self.MPC_HORIZON),
)
)


self.debug_obstacle_active_publisher.publish(Float64(0.0))


if self.TIME:
create_mat_time_bounds = 1000.0 * (time.time() - t)
Expand Down Expand Up @@ -602,8 +623,8 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current
solution_trajectory = np.reshape(results.x, (self.MPC_HORIZON, self.N_STATES + self.N_CONTROLS))
state_trajectory = solution_trajectory[:, self.N_CONTROLS:(self.N_CONTROLS + self.N_STATES)]

print("status", results.info.status, results.info.status_val)
if not (results.info.status == "solved" or results.info.status == "solved inaccurate"):
print("solution failed", results.info.status)
return reference_trajectory

state_trajectory += reference_trajectory
Expand Down Expand Up @@ -689,6 +710,14 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current
reference_navsat.latitude = ref_gps[0]
reference_navsat.longitude = ref_gps[1]
self.debug_reference_pos_publisher.publish(reference_navsat)


x_dist = current_pose.x - other_pose.x
y_dist = current_pose.y - other_pose.y

self.debug_separation_publisher.publish(
Float64(np.linalg.norm(np.array([x_dist, y_dist]))))

except rospy.ROSException:
pass
# print("ROS Publishing Error")
Expand All @@ -710,14 +739,15 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current
return state_trajectory
# return steer_angle

def compute_control(self, current_pose: Pose, trajectory: Trajectory, current_speed: float):
state_trajectory = self.compute_trajectory(current_pose, trajectory, current_speed)
def compute_control(self, current_pose: Pose, other_pose:Pose, trajectory: Trajectory, current_speed: float):
state_trajectory = self.compute_trajectory(current_pose, other_pose, trajectory, current_speed)
steer_angle = state_trajectory[0, self.N_STATES - 1]

return steer_angle


if __name__ == "__main__":

mpc_controller = ModelPredictiveController(
ref_trajectory=Trajectory("/rb_ws/src/buggy/paths/quartermiletrack.json"),
ROS=True)
Expand Down

0 comments on commit 3bfc109

Please sign in to comment.