delete comments, change variable names

guided_mrmp/controllers/multi_mpc.py

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