modify panda collision model

CMakeLists.txt

@@ -38,8 +38,14 @@ if(NOT INSTALL_ROS_INTERFACE_ONLY)
   set(pkg1 ${PROJECT_NAME} hpp_interface.py mpc_search.py mpc.py
            trajectory_buffer.py trajectory_point.py)
   set(pkg2 ${PROJECT_NAME}/hpp_panda planner.py scenes.py)
-  set(pkg3 ${PROJECT_NAME}/main/panda main_hpp_mpc_buffer.py main_hpp_mpc.py
-           main_meshcat_display.py main_optim_traj.py main_scenes.py)
+  set(pkg3
+      ${PROJECT_NAME}/main/panda
+      main_hpp_mpc_buffer.py
+      main_hpp_mpc.py
+      main_meshcat_display.py
+      main_optim_traj.py
+      main_scenes.py
+      main_hpp_mpc_replay_ros.py)
   set(pkg4 ${PROJECT_NAME}/main/ur3 main_hpp_mpc.py)
   set(pkg5 ${PROJECT_NAME}/ocps ocp_croco_hpp.py ocp_pose_ref.py ocp.py)
   set(pkg6 ${PROJECT_NAME}/robot_model obstacle_params_parser.py panda_model.py

README.md

@@ -28,8 +28,9 @@ Different mains are available:
     - `python3 -m agimus_controller.main.ur3.main_hpp_mpc -N=1`
 - panda scripts :
     -  `python3 -m agimus_controller.main.panda.main_hpp_mpc_buffer`
-    -  `python3 -m agimus_controller.main.panda.main_hpp_mpc`
-    -  `python3 -m agimus_controller.main.panda.main_reaching_goal`
+    -  `python3 -im agimus_controller.main.panda.main_hpp_mpc`
+    -  `python3 -im agimus_controller.main.panda.main_hpp_mpc_replay_ros`
+    -  `python3 -im agimus_controller.main.panda.main_reaching_goal`
     -  `python3 -m agimus_controller.main.panda.main_meshcat_display`
     -  `python3 -m agimus_controller.main.panda.main_optim_traj`
     -  `python3 -m agimus_controller.main.panda.main_scenes`

agimus_controller/hpp_panda/scenes.py

@@ -150,21 +150,13 @@ def get_shapes_avoiding_collision(self):
         """
         if self._name_scene == "box" or "wall":
             self.shapes_avoiding_collision = [
-                "panda_link7_sc_capsule_0",
-                "panda_link7_sc_capsule_1",
-                "panda_link6_sc_capsule_0",
-                "panda_link5_sc_capsule_1",
-                "panda_link5_sc_capsule_0",
-                "panda_rightfinger_0",
-                "panda_leftfinger_0",
+                # "panda_link5_sc_capsule_0",
+                # "panda_leftfinger_0",
             ]
         elif self._name_scene == "ball":
             self.shapes_avoiding_collision = [
-                "panda_leftfinger_0",
-                "panda_rightfinger_0",
-                "panda_link6_sc_capsule_0",
-                "panda_link5_sc_capsule_0",
-                "panda_link5_sc_capsule_1",
+                # "panda_link5_sc_capsule_0",
+                # "panda_leftfinger_0",
             ]
         else:
             raise NotImplementedError(

agimus_controller/main/panda/main_hpp_mpc.py

@@ -11,6 +11,13 @@
 from agimus_controller.ocps.parameters import OCPParameters
 from agimus_controller.main.servers import Servers
 
+from agimus_controller.utils.ocp_analyzer import (
+    return_cost_vectors,
+    return_constraint_vector,
+    plot_costs_from_dic,
+    plot_constraints_from_dic,
+)
+
 
 class APP(object):
     def main(self, use_gui=False, spawn_servers=False):
@@ -43,22 +50,26 @@ def main(self, use_gui=False, spawn_servers=False):
 
         ocp = OCPCrocoHPP(rmodel, cmodel, ocp_params)
 
-        mpc = MPC(ocp, x_plan, a_plan, rmodel, cmodel)
+        self.mpc = MPC(ocp, x_plan, a_plan, rmodel, cmodel)
 
         start = time.time()
-        mpc.simulate_mpc(save_predictions=False)
+        self.mpc.simulate_mpc(save_predictions=True)
         end = time.time()
         print("Time of solving: ", end - start)
-        u_plan = mpc.ocp.get_u_plan(x_plan, a_plan)
+        costs = return_cost_vectors(self.mpc.ocp.solver, weighted=True)
+        constraint = return_constraint_vector(self.mpc.ocp.solver)
+        plot_costs_from_dic(costs)
+        plot_constraints_from_dic(constraint)
+        u_plan = self.mpc.ocp.get_u_plan(x_plan, a_plan)
         self.mpc_plots = MPCPlots(
-            croco_xs=mpc.croco_xs,
-            croco_us=mpc.croco_us,
+            croco_xs=self.mpc.croco_xs,
+            croco_us=self.mpc.croco_us,
             whole_x_plan=x_plan,
             whole_u_plan=u_plan,
             rmodel=rmodel,
             vmodel=pandawrapper.get_reduced_visual_model(),
             cmodel=cmodel,
-            DT=mpc.ocp.params.dt,
+            DT=self.mpc.ocp.params.dt,
             ee_frame_name=ee_frame_name,
             viewer=viewer,
         )

agimus_controller/main/panda/main_hpp_mpc_replay_ros.py

@@ -0,0 +1,101 @@
+import time
+import numpy as np
+from agimus_controller.hpp_interface import HppInterface
+from agimus_controller.mpc import MPC
+from agimus_controller.utils.path_finder import get_package_path, get_mpc_params_dict
+from agimus_controller.visualization.plots import MPCPlots
+from agimus_controller.ocps.ocp_croco_hpp import OCPCrocoHPP
+from agimus_controller.robot_model.panda_model import (
+    PandaRobotModel,
+    PandaRobotModelParameters,
+)
+from agimus_controller.ocps.parameters import OCPParameters
+from agimus_controller.main.servers import Servers
+
+from agimus_controller.utils.ocp_analyzer import (
+    return_cost_vectors,
+    plot_costs_from_dic,
+)
+
+
+class APP(object):
+    def main(self, use_gui=False, spawn_servers=False):
+        if spawn_servers:
+            self.servers = Servers()
+            self.servers.spawn_servers(use_gui)
+
+        panda_params = PandaRobotModelParameters()
+        panda_params.collision_as_capsule = True
+        panda_params.self_collision = False
+        agimus_demos_description_dir = get_package_path("agimus_demos_description")
+        collision_file_path = (
+            agimus_demos_description_dir / "pick_and_place" / "obstacle_params.yaml"
+        )
+        pandawrapper = PandaRobotModel.load_model(
+            params=panda_params, env=collision_file_path
+        )
+        mpc_params_dict = get_mpc_params_dict(task_name="pick_and_place")
+        ocp_params = OCPParameters()
+        ocp_params.set_parameters_from_dict(mpc_params_dict["ocp"])
+        rmodel = pandawrapper.get_reduced_robot_model()
+        cmodel = pandawrapper.get_reduced_collision_model()
+        ee_frame_name = panda_params.ee_frame_name
+
+        self.hpp_interface = HppInterface()
+        q_init, q_goal = self.hpp_interface.get_panda_q_init_q_goal()
+        self.hpp_interface.set_panda_planning(q_init, q_goal, use_gepetto_gui=use_gui)
+        viewer = self.hpp_interface.get_viewer()
+        # x_plan, a_plan, _ = hpp_interface.get_hpp_x_a_planning(1e-2)
+        hpp_traj_dict = np.load(
+            "/home/gepetto/ros_ws/src/agimus_controller/agimus_controller/main/panda/"
+            + "hpp_trajectory.npy",
+            allow_pickle=True,
+        ).item()
+        poses = hpp_traj_dict["poses"]
+        vels = hpp_traj_dict["vels"]
+        self.x_plan = np.concatenate([poses, vels], axis=1)
+        self.a_plan = hpp_traj_dict["accs"]
+
+        ocp = OCPCrocoHPP(rmodel, cmodel, ocp_params)
+
+        self.mpc = MPC(ocp, self.x_plan, self.a_plan, rmodel, cmodel)
+
+        start = time.time()
+        self.mpc.simulate_mpc(save_predictions=True)
+        end = time.time()
+        print("Time of solving: ", end - start)
+        max_kkt = max(self.mpc.mpc_data["kkt_norm"])
+        mean_kkt = np.mean(self.mpc.mpc_data["kkt_norm"])
+        mean_iter = np.mean(self.mpc.mpc_data["nb_iter"])
+        mean_solve_time = np.mean(self.mpc.mpc_data["step_time"])
+        index = self.mpc.mpc_data["kkt_norm"].index(max_kkt)
+        print(f"max kkt {max_kkt} index {index}")
+        print(f"mean kkt {mean_kkt} mean iter {mean_iter}")
+        print(f"mean solve time {mean_solve_time}")
+        costs = return_cost_vectors(self.mpc.ocp.solver, weighted=True)
+        # constraint = return_constraint_vector(self.mpc.ocp.solver)
+        plot_costs_from_dic(costs)
+        # plot_constraints_from_dic(constraint)
+        u_plan = self.mpc.ocp.get_u_plan(self.x_plan, self.a_plan)
+        self.mpc_plots = MPCPlots(
+            croco_xs=self.mpc.croco_xs,
+            croco_us=self.mpc.croco_us,
+            whole_x_plan=self.x_plan,
+            whole_u_plan=u_plan,
+            rmodel=rmodel,
+            vmodel=pandawrapper.get_reduced_visual_model(),
+            cmodel=cmodel,
+            DT=self.mpc.ocp.params.dt,
+            ee_frame_name=ee_frame_name,
+            viewer=viewer,
+        )
+        return True
+
+
+def main():
+    return APP().main(use_gui=False, spawn_servers=False)
+
+
+if __name__ == "__main__":
+    app = APP()
+    app.main(use_gui=True, spawn_servers=True)

agimus_controller/main/panda/main_reaching_goal.py

@@ -13,9 +13,7 @@
 from agimus_controller.main.servers import Servers
 from agimus_controller.utils.ocp_analyzer import (
     return_cost_vectors,
-    return_constraint_vector,
     plot_costs_from_dic,
-    plot_constraints_from_dic,
 )
 
 
@@ -47,12 +45,17 @@ def main(self, use_gui=False, spawn_servers=False):
         self.mpc.simulate_mpc(save_predictions=True)
         solver = self.mpc.ocp.solver
         costs = return_cost_vectors(solver, weighted=True)
-        constraint = return_constraint_vector(solver)
+        # constraint = return_constraint_vector(solver)
         plot_costs_from_dic(costs)
-        plot_constraints_from_dic(constraint)
+        # plot_constraints_from_dic(constraint)
         max_kkt = max(self.mpc.mpc_data["kkt_norm"])
+        mean_kkt = np.mean(self.mpc.mpc_data["kkt_norm"])
+        mean_iter = np.mean(self.mpc.mpc_data["nb_iter"])
+        mean_solve_time = np.mean(self.mpc.mpc_data["solve_time"])
         index = self.mpc.mpc_data["kkt_norm"].index(max_kkt)
         print(f"max kkt {max_kkt} index {index}")
+        print(f"mean kkt {mean_kkt} mean iter {mean_iter}")
+        print(f"mean solve time {mean_solve_time}")
         end = time.time()
         print("Time of solving: ", end - start)
         ee_frame_name = ocp_params.effector_frame_name

agimus_controller/mpc.py

@@ -1,5 +1,6 @@
 from __future__ import annotations
 import numpy as np
+import time
 from agimus_controller.utils.pin_utils import get_ee_pose_from_configuration
 
 
@@ -54,7 +55,7 @@ def get_predictions(self):
 
     def get_collision_residuals(self):
         constraints_residual_dict = self.ocp.solver.problem.runningDatas[
-            0
+            1
         ].differential.constraints.constraints.todict()
         constraints_values = {}
         for constraint_key in constraints_residual_dict.keys():
@@ -74,13 +75,15 @@ def simulate_mpc(self, save_predictions=False):
         x_plan = self.whole_x_plan[:T, :]
         a_plan = self.whole_a_plan[:T, :]
         self.ocp.set_planning_variables(x_plan, a_plan)
+        start_mpc_step_time = time.time()
         x, u0 = self.mpc_first_step(x_plan, self.ocp.u_plan[: T - 1], x0)
+        mpc_step_time = time.time() - start_mpc_step_time
         mpc_xs[1, :] = x
         mpc_us[0, :] = u0
         next_node_idx = T
         if save_predictions:
-            self.create_mpc_data()
-        for idx in range(1, self.whole_traj_T - 1):
+            self.create_mpc_data(step_time=mpc_step_time)
+        for idx in range(1, self.whole_traj_T - 1):  # self.whole_traj_T - 1
             x_plan = self.update_planning(x_plan, self.whole_x_plan[next_node_idx, :])
             a_plan = self.update_planning(a_plan, self.whole_a_plan[next_node_idx, :])
             placement_ref = get_ee_pose_from_configuration(
@@ -89,13 +92,15 @@ def simulate_mpc(self, save_predictions=False):
                 self.ocp._effector_frame_id,
                 x_plan[-1, : self.nq],
             )
+            start_mpc_step_time = time.time()
             x, u = self.mpc_step(x, x_plan[-1], a_plan[-1], placement_ref)
+            mpc_step_time = time.time() - start_mpc_step_time
             if next_node_idx < self.whole_x_plan.shape[0] - 1:
                 next_node_idx += 1
             mpc_xs[idx + 1, :] = x
             mpc_us[idx, :] = u
             if save_predictions:
-                self.fill_predictions_and_refs_arrays()
+                self.fill_predictions_and_refs_arrays(step_time=mpc_step_time)
         self.croco_xs = mpc_xs
         self.croco_us = mpc_us
         if save_predictions:
@@ -135,7 +140,7 @@ def mpc_step(self, x0, new_x_ref, new_a_ref, placement_ref):
         x0 = self.get_next_state(x0, self.ocp.solver.problem)
         return x0, self.ocp.solver.us[0]
 
-    def create_mpc_data(self):
+    def create_mpc_data(self, step_time=None):
         xs, us = self.get_predictions()
         x_ref, p_ref, u_ref = self.get_reference()
         self.mpc_data["preds_xs"] = [xs]
@@ -145,11 +150,15 @@ def create_mpc_data(self):
         self.mpc_data["control_refs"] = [u_ref]
         self.mpc_data["kkt_norm"] = [self.ocp.solver.KKT]
         self.mpc_data["nb_iter"] = [self.ocp.solver.iter]
+        self.mpc_data["nb_qp_iter"] = [self.ocp.solver.qp_iters]
+
         if self.ocp.params.use_constraints:
             collision_residuals = self.get_collision_residuals()
             self.mpc_data["coll_residuals"] = collision_residuals
+        if step_time is not None:
+            self.mpc_data["step_time"] = [step_time]
 
-    def fill_predictions_and_refs_arrays(self):
+    def fill_predictions_and_refs_arrays(self, step_time=None):
         xs, us = self.get_predictions()
         x_ref, p_ref, u_ref = self.get_reference()
         self.mpc_data["preds_xs"].append(xs)
@@ -159,12 +168,15 @@ def fill_predictions_and_refs_arrays(self):
         self.mpc_data["control_refs"].append(u_ref)
         self.mpc_data["kkt_norm"].append(self.ocp.solver.KKT)
         self.mpc_data["nb_iter"].append(self.ocp.solver.iter)
+        self.mpc_data["nb_qp_iter"].append(self.ocp.solver.qp_iters)
         if self.ocp.params.use_constraints:
             collision_residuals = self.get_collision_residuals()
             for coll_residual_key in collision_residuals.keys():
                 self.mpc_data["coll_residuals"][coll_residual_key] += (
                     collision_residuals[coll_residual_key]
                 )
+        if step_time is not None:
+            self.mpc_data["step_time"].append(step_time)
 
     def get_ee_pred_from_xs_pred(self, xs):
         pred_ee = np.zeros((xs.shape[0], 3))