update

src/pollen_manipulation/reachy_2/api.py

@@ -49,6 +49,13 @@ def __init__(
         simu_preview: bool = True,
     ) -> None:
         self.reachy_real = reachy
+        ok = False
+        while not ok:
+            try:
+                ok = self.reachy_real.l_arm != None
+            except Exception as e:
+                print("Error while getting the real arm", e)
+                print("retrying")
         # self.reachy = self.reachy_real
         self.simu_preview = simu_preview
         self.pgprc = ParallelGraspPoseReachabilityChecker()
@@ -70,7 +77,16 @@ def __init__(
             if not self.reachy_simu.is_connected():
                 raise ValueError("Simu preview is not available, cannot connect to the simu")
             self.reachy_simu.turn_on()  # turn on the simu robot by default
-            time.sleep(5)
+            while not self.reachy_simu.is_on():
+                time.sleep(0.1)
+            time.sleep(1)
+            ok = False
+            while not ok:
+                try:
+                    ok = self.reachy_simu.l_arm != None
+                except Exception as e:
+                    print("Error while getting the simu arm", e)
+                    print("retrying")
 
         self.T_world_cam = T_world_cam
         self.K_cam_left = K_cam_left
@@ -245,21 +261,9 @@ def get_reachable_grasp_poses(  # noqa: C901
                 # front[2][0]=1
 
                 if not left:
-                    front = np.array(
-                        [
-                            [0, -0.71, -0.71],
-                            [0, 0.71, -0.71],
-                            [1, 0, 0],
-                        ]
-                    )
+                    front = np.array([[0.0, -0.7071068, -0.7071068], [0.0, 0.7071068, -0.7071068], [1.0, 0.0, 0.0]])
                 else:
-                    front = np.array(
-                        [
-                            [0, 0.71, -0.71],
-                            [0, 0.71, 0.71],
-                            [1, 0, 0],
-                        ]
-                    )
+                    front = np.array([[0.0, 0.7071068, -0.7071068], [0.0, 0.7071068, 0.7071068], [1.0, -0.0, 0.0]])
 
                 dist_front = get_angle_dist(T_world_graspPose[:3, :3], front)
 
@@ -390,17 +394,17 @@ def get_reachable_grasp_poses(  # noqa: C901
         return reachable_grasp_poses, reachable_scores, all_grasp_poses, all_scores
 
     def synchro_simu_joints(self) -> None:
-        l_real_joints = self.reachy_real.l_arm.get_joints_positions()
-        l_gripper_opening = self.reachy_real.l_arm.gripper.opening
+        l_real_joints = self.get_reachy().l_arm.get_joints_positions()
+        l_gripper_opening = self.get_reachy().l_arm.gripper.opening
 
-        r_real_joints = self.reachy_real.r_arm.get_joints_positions()
-        r_gripper_opening = self.reachy_real.r_arm.gripper.opening
+        r_real_joints = self.get_reachy().r_arm.get_joints_positions()
+        r_gripper_opening = self.get_reachy().r_arm.gripper.opening
 
-        self.reachy_simu.l_arm.goto_joints(l_real_joints, duration=0.1)
-        self.reachy_simu.r_arm.goto_joints(r_real_joints, duration=0.1)
+        self.get_reachy(simu=True).l_arm.goto_joints(l_real_joints, duration=0.1)
+        self.get_reachy(simu=True).r_arm.goto_joints(r_real_joints, duration=0.1)
 
-        self.reachy_simu.l_arm.gripper.set_opening(l_gripper_opening)
-        self.reachy_simu.r_arm.gripper.set_opening(r_gripper_opening)
+        self.get_reachy(simu=True).l_arm.gripper.set_opening(l_gripper_opening)
+        self.get_reachy(simu=True).r_arm.gripper.set_opening(r_gripper_opening)
         time.sleep(0.2)
 
     def _execute_grasp(
@@ -504,6 +508,7 @@ def place_object(
         left: bool = False,
         use_cartesian_interpolation: bool = True,
         x_offset: float = 0.0,
+        keep_orientation=False,
     ) -> bool:
 
         target_pose[:3, 3][0] += x_offset
@@ -517,6 +522,7 @@ def place_object(
                 left=left,
                 use_cartesian_interpolation=use_cartesian_interpolation,
                 play_in_simu=True,
+                keep_orientation=True,
             )
             simu = self.ask_simu_preview()
 
@@ -529,6 +535,7 @@ def place_object(
             duration=duration,
             left=left,
             use_cartesian_interpolation=use_cartesian_interpolation,
+            keep_orientation=True,
         )
         return place_success
 
@@ -540,6 +547,7 @@ def _place(
         left: bool = False,
         use_cartesian_interpolation: bool = True,
         play_in_simu: bool = False,
+        keep_orientation=False,
     ) -> bool:
         """
         Moves the arm to the target pose and then opens the gripper
@@ -562,22 +570,28 @@ def _place(
 
         target_pose = np.array(target_pose).reshape(4, 4)  # just in case :)
 
+        if left:
+            arm = self.get_reachy(simu=simu).l_arm
+        else:
+            arm = self.get_reachy(simu=simu).r_arm
+
         # The rotation component of the pose is the identity matrix, but the gripper's frame has a rotation (z aligned with the forearm, x orthogonal to the fingers)
         # We need to rotate it
-        if left:
-            target_pose[:3, :3] = self.left_start_pose[:3, :3]
+
+        if keep_orientation:
+            current_orentation = arm.forward_kinematics()[:3, :3]
+            target_pose[:3, :3] = current_orentation
         else:
-            target_pose[:3, :3] = self.right_start_pose[:3, :3]
+            if left:
+                target_pose[:3, :3] = self.left_start_pose[:3, :3]
+            else:
+                target_pose[:3, :3] = self.right_start_pose[:3, :3]
+
         target_pose[:3, 3] += np.array([0, 0, place_height])
 
         if np.linalg.norm(target_pose[:3, 3]) > 1.0 or target_pose[:3, 3][0] < 0.0:  # safety check
             raise ValueError("Target pose is too far away (norm > 1.0) or x < 0.0")
 
-        if left:
-            arm = self.get_reachy(simu=simu).l_arm
-        else:
-            arm = self.get_reachy(simu=simu).r_arm
-
         target_pose = find_close_reachable_pose(target_pose, self.pgprc.check_grasp_pose_reachability, left=left)
         # target_pose = find_close_reachable_pose(target_pose, self._is_pose_reachable, left=left)
         if target_pose is None:
@@ -697,6 +711,8 @@ def close_gripper(self, left: bool = False, play_in_simu: bool = False) -> None:
 
     def turn_robot_on(self) -> None:
         self.get_reachy().turn_on()
+        while not self.get_reachy().is_on():
+            time.sleep(0.1)
 
     def stop(self) -> None:
         self.get_reachy().turn_off_smoothly()

src/pollen_manipulation/reachy_2/parallel_grasp_pose_reachability_checker.py

@@ -72,21 +72,21 @@ def check_n_grasp_poses_reachability(
 
     def check_grasp_pose_reachability(
         self,
-        grasp_pose: npt.NDArray[np.float32],
+        pose: npt.NDArray[np.float32],
         left: bool = True,
     ) -> bool:
-        pregrasp_pose = grasp_pose.copy()
-        pregrasp_pose = fv_utils.translateInSelf(grasp_pose, [0, 0, 0.1])
+        pregrasp_pose = pose.copy()
+        pregrasp_pose = fv_utils.translateInSelf(pose, [0, 0, 0.1])
 
-        lift_pose = grasp_pose.copy()
+        lift_pose = pose.copy()
         lift_pose[:3, 3] += np.array([0, 0, 0.10])  # warning, was 0.20
 
         pregrasp_pose_reachable = self.is_reachable(pregrasp_pose, left)
         # if not pregrasp_pose_reachable:
         #     print(f"\t pregrasp not reachable")
         #     return False
 
-        grasp_pose_reachable = self.is_reachable(grasp_pose, left)
+        grasp_pose_reachable = self.is_reachable(pose, left)
         # if not grasp_pose_reachable:
         #     print(f"\t grasp not reachable")
         #     return False

src/pollen_manipulation/utils.py

@@ -1,8 +1,10 @@
+import time
 from typing import Callable, Optional
 
 import FramesViewer.utils as fv_utils
 import numpy as np
 import numpy.typing as npt
+from FramesViewer.viewer import Viewer
 from scipy.spatial.transform import Rotation as R
 
 
@@ -37,12 +39,38 @@ def find_close_reachable_pose(
     if reachable:
         return pose
 
-    for theta_x in range(0, 360):
-        # rotate 1 degree around x axis
-        reachable = reachability_function(pose, left)
+    print("=== Could not place using the current pose")
+    # If the current pose (that uses the current pose) is not reachable, revert to previous strategy
+
+    if not left:
+        rot = np.array([[0.0, -0.7071068, -0.7071068], [0.0, 0.7071068, -0.7071068], [1.0, 0.0, 0.0]])
+    else:
+        rot = np.array([[0.0, 0.7071068, -0.7071068], [0.0, 0.7071068, 0.7071068], [1.0, -0.0, 0.0]])
+
+    pose[:3, :3] = rot
+
+    fv = Viewer()
+    fv.start()
+
+    rot_tol = 20  # deg
+
+    for i in range(100):
+        thetas = (np.random.rand(2) - 0.5) * 2 * rot_tol
+        thetas = np.hstack((thetas, [0]))
+
+        pose_candidate = fv_utils.rotateInSelf(pose.copy(), thetas, degrees=True)
+        reachable = reachability_function(pose_candidate, left)
         if reachable:
             return pose
-        pose = fv_utils.rotateInSelf(pose, [-1 if left else 1, 0, 0], degrees=True)
+        fv.pushFrame(pose_candidate, "truc")
+        time.sleep(0.1)
+
+    # for theta_x in range(0, 40):
+    #     # rotate 1 degree around x axis
+    #     reachable = reachability_function(pose, left)
+    #     if reachable:
+    #         return pose
+    #     pose = fv_utils.rotateInSelf(pose.copy(), [-1 if left else 1, 0, 0], degrees=True)
 
     return None