Skip to content

Commit

Permalink
update
Browse files Browse the repository at this point in the history
  • Loading branch information
@apirrone
apirrone committed Jun 23, 2024
1 parent 6120a78 commit b93455e
Show file tree
Hide file tree
Showing 2 changed files with 121 additions and 63 deletions.
179 changes: 121 additions & 58 deletions src/pollen_manipulation/reachy_2/api.py
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
import time
from copy import deepcopy
from enum import Enum
from threading import Thread
from typing import Any, Dict, List, Tuple

import cv2
Expand All @@ -8,7 +10,6 @@
import numpy.typing as npt
from contact_graspnet_pytorch.wrapper import ContactGraspNetWrapper
from reachy2_sdk import ReachySDK
from threading import Thread

from pollen_manipulation.reachy_2.parallel_grasp_pose_reachability_checker import (
ParallelGraspPoseReachabilityChecker,
Expand Down Expand Up @@ -72,7 +73,7 @@ def __init__(
raise ValueError("Cancelling")

if self.simu_preview:
if self.reachy._host == "localhost":
if self.get_reachy()._host == "localhost":
raise ValueError("Simu preview is not available the main robot is localhost")
self.reachy_simu = ReachySDK("localhost", with_synchro=False)
if not self.reachy_simu.is_connected():
Expand Down Expand Up @@ -104,23 +105,16 @@ def __init__(
# Effector tracking with the head
self.is_using_left_arm = False
self._reset_head = False
self._effector_head_tracking_thread = Thread(target=self._effector_head_tracking)
self._start_thread()
self._effector_head_tracking_thread.start()
# self._effector_head_tracking_thread = Thread(target=self._effector_head_tracking)
# self._start_thread()
# self._effector_head_tracking_thread.start()

def _start_thread(self) -> None:
self._track_effector = True
# def _start_thread(self) -> None:
# self._track_effector = True

def _stop_thread(self) -> None:
self._track_effector = False
self._effector_head_tracking_thread.join()

@property
def reachy(self, simu: bool = False) -> ReachySDK:
if simu:
assert self.simu_preview == True
return self.reachy_simu
return self.reachy_real
# def _stop_thread(self) -> None:
# self._track_effector = False
# self._effector_head_tracking_thread.join()

def ask_simu_preview(self) -> str:
"""
Expand Down Expand Up @@ -175,22 +169,25 @@ def grasp_object(
grasp_pose = grasp_poses[0]
score = scores[0]

if left:
arm = self.get_reachy(simu=False).l_arm
else:
arm = self.get_reachy(simu=False).r_arm

try:
arm.publish_grasp_poses([grasp_pose], [score])
except Exception as e:
print("Error while publishing grasp poses: ", e)
# try:
# if left:
# self.get_reachy(simu=False).l_arm.publish_grasp_poses([grasp_pose], [score])
# else:
# self.get_reachy(simu=False).r_arm.publish_grasp_poses([grasp_pose], [score])
# except Exception as e:
# print("Error while publishing grasp poses: ", e)

print("GRASP POSE selected: ", grasp_pose, "SCORE: ", score)

simu = self.ask_simu_preview()
while simu == "simu": # while the user wants to run the move on the simu robot
simu_arm = self.get_reachy(simu=True).l_arm if left else self.get_reachy(simu=True).r_arm
simu_arm.publish_grasp_poses([grasp_pose], [score])
try:
if left:
self.get_reachy(simu=True).l_arm.publish_grasp_poses([grasp_pose], [score])
else:
self.get_reachy(simu=True).r_arm.publish_grasp_poses([grasp_pose], [score])
except Exception as e:
print("Error while publishing grasp poses: ", e)
grasp_success = self._execute_grasp(
grasp_pose,
left=left,
Expand Down Expand Up @@ -409,17 +406,30 @@ 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.get_reachy().l_arm.get_joints_positions()
l_gripper_opening = self.get_reachy().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.get_reachy().r_arm.get_joints_positions()
# r_gripper_opening = self.get_reachy().r_arm.gripper.opening

# 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)

r_real_joints = self.get_reachy().r_arm.get_joints_positions()
r_gripper_opening = self.get_reachy().r_arm.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)

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)
l_real_joints = self.reachy_real.l_arm.get_joints_positions()
l_gripper_opening = self.reachy_real.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

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.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(
Expand All @@ -430,8 +440,6 @@ def _execute_grasp(
use_cartesian_interpolation: bool = True,
play_in_simu: bool = False,
) -> bool:
self.is_using_left_arm = left
self._track_effector = True

simu = self.simu_preview and play_in_simu
if simu:
Expand All @@ -448,12 +456,22 @@ def _execute_grasp(
print("Grasp pose is too far away (norm > 1.0) or x < 0.0")
return False

if left:
arm = self.get_reachy(simu=simu).l_arm
# TODO we went back to using reachy_simu and reachy_real instead of get_reachy(simu=...) here
if simu:
if left:
arm = self.reachy_simu.l_arm
else:
arm = self.reachy_simu.r_arm
else:
arm = self.get_reachy(simu=simu).r_arm
if left:
arm = self.reachy_real.l_arm
else:
arm = self.reachy_real.r_arm

self.open_gripper(left=left, play_in_simu=play_in_simu)
x, y, z = pregrasp_pose[:3, 3]
self.get_reachy(simu=simu).head.look_at(x, y, z, duration=duration)

goto_id = arm.goto_from_matrix(
target=pregrasp_pose, duration=duration, with_cartesian_interpolation=use_cartesian_interpolation
)
Expand All @@ -465,6 +483,8 @@ def _execute_grasp(
# while not self.reachy.is_move_finished(goto_id):
# time.sleep(0.1)

x, y, z = grasp_pose[:3, 3]
self.get_reachy(simu=simu).head.look_at(x, y, z, duration=duration)
goto_id = arm.goto_from_matrix(
target=grasp_pose, duration=duration, with_cartesian_interpolation=use_cartesian_interpolation
)
Expand All @@ -480,6 +500,9 @@ def _execute_grasp(

lift_pose = grasp_pose.copy()
lift_pose[:3, 3] += np.array([0, 0, 0.10])

x, y, z = lift_pose[:3, 3]
self.get_reachy(simu=simu).head.look_at(x, y, z, duration=duration)
goto_id = arm.goto_from_matrix(
target=lift_pose, duration=duration, with_cartesian_interpolation=use_cartesian_interpolation
)
Expand All @@ -490,7 +513,7 @@ def _execute_grasp(
# while not self.reachy.is_move_finished(goto_id):
# time.sleep(0.1)

grasp_success = self.check_grasp_success(left=left)
grasp_success = self.check_grasp_success(left=left, simu=play_in_simu)

self.last_pregrasp_pose = pregrasp_pose
self.last_grasp_pose = grasp_pose
Expand All @@ -503,11 +526,14 @@ def _execute_grasp(

return grasp_success

def check_grasp_success(self, left: bool = False, opening_threshold: float = 0.2) -> bool:
def check_grasp_success(self, left: bool = False, opening_threshold: float = 0.2, simu: bool = False) -> bool:
if simu:
return True

if left:
arm = self.reachy.l_arm
arm = self.reachy_real.l_arm
else:
arm = self.reachy.r_arm
arm = self.reachy_real.r_arm
if arm.gripper.opening < opening_threshold:
print("Grasp failed, opening is too small")
return False
Expand Down Expand Up @@ -539,7 +565,7 @@ def place_object(
left=left,
use_cartesian_interpolation=use_cartesian_interpolation,
play_in_simu=True,
keep_orientation=True,
keep_orientation=keep_orientation,
)
simu = self.ask_simu_preview()

Expand All @@ -552,7 +578,7 @@ def place_object(
duration=duration,
left=left,
use_cartesian_interpolation=use_cartesian_interpolation,
keep_orientation=True,
keep_orientation=keep_orientation,
)
return place_success

Expand Down Expand Up @@ -605,16 +631,36 @@ def _place(
target_pose[:3, :3] = self.right_start_pose[:3, :3]

target_pose[:3, 3] += np.array([0, 0, place_height])
pre_target_pose = target_pose.copy()
pre_target_pose[:3, 3] += np.array([0, 0, 0.05])

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")

target_pose = find_close_reachable_pose(target_pose, self.pgprc.check_grasp_pose_reachability, left=left)
pre_target_pose = find_close_reachable_pose(pre_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:
if target_pose is None or pre_target_pose is None:
print("Could not find a reachable target pose.")
return False

x, y, z = pre_target_pose[:3, 3]
self.get_reachy(simu=simu).head.look_at(x, y, z, duration=duration)
goto_id = arm.goto_from_matrix(
target=pre_target_pose, duration=duration, with_cartesian_interpolation=use_cartesian_interpolation
)

if goto_id.id == -1:
print("Goto ID for pregrasp pose is -1")
return False

if goto_id.id != 0:
while not self.get_reachy(simu=simu).is_move_finished(goto_id):
print("Waiting for movement to finish...")
time.sleep(0.1)

x, y, z = target_pose[:3, 3]
self.get_reachy(simu=simu).head.look_at(x, y, z, duration=duration)
goto_id = arm.goto_from_matrix(
target=target_pose, duration=duration, with_cartesian_interpolation=use_cartesian_interpolation
)
Expand All @@ -629,6 +675,22 @@ def _place(
time.sleep(0.1)

self.open_gripper(left=left, play_in_simu=play_in_simu)

x, y, z = pre_target_pose[:3, 3]
self.get_reachy(simu=simu).head.look_at(x, y, z, duration=duration)
goto_id = arm.goto_from_matrix(
target=pre_target_pose, duration=duration, with_cartesian_interpolation=use_cartesian_interpolation
)

if goto_id.id == -1:
print("Goto ID for pregrasp pose is -1")
return False

if goto_id.id != 0:
while not self.get_reachy(simu=simu).is_move_finished(goto_id):
print("Waiting for movement to finish...")
time.sleep(0.1)

if left:
self.get_robot_state(simu=simu).LeftArmState = ArmState.PLACING
else:
Expand Down Expand Up @@ -657,6 +719,8 @@ def goto_rest_position(
assert not np.array_equal(start_pose, np.eye(4))
if not replay:

x, y, z = start_pose[:3, 3]
self.get_reachy(simu=simu).head.look_at(x, y, z, duration=goto_duration * 2.0)
arm.goto_from_matrix(
start_pose, duration=goto_duration * 2.0, with_cartesian_interpolation=use_cartesian_interpolation
)
Expand All @@ -677,10 +741,14 @@ def goto_rest_position(
# target=self.last_grasp_pose, duration=goto_duration, with_cartesian_interpolation=use_cartesian_interpolation
# )

x, y, z = self.last_pregrasp_pose[:3, 3]
self.get_reachy(simu=simu).head.look_at(x, y, z, duration=goto_duration * 2.0)
arm.goto_from_matrix(
target=self.last_pregrasp_pose, duration=goto_duration, with_cartesian_interpolation=use_cartesian_interpolation
)

x, y, z = start_pose[:3, 3]
self.get_reachy(simu=simu).head.look_at(x, y, z, duration=goto_duration * 2.0)
arm.goto_from_matrix(start_pose, duration=goto_duration, with_cartesian_interpolation=use_cartesian_interpolation)

if open_gripper:
Expand All @@ -692,13 +760,9 @@ def goto_rest_position(
else:
self.get_robot_state(simu=simu).RightArmState = ArmState.REST

self._reset_head = True
self._track_effector = False

return True

def get_reachy(self, simu: bool = False) -> ReachySDK:
print("LAST GET REACHY", "SIMU" if simu else "REAL")
if simu:
return self.reachy_simu

Expand Down Expand Up @@ -735,26 +799,25 @@ def turn_robot_on(self) -> None:
time.sleep(0.1)

def stop(self) -> None:
self._stop_thread()
self.get_reachy().turn_off_smoothly()

def _effector_head_tracking(self) -> None:
y = 0.1
while True:
if self._track_effector:
if self.is_using_left_arm:
arm = self.reachy.l_arm
arm = self.get_reachy().l_arm
else:
arm = self.reachy.r_arm
arm = self.get_reachy().r_arm

x, y, z = arm.forward_kinematics(arm.get_joints_positions())[:3, 3]
x = min(0.35, x)

self.reachy.head.look_at(x, y, z, duration=0.2)
self.get_reachy().head.look_at(x, y, z, duration=0.05)

time.sleep(0.2)
time.sleep(0.05)

if self._reset_head:
self.reachy.head.look_at(0.35, 0.0, 0.0, duration=1.0)
self.get_reachy().head.look_at(0.35, 0.0, 0.0, duration=1.0)
self._reset_head = False
time.sleep(1.0)
5 changes: 0 additions & 5 deletions src/pollen_manipulation/utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
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


Expand Down Expand Up @@ -49,9 +48,6 @@ def find_close_reachable_pose(

pose[:3, :3] = rot

fv = Viewer()
fv.start()

rot_tol = 20 # deg

for i in range(100):
Expand All @@ -62,7 +58,6 @@ def find_close_reachable_pose(
reachable = reachability_function(pose_candidate, left)
if reachable:
return pose
fv.pushFrame(pose_candidate, "truc")
time.sleep(0.1)

# for theta_x in range(0, 40):
Expand Down

0 comments on commit b93455e

Please sign in to comment.