From a5832b6241ae6b84e9993438d292e753623b3a5c Mon Sep 17 00:00:00 2001 From: apirrone Date: Thu, 20 Jun 2024 11:43:22 +0200 Subject: [PATCH] simu preview --- .../Grasp_poses_visualization_Reachy2.ipynb | 2 +- src/pollen_manipulation/reachy_1/api.py | 15 ++ src/pollen_manipulation/reachy_2/api.py | 166 ++++++++++++++++-- 3 files changed, 165 insertions(+), 18 deletions(-) diff --git a/examples/notebooks/Grasp_poses_visualization_Reachy2.ipynb b/examples/notebooks/Grasp_poses_visualization_Reachy2.ipynb index 77227b4..166fa9e 100644 --- a/examples/notebooks/Grasp_poses_visualization_Reachy2.ipynb +++ b/examples/notebooks/Grasp_poses_visualization_Reachy2.ipynb @@ -361,7 +361,7 @@ "metadata": {}, "outputs": [], "source": [ - "manipulation_api.execute_grasp(grasp_poses[0], duration=2.0, left=True, use_cartesian_interpolation=True)" + "manipulation_api._execute_grasp(grasp_poses[0], duration=2.0, left=True, use_cartesian_interpolation=True)" ] }, { diff --git a/src/pollen_manipulation/reachy_1/api.py b/src/pollen_manipulation/reachy_1/api.py index f485436..6eab257 100644 --- a/src/pollen_manipulation/reachy_1/api.py +++ b/src/pollen_manipulation/reachy_1/api.py @@ -1,3 +1,18 @@ +# DEPRECATED ?? +# DEPRECATED ?? +# DEPRECATED ?? +# DEPRECATED ?? +# DEPRECATED ?? +# DEPRECATED ?? +# DEPRECATED ?? +# DEPRECATED ?? +# DEPRECATED ?? +# DEPRECATED ?? +# DEPRECATED ?? +# DEPRECATED ?? +# DEPRECATED ?? +# It's been a long time since we used this. + import time from threading import Thread from typing import Any, Dict, List, Tuple diff --git a/src/pollen_manipulation/reachy_2/api.py b/src/pollen_manipulation/reachy_2/api.py index 01aa1e9..a385bb8 100644 --- a/src/pollen_manipulation/reachy_2/api.py +++ b/src/pollen_manipulation/reachy_2/api.py @@ -17,8 +17,31 @@ class Reachy2ManipulationAPI: - def __init__(self, reachy: ReachySDK, T_world_cam: npt.NDArray[np.float32], K_cam_left: npt.NDArray[np.float32]): - self.reachy = reachy + def __init__( + self, + reachy: ReachySDK, + T_world_cam: npt.NDArray[np.float32], + K_cam_left: npt.NDArray[np.float32], + simu_preview: bool = True, + ) -> None: + self.reachy_real = reachy + self.reachy = self.reachy_real + self.simu_preview = simu_preview + + # TODO Maybe remove this if it is too annoying + if not self.simu_preview: + inp = input("Warning, simu preview is disabled, continue ? (N/y)") + if inp.lower() != "y": + raise ValueError("Cancelling") + + if self.simu_preview: + if self.reachy._host == "localhost": + raise ValueError("Simu preview is not available the main robot is localhost") + self.reachy_simu = ReachySDK("localhost") + if not self.reachy_simu.is_connected(): + raise ValueError("Simu preview is not available, cannot connect to the simu") + + self.reachy = self.reachy_simu self.T_world_cam = T_world_cam self.K_cam_left = K_cam_left @@ -28,6 +51,26 @@ def __init__(self, reachy: ReachySDK, T_world_cam: npt.NDArray[np.float32], K_ca self.grasp_net = ContactGraspNetWrapper() + def ask_simu_preview(self) -> str: + """ + If self.simu_preview is True, asks the user if they want to run the move on the simu robot before running it on the real robot. + Returns True if the user has chosen to run the move on the simu robot, False otherwise. + """ + ret = "real" + if not self.simu_preview: + return ret + + inp = input("Run the move on the simu robot before running it on the real robot ? (Y/n/s[kip]): ") + if inp.lower() == "n": + print("Ok, running the move on the real robot") + ret = "real" + elif inp.lower() == "s" or inp.lower() == "skip": + print("Ok, skipping the move") + ret = "skip" + else: + ret = "simu" + return ret + def grasp_object( self, object_info: Dict[str, Any], @@ -64,9 +107,24 @@ def grasp_object( arm.publish_grasp_poses([grasp_pose], [score]) print("GRASP POSE selected: ", grasp_pose, "SCORE: ", score) - grasp_success = self.execute_grasp( + simu = self.ask_simu_preview() + while simu == "simu": # while the user wants to run the move on the simu robot + grasp_success = self._execute_grasp( + grasp_pose, + left=left, + duration=grasp_gotos_duration, + use_cartesian_interpolation=use_cartesian_interpolation, + play_in_simu=True, + ) + simu = self.ask_simu_preview() + + if simu == "skip": + return False + + grasp_success = self._execute_grasp( grasp_pose, left=left, duration=grasp_gotos_duration, use_cartesian_interpolation=use_cartesian_interpolation ) + return grasp_success def _get_euler_from_homogeneous_matrix( @@ -157,11 +215,11 @@ def get_reachable_grasp_poses( # noqa: C901 # if dist_top != 0.0: # orientation_score /= np.abs(dist_top) if np.isnan(dist_front): - print(f'NAN dist_front: {T_world_graspPose_sym[:3, :3]}') - orientation_score*=0.000001 + print(f"NAN dist_front: {T_world_graspPose_sym[:3, :3]}") + orientation_score *= 0.000001 if dist_front == 0.0: - print(f'Perfect dist_front?!: {T_world_graspPose_sym[:3, :3]}') + print(f"Perfect dist_front?!: {T_world_graspPose_sym[:3, :3]}") if dist_front != 0.0 and not np.isnan(dist_front): orientation_score /= 1 + np.abs(dist_front) @@ -195,11 +253,11 @@ def get_reachable_grasp_poses( # noqa: C901 # if dist_top != 0.0: # orientation_score /= np.abs(dist_top) if np.isnan(dist_front): - print(f'NAN dist_front sym: {T_world_graspPose_sym[:3, :3]}') - orientation_score*=0.000001 + print(f"NAN dist_front sym: {T_world_graspPose_sym[:3, :3]}") + orientation_score *= 0.000001 if dist_front == 0.0: - print(f'Perfect dist_front sym?!: {T_world_graspPose_sym[:3, :3]}') + print(f"Perfect dist_front sym?!: {T_world_graspPose_sym[:3, :3]}") if dist_front != 0.0 and not np.isnan(dist_front): orientation_score /= 1 + np.abs(dist_front) @@ -258,14 +316,22 @@ def get_reachable_grasp_poses( # noqa: C901 print(f"Number of reachable grasp poses: {len(reachable_grasp_poses)}") return reachable_grasp_poses, reachable_scores, all_grasp_poses, all_scores - def execute_grasp( + def _execute_grasp( self, grasp_pose: npt.NDArray[np.float32], duration: float, left: bool = False, use_cartesian_interpolation: bool = True, + play_in_simu: bool = False, ) -> bool: - print("Executing grasp") + + if self.simu_preview and play_in_simu: + self.reachy = self.reachy_simu + # TODO set simu reachy to the same state as the real one + else: + self.reachy = self.reachy_real + + print("Executing grasp in ", "simu" if play_in_simu else "real robot") grasp_pose = fv_utils.translateInSelf( grasp_pose, [0, 0, -0.0584] ) # Graspnet returns the base of the gripper mesh, we translate to get the base of the opening @@ -324,14 +390,47 @@ def execute_grasp( def drop_object(self) -> bool: return True - # TODO: Implement this method - def place( + def place_object( + self, + target_pose: npt.NDArray[np.float32], + place_height: float = 0.0, + duration: float = 4, + left: bool = False, + use_cartesian_interpolation: bool = True, + ) -> bool: + + simu = self.ask_simu_preview() + while simu == "simu": # while the user wants to run the move on the simu robot + place_success = self._place( + target_pose, + place_height=place_height, + duration=duration, + left=left, + use_cartesian_interpolation=use_cartesian_interpolation, + play_in_simu=True, + ) + simu = self.ask_simu_preview() + + if simu == "skip": + return False + + place_success = self._place( + target_pose, + place_height=place_height, + duration=duration, + left=left, + use_cartesian_interpolation=use_cartesian_interpolation, + ) + return place_success + + def _place( self, target_pose: npt.NDArray[np.float32], place_height: float = 0.0, duration: float = 4, left: bool = False, use_cartesian_interpolation: bool = True, + play_in_simu: bool = False, ) -> bool: """ Moves the arm to the target pose and then opens the gripper @@ -346,6 +445,14 @@ def place( """ + if self.simu_preview and play_in_simu: + self.reachy = self.reachy_simu + # TODO set simu reachy to the same state as the real one + else: + self.reachy = self.reachy_real + + print("Executing place in ", "simu" if play_in_simu else "real robot") + target_pose = np.array(target_pose).reshape(4, 4) # just in case :) # 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) @@ -393,7 +500,14 @@ def goto_rest_position( open_gripper: bool = True, goto_duration: float = 4.0, use_cartesian_interpolation: bool = True, + play_in_simu: bool = False, ) -> None: + + 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, @@ -414,20 +528,38 @@ def goto_rest_position( if open_gripper: self.open_gripper(left=left) - def open_gripper(self, left: bool = False) -> None: + 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 + else: + self.reachy = self.reachy_real + if left: self.reachy.l_arm.gripper.open() else: self.reachy.r_arm.gripper.open() - def close_gripper(self, left: bool = False) -> None: + def close_gripper(self, left: bool = False, play_in_simu: bool = False) -> None: + if self.simu_preview and play_in_simu: + self.reachy = self.reachy_simu + else: + self.reachy = self.reachy_real + if left: self.reachy.l_arm.gripper.close() else: self.reachy.r_arm.gripper.close() - def turn_robot_on(self) -> None: + def turn_robot_on(self, play_in_simu: bool = False) -> None: + if self.simu_preview and play_in_simu: + self.reachy = self.reachy_simu + else: + self.reachy = self.reachy_real self.reachy.turn_on() - def stop(self) -> None: + def stop(self, play_in_simu: bool = False) -> None: + if self.simu_preview and play_in_simu: + self.reachy = self.reachy_simu + else: + self.reachy = self.reachy_real self.reachy.turn_off_smoothly()