Skip to content
Snippets Groups Projects
Commit 5ddd4d10 authored by rachelmoan's avatar rachelmoan
Browse files

delete comments, change variable names

parent 76db9e3f
No related branches found
No related tags found
No related merge requests found
......@@ -113,13 +113,8 @@ class MultiMPC:
# Cost function
cost = 0
for k in range(self.control_horizon):
for i in range(self.num_robots):# 0, 3 # 3,6
# print(f"k = {k}/{self.control_horizon-1}")
# print(f"target a = {target[i]}")
# print(f"target b = {target[i][:][k]}")
# # print(f"target c = {target[i][:][k]}")
for i in range(self.num_robots):
this_target = [target[i][0][k], target[i][1][k], target[i][2][k]]
# print(f"this_target = {this_target}")
# difference between the current state and the target state
cost += ca.mtimes([(X[i*3 : i*3 +3, k+1] - this_target).T, self.Q, X[i*3 : i*3 +3, k+1] - this_target])
......@@ -173,12 +168,13 @@ class MultiMPC:
opti.subject_to(X[i*3:i*3+3, 0] == initial_state[i])
for i in range(self.num_robots):
opti.subject_to(opti.bounded(-self.robot_model.max_acc, U[i*2:i*2+2, :], self.robot_model.max_acc))
opti.subject_to(ca.fabs(U[i*2, 0] - prev_cmd[i][0]) / self.dt <= self.robot_model.max_d_acc)
opti.subject_to(opti.bounded(-self.robot_model.max_vel, U[i*2, :], self.robot_model.max_vel))
opti.subject_to(opti.bounded(-self.robot_model.max_steer_angle, U[i*2+1, :], self.robot_model.max_steer_angle))
opti.subject_to(ca.fabs(U[i*2, 0] - prev_cmd[i][0]) / self.dt <= self.robot_model.max_d_vel)
opti.subject_to(ca.fabs(U[i*2+1, 0] - prev_cmd[i][1]) / self.dt <= self.robot_model.max_d_steer)
for k in range(1, self.control_horizon):
opti.subject_to(ca.fabs(U[i*2, k] - U[i*2, k-1]) / self.dt <= self.robot_model.max_d_acc)
opti.subject_to(ca.fabs(U[i*2, k] - U[i*2, k-1]) / self.dt <= self.robot_model.max_d_vel)
opti.subject_to(ca.fabs(U[i*2+1, k] - U[i*2+1, k-1]) / self.dt <= self.robot_model.max_d_steer)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment