Merge pull request #88 from TheoMF/topic/tmartinez/new_ocp

Topic/tmartinez/new ocp

CMakeLists.txt

@@ -41,7 +41,7 @@ if(NOT INSTALL_ROS_INTERFACE_ONLY)
   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(pkg4 ${PROJECT_NAME}/main/ur3 main_hpp_mpc.py)
-  set(pkg5 ${PROJECT_NAME}/ocps ocp_croco_hpp.py ocp.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
            robot_model.py ur3_model.py)
   set(pkg7 ${PROJECT_NAME}/utils iostream.py ocp_analyzer.py pin_utils.py
@@ -84,7 +84,8 @@ if(BUILD_ROS_INTERFACE)
                  ${MY_PYTHON_DEPENDENCIES})
   catkin_install_python(
     PROGRAMS node/agimus_controller_node node/hpp_agimus_controller_node
-    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+    node/reaching_goal_controller_node DESTINATION
+    ${CATKIN_PACKAGE_BIN_DESTINATION})
   install(PROGRAMS node/hpp_corbaserver
           DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/node)
   install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

agimus_controller/main/panda/main_reaching_goal.py

@@ -0,0 +1,83 @@
+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_mpc_params_dict
+from agimus_controller.visualization.plots import MPCPlots
+from agimus_controller.ocps.ocp_pose_ref import OCPPoseRef
+from agimus_controller.robot_model.panda_model import (
+    get_task_models,
+    get_robot_constructor,
+)
+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):
+        if spawn_servers:
+            self.servers = Servers()
+            self.servers.spawn_servers(use_gui)
+
+        rmodel, cmodel, vmodel = get_task_models(task_name="reaching_goal")
+        self.robot_constructor = get_robot_constructor(task_name="reaching_goal")
+        mpc_params_dict = get_mpc_params_dict(task_name="reaching_goal")
+        ocp_params = OCPParameters()
+        ocp_params.set_parameters_from_dict(mpc_params_dict["ocp"])
+
+        hpp_interface = HppInterface()
+        q_init, q_goal = hpp_interface.get_panda_q_init_q_goal()
+        hpp_interface.set_panda_planning(q_init, q_goal, use_gepetto_gui=use_gui)
+        viewer = hpp_interface.get_viewer()
+        x0 = q_init + [0] * 7
+        length = 500
+        x_plan = np.array(x0 * length)
+        x_plan = np.reshape(x_plan, (length, 14))
+        a_plan = np.zeros((length, 7))
+        ocp = OCPPoseRef(rmodel, cmodel, ocp_params, np.array(q_goal))
+
+        self.mpc = MPC(ocp, x_plan, a_plan, rmodel, cmodel)
+        start = time.time()
+        self.mpc.simulate_mpc(save_predictions=True)
+        solver = self.mpc.ocp.solver
+        costs = return_cost_vectors(solver, weighted=True)
+        constraint = return_constraint_vector(solver)
+        plot_costs_from_dic(costs)
+        plot_constraints_from_dic(constraint)
+        max_kkt = max(self.mpc.mpc_data["kkt_norm"])
+        index = self.mpc.mpc_data["kkt_norm"].index(max_kkt)
+        print(f"max kkt {max_kkt} index {index}")
+        end = time.time()
+        print("Time of solving: ", end - start)
+        ee_frame_name = ocp_params.effector_frame_name
+        self.mpc_plots = MPCPlots(
+            croco_xs=self.mpc.croco_xs,
+            croco_us=self.mpc.croco_us,
+            whole_x_plan=x_plan,
+            whole_u_plan=np.zeros((length - 1, 7)),
+            rmodel=rmodel,
+            vmodel=vmodel,
+            cmodel=cmodel,
+            DT=self.mpc.ocp.params.dt,
+            ee_frame_name=ee_frame_name,
+            viewer=viewer,
+        )
+
+        if use_gui:
+            self.mpc_plots.display_path_gepetto_gui()
+        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)