From 7f58651013a335bfc2525ce710e3abea5a263f34 Mon Sep 17 00:00:00 2001 From: apirrone Date: Sun, 23 Jun 2024 15:40:06 +0200 Subject: [PATCH] update --- src/pollen_manipulation/reachy_2/api.py | 78 +++++++++++-------- ...arallel_grasp_pose_reachability_checker.py | 10 +-- src/pollen_manipulation/utils.py | 36 ++++++++- 3 files changed, 84 insertions(+), 40 deletions(-) diff --git a/src/pollen_manipulation/reachy_2/api.py b/src/pollen_manipulation/reachy_2/api.py index a9d15aa..dfe7358 100644 --- a/src/pollen_manipulation/reachy_2/api.py +++ b/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() diff --git a/src/pollen_manipulation/reachy_2/parallel_grasp_pose_reachability_checker.py b/src/pollen_manipulation/reachy_2/parallel_grasp_pose_reachability_checker.py index 436f777..e99b13b 100644 --- a/src/pollen_manipulation/reachy_2/parallel_grasp_pose_reachability_checker.py +++ b/src/pollen_manipulation/reachy_2/parallel_grasp_pose_reachability_checker.py @@ -72,13 +72,13 @@ 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) @@ -86,7 +86,7 @@ def check_grasp_pose_reachability( # 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 diff --git a/src/pollen_manipulation/utils.py b/src/pollen_manipulation/utils.py index 96ccda2..5616c5a 100644 --- a/src/pollen_manipulation/utils.py +++ b/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