From 68daa28cec537999197feea6b0cbfb0dfde07180 Mon Sep 17 00:00:00 2001
From: rachelmoan <moanrachel516@gmail.com>
Date: Mon, 9 Sep 2024 20:56:38 -0500
Subject: [PATCH] Restructuring code a bit
---
guided_mrmp/optimizer.py | 71 -------------------
.../{multirobot => }/db_guided_mrmp.py | 25 ++-----
2 files changed, 7 insertions(+), 89 deletions(-)
delete mode 100644 guided_mrmp/optimizer.py
rename guided_mrmp/planners/{multirobot => }/db_guided_mrmp.py (91%)
diff --git a/guided_mrmp/optimizer.py b/guided_mrmp/optimizer.py
deleted file mode 100644
index 74f07be..0000000
--- a/guided_mrmp/optimizer.py
+++ /dev/null
@@ -1,71 +0,0 @@
-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
diff --git a/guided_mrmp/planners/multirobot/db_guided_mrmp.py b/guided_mrmp/planners/db_guided_mrmp.py
similarity index 91%
rename from guided_mrmp/planners/multirobot/db_guided_mrmp.py
rename to guided_mrmp/planners/db_guided_mrmp.py
index fec2c2e..d95b523 100644
--- a/guided_mrmp/planners/multirobot/db_guided_mrmp.py
+++ b/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])
--
GitLab