Merge branch 'develop' of github.com:pollen-robotics/pollen-manipulat…

…ion into develop

src/pollen_manipulation/reachy_2/api.py

@@ -49,6 +49,10 @@ def __init__(
         self.right_start_pose = fv_utils.make_pose([0.20, -0.24, -0.23], [0, -90, 0])
         self.left_start_pose = fv_utils.make_pose([0.20, 0.24, -0.23], [0, -90, 0])
 
+        self.last_pregrasp_pose: npt.NDArray[np.float64] = np.eye(4)
+        self.last_grasp_pose: npt.NDArray[np.float64] = np.eye(4)
+        self.last_lift_pose: npt.NDArray[np.float64] = np.eye(4)
+
         self.grasp_net = ContactGraspNetWrapper()
 
     def ask_simu_preview(self) -> str:
@@ -334,7 +338,7 @@ def synchro_simu_joints(self):
 
     def _execute_grasp(
         self,
-        grasp_pose: npt.NDArray[np.float32],
+        grasp_pose: npt.NDArray[np.float64],
         duration: float,
         left: bool = False,
         use_cartesian_interpolation: bool = True,
@@ -401,6 +405,10 @@ def _execute_grasp(
         # while not self.reachy.is_move_finished(goto_id):
         #     time.sleep(0.1)
 
+        self.last_pregrasp_pose = pregrasp_pose
+        self.last_grasp_pose = grasp_pose
+        self.last_lift_pose = lift_pose
+
         return True
 
     # TODO: Implement this method
@@ -518,33 +526,50 @@ def goto_rest_position(
         goto_duration: float = 4.0,
         use_cartesian_interpolation: bool = True,
         play_in_simu: bool = False,
-    ) -> None:
+    ) -> bool:
 
         if self.simu_preview and play_in_simu:
             self.reachy = self.reachy_simu
         else:
             self.reachy = self.reachy_real
 
         if not left:
-            goto_id = self.reachy.r_arm.goto_from_matrix(
-                self.right_start_pose,
-                duration=goto_duration,
-                with_cartesian_interpolation=use_cartesian_interpolation,
-            )
+            arm = self.reachy.r_arm
+            start_pose = self.right_start_pose
         else:
-            goto_id = self.reachy.l_arm.goto_from_matrix(
-                self.left_start_pose,
-                duration=goto_duration,
-                with_cartesian_interpolation=use_cartesian_interpolation,
+            arm = self.reachy.l_arm
+            start_pose = self.left_start_pose
+
+        if np.array_equal(start_pose, np.eye(4)):
+            arm.goto_from_matrix(
+                start_pose, duration=goto_duration * 2.0, with_cartesian_interpolation=use_cartesian_interpolation
             )
 
-        if goto_id.id != 0:
-            while not self.reachy.is_move_finished(goto_id):
-                time.sleep(0.1)
+            if open_gripper:
+                self.open_gripper(left=left)
+            return True
+
+        arm.goto_from_matrix(
+            target=self.last_lift_pose, duration=goto_duration, with_cartesian_interpolation=use_cartesian_interpolation
+        )
+
+        arm.goto_from_matrix(
+            target=self.last_grasp_pose, duration=goto_duration, with_cartesian_interpolation=use_cartesian_interpolation
+        )
+
+        arm.goto_from_matrix(
+            target=self.last_pregrasp_pose, duration=goto_duration, with_cartesian_interpolation=use_cartesian_interpolation
+        )
+
+        arm.goto_from_matrix(
+            start_pose, duration=goto_duration, with_cartesian_interpolation=use_cartesian_interpolation
+        )
 
         if open_gripper:
             self.open_gripper(left=left)
 
+        return True
+
     def open_gripper(self, left: bool = False, play_in_simu: bool = False) -> None:
         if self.simu_preview and play_in_simu:
             self.reachy = self.reachy_simu