Restructuring code a bit

guided_mrmp/optimizer.py

-import cvxpy as opt
-import numpy as np
-
-class Optimizer:
-    def __init__(self, nx, nu, control_horizon, Q, Qf, R, P):
-        self.nx = nx
-        self.nu = nu
-        self.control_horizon = control_horizon
-        self.Q = Q
-        self.Qf = Qf
-        self.R = R
-        self.P = P
-
-    def solve(self, initial_state, target, prev_cmd, A, B, C, robot_model, dt):
-        """
-        Sets up and solves the optimization problem.
-
-        Args:
-            initial_state (array-like): current estimate of [x, y, heading]
-            target (ndarray): state space reference, in the same frame as the provided current state
-            prev_cmd (array-like): previous [v, delta]
-            A, B, C: Linearized state-space matrices
-            robot_model: Robot model containing constraints
-            dt: Time step
-
-        Returns:
-            x, u: Optimal state and input trajectories
-        """
-
-        # set up variables for the optimization problem
-        x = opt.Variable((self.nx, self.control_horizon + 1), name="states")
-        u = opt.Variable((self.nu, self.control_horizon), name="actions")
-        cost = 0
-        constr = []
-
-        # Tracking error cost
-        for k in range(self.control_horizon):
-            cost += opt.quad_form(x[:, k + 1] - target[:, k], self.Q)
-
-        # Final point tracking cost
-        cost += opt.quad_form(x[:, -1] - target[:, -1], self.Qf)
-
-        # Actuation magnitude cost
-        for k in range(self.control_horizon):
-            cost += opt.quad_form(u[:, k], self.R)
-
-        # Actuation rate of change cost
-        for k in range(1, self.control_horizon):
-            cost += opt.quad_form(u[:, k] - u[:, k - 1], self.P)
-
-        # Kinematics Constraints
-        for k in range(self.control_horizon):
-            constr += [x[:, k + 1] == A @ x[:, k] + B @ u[:, k] + C]
-
-        # initial state
-        constr += [x[:, 0] == initial_state]
-
-        # actuation bounds
-        constr += [opt.abs(u[:, 0]) <= robot_model.max_acc]
-        constr += [opt.abs(u[:, 1]) <= robot_model.max_steer]
-
-        # Actuation rate of change bounds
-        constr += [opt.abs(u[0, 0] - prev_cmd[0]) / dt <= robot_model.max_d_acc]
-        constr += [opt.abs(u[1, 0] - prev_cmd[1]) / dt <= robot_model.max_d_steer]
-        for k in range(1, self.control_horizon):
-            constr += [opt.abs(u[0, k] - u[0, k - 1]) / dt <= robot_model.max_d_acc]
-            constr += [opt.abs(u[1, k] - u[1, k - 1]) / dt <= robot_model.max_d_steer]
-
-        prob = opt.Problem(opt.Minimize(cost), constr)
-        solution = prob.solve(solver=opt.OSQP, warm_start=True, verbose=False)
-        return x, u

guided_mrmp/planners/multirobot/db_guided_mrmp.py → guided_mrmp/planners/db_guided_mrmp.py

@@ -96,12 +96,9 @@ class GuidedMRMP:
             traj1 = desired_trajs[r1_idx]
             traj1 = list(zip(traj1[0],traj1[1]))
             
-            for r2_idx, r2 in enumerate(self.robots):
-                if r1.label == r2.label:
-                    continue
-
+            for r2_idx, r2 in enumerate(self.robots[r1_idx+1:]):
                 # control = desired_controls[r2_idx]
-                traj2 = desired_trajs[r2_idx]
+                traj2 = desired_trajs[r2_idx+r1_idx+1]
                 traj2 = list(zip(traj2[0],traj2[1]))
 
                 for p1, p2 in zip(traj1, traj2):
@@ -154,10 +151,12 @@ class GuidedMRMP:
             self.add_vis_target_traj(screen, r, x_mpc)
             
             # only the first one is used to advance the simulation
-            control = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
+            control = [u_mpc[0, 0], u_mpc[1, 0]]
             
             next_controls.append(np.asarray(control))
-            next_trajs.append(self.ego_to_global(r, x_mpc.value))
+
+            # print(f"control = {u_mpc}")
+            next_trajs.append(self.ego_to_global(r, x_mpc))
         return next_controls, next_trajs
 
 
@@ -186,16 +185,6 @@ class GuidedMRMP:
         #     for r,control in zip(conflict,new_controls):
         #         r.next_control = control
             
-        # update the state of each robot
-
-        # for idx, r in enumerate(self.robots):
-        #     control = r.next_control
-        #     r.current_position = self.dynamics_models[idx].next_state(r.current_position, control, dt)
-        #     # self.current_trajs[idx].append(r.state)
-            
-        #     r.x_history.append(r.state[0])
-        #     r.y_history.append(r.state[1])
-        #     r.h_history.append(r.state[2])
 
         # return the valid controls
         for r, next_control in zip(self.robots, next_desired_controls):
@@ -237,7 +226,7 @@ class GuidedMRMP:
         """
         Add the visualization to the screen.
         """
-        traj = self.ego_to_global(robot, traj.value)
+        traj = self.ego_to_global(robot, traj)
         for i in range(len(traj[0])-1):
             x = int(traj[0,i])
             y = int(traj[1,i])