From 94807841b6798c6474105db44c277ad607d09258 Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Mon, 14 Apr 2025 20:08:06 +0200 Subject: [PATCH 01/24] - work in progress hand integration --- python/examples/env_cartesian_control.py | 8 +- python/rcsss/envs/base.py | 60 ++++++++ python/rcsss/envs/factories.py | 10 +- python/rcsss/hand/hand.py | 185 +++++++++++++++++++++++ 4 files changed, 257 insertions(+), 6 deletions(-) create mode 100644 python/rcsss/hand/hand.py diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index ff0ef334..f2fde943 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -80,19 +80,21 @@ def main(): for _ in range(10): for _ in range(10): # move 1cm in x direction (forward) and close gripper - act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} + act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "hand": 0} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") return + from time import sleep + sleep(5) for _ in range(10): # move 1cm in negative x direction (backward) and open gripper - act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} + act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "hand": 1} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") return - + if __name__ == "__main__": main() diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index 1724998b..d4e213a8 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -96,6 +96,9 @@ class GripperDictType(RCSpaceType): # 0 for closed, 1 for open (>=0.5 for open) gripper: Annotated[float, gym.spaces.Box(low=0, high=1, dtype=np.float32)] +class HandDictType(RCSpaceType): + # 0 for closed, 1 for open (>=0.5 for open) + hand: Annotated[float, gym.spaces.Box(low=0, high=1, dtype=np.float32)] class CameraDictType(RCSpaceType): frames: dict[ @@ -577,3 +580,60 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: self._last_gripper_cmd = gripper_action del action[self.gripper_key] return action + +class HandWrapper(ActObsInfoWrapper): + # TODO: sticky gripper, like in aloha + + BINARY_HAND_CLOSED = 0 + BINARY_HAND_OPEN = 1 + + def __init__(self, env, hand, binary: bool = True): + super().__init__(env) + self.unwrapped: FR3Env + self.observation_space: gym.spaces.Dict + self.observation_space.spaces.update(get_space(HandDictType).spaces) + self.action_space: gym.spaces.Dict + self.action_space.spaces.update(get_space(HandDictType).spaces) + self.hand_key = get_space_keys(HandDictType)[0] + self._hand = hand + self.binary = binary + self._last_hand_cmd = None + + def reset(self, **kwargs) -> tuple[dict[str, Any], dict[str, Any]]: + self._hand.reset() + self._last_hand_cmd = None + return super().reset(**kwargs) + + def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: + observation = copy.deepcopy(observation) + if self.binary: + observation[self.hand_key] = ( + self._last_hand_cmd if self._last_hand_cmd is not None else self.BINARY_HAND_OPEN + ) + else: + observation[self.hand_key] = self._hand.get_normalized_width() + + info = False + return observation, info + + def action(self, action: dict[str, Any]) -> dict[str, Any]: + + action = copy.deepcopy(action) + assert self.hand_key in action, "hand action not found." + + hand_action = np.round(action[self.hand_key]) if self.binary else action[self.hand_key] + hand_action = np.clip(hand_action, 0.0, 1.0) + + if self._last_hand_cmd is None or self._last_hand_cmd != hand_action: + if self.binary: + if hand_action == self.BINARY_HAND_CLOSED: + self._hand.grasp() + print("Hand closed") + else: + self._hand.open() + print("Hand opened") + else: + self._hand.set_normalized_width(hand_action) + self._last_hand_cmd = hand_action + del action[self.hand_key] + return action \ No newline at end of file diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index b5c85388..cd10b9c1 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -224,9 +224,13 @@ def fr3_sim_env( env = CameraSetWrapper(env, camera_set, include_depth=True) if gripper_cfg is not None: - gripper = sim.FrankaHand(simulation, "0", gripper_cfg) - env = GripperWrapper(env, gripper, binary=True) - + #gripper = sim.FrankaHand(simulation, "0", gripper_cfg) + #env = GripperWrapper(env, gripper, binary=True) + from rcsss.hand.hand import Hand + from rcsss.hand.hand import TilburgHandControl + from rcsss.envs.base import HandWrapper + hand = Hand(TilburgHandControl()) + env = HandWrapper(env, hand, binary=True) if collision_guard: env = CollisionGuard.env_from_xml_paths( env, diff --git a/python/rcsss/hand/hand.py b/python/rcsss/hand/hand.py new file mode 100644 index 00000000..49ace76a --- /dev/null +++ b/python/rcsss/hand/hand.py @@ -0,0 +1,185 @@ +import sys +import os +import logging +from time import sleep +import copy +from tilburg_hand import TilburgHandMotorInterface, Finger, Wrist, Unit + +# Setup logger +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger(__name__) + +class TilburgHandControl: + def __init__(self, config_folder_path="config", verbose=False): + """ + Initializes the Tilburg Hand Control interface. + """ + self.config_folder_path = config_folder_path + self.tilburg_hand_config_file_path = os.path.join(config_folder_path, "config.json") + self.tilburg_hand_calibration_file_path = os.path.join(config_folder_path, "calibration.json") + + self._motors = TilburgHandMotorInterface( + config_file=self.tilburg_hand_config_file_path, + calibration_file=self.tilburg_hand_calibration_file_path, + verbose=verbose + ) + + self.pos_value_unit = Unit.NORMALIZED + self._pos_normalized = [0] * self._motors.n_motors + if self._motors.connect() < 0: + logger.error("Failed to connect to the motors' board.") + sys.exit(1) + + logger.info("Connected to the motors' board.") + + self.fingers_joints = Finger + self.fingers_joints_list = [ + Finger.INDEX_ABD, Finger.MIDDLE_ABD, Finger.THUMB_ABD, Finger.RING_ABD, + Finger.INDEX_DIP, Finger.MIDDLE_DIP, Finger.RING_DIP, Finger.THUMB_IP, + Finger.INDEX_PIP, Finger.MIDDLE_PIP, Finger.RING_PIP, + Finger.INDEX_MCP, Finger.MIDDLE_MCP, Finger.RING_MCP, Finger.THUMB_MCP, + Finger.THUMB_CMC + ] + self.pos_vector_names_list = [ + "THUMB_IP", "THUMB_MCP", "THUMB_ABD", "THUMB_CMC", + "INDEX_DIP", "INDEX_PIP", "INDEX_MCP", "INDEX_ABD", + "MIDDLE_DIP", "MIDDLE_PIP", "MIDDLE_MCP", "MIDDLE_ABD", + "RING_DIP", "RING_PIP", "RING_MCP", "RING_ABD", + "WRIST_PITCH", "WRIST_YAW" + ] + + self.wrist_joints_list = [Wrist.PITCH, Wrist.YAW] + self.set_zero_pos() + logger.info("Setting all joints to zero position.") + + def grasp(self, value: float, template: list = None): + """ + Performs a grasp with a specified intensity (0.0 to 1.0). + """ + if template is None: + template = [ + 0.0, 0.0, 1.0, 0.0, + 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, + 1.0 + ] + + if len(template) != len(self.fingers_joints_list): + logger.warning("Grasp template length mismatch. Aborting grasp.") + return + + fingers_joints_pos_values_list = [ + val * value for val in template + ] + + for joint, pos in zip(self.fingers_joints_list, fingers_joints_pos_values_list): + self._pos_normalized[joint] = pos + + self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) + logger.info(f"Grasp command sent with value: {value}") + + def set_zero_pos(self): + """ + Sets all finger joint positions to zero. + """ + for joint in self.fingers_joints_list: + self._pos_normalized[joint] = 0 + self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) + logger.info("All joints reset to zero position.") + + def sleep(self, seconds: float): + """ + Delays the program for a specified number of seconds. + """ + sleep(seconds) + + def set_joint_pos(self, finger_joint, pos_value: float): + """ + Sets a single joint to a specific normalized position. + """ + self._motors.set_pos_single(finger_joint, pos_value, unit=self.pos_value_unit) + self._pos_normalized[finger_joint] = pos_value + logger.info(f"Set joint {finger_joint.name} to {pos_value:.2f}") + + def reset_joint_pos(self, finger_joint): + """ + Resets a specific joint to zero. + """ + self._motors.set_pos_single(finger_joint, 0, unit=self.pos_value_unit) + self._pos_normalized[finger_joint] = 0 + logger.info(f"Reset joint {finger_joint.name} to 0") + + def disconnect(self): + """ + Gracefully disconnects from the motor interface. + """ + self._motors.disconnect() + logger.info("Disconnected from the motors' board") + + def get_pos_vector(self): + """ + Returns the current position vector of the motors. + """ + return self._pos_normalized + + def get_pos_single(self, finger_joint): + """ + Returns the current position of a single joint. + """ + return self._pos_normalized[finger_joint] + +class Hand: + """ + Dummy class to represent the hand. In a real implementation, this would + interface with the actual hand hardware. + """ + def __init__(self, hand: TilburgHandControl): + self._hand = hand + + def grasp(self): + self._hand.grasp(value = 0.9) + + def reset(self): + self._hand.set_zero_pos() + + def get_state(self): + pass + + def disconnect(self): + self._hand.disconnect() + + def get_normalized_width(self): + pass + + def open(self): + self.reset() + + def __del__(self): + self._hand.disconnect() + logger.info("Hand object deleted and disconnected from motors' board.") + +if __name__ == "__main__": + # try: + # hand = TilburgHandControl(verbose=True) + # hand.grasp(0.9) + # logger.info(f"current joints poses: {hand.get_pos_vector()}") + # logger.info(hand.pos_vector_names_list) + # hand.sleep(5) + # hand.set_zero_pos() + # hand.sleep(1) + # hand.set_joint_pos(hand.fingers_joints.THUMB_CMC, 0.5) + # hand.sleep(2) + # logger.info(f"current joint {hand.fingers_joints.THUMB_CMC.name}: {hand.get_pos_single(hand.fingers_joints.THUMB_CMC)}") + # hand.reset_joint_pos(hand.fingers_joints.THUMB_CMC) + # except Exception as e: + # logger.exception(f"An error occurred: {e}") + # finally: + # hand.disconnect() + hand = Hand(TilburgHandControl(verbose=True)) + hand.reset() + hand.grasp() + sleep(5) + hand.reset() + sleep(2) + hand.disconnect() \ No newline at end of file From bc9c6e1921a4f300d56769e7659a180ba1a8bfb8 Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Tue, 15 Apr 2025 17:24:17 +0200 Subject: [PATCH 02/24] - refactor the hand module --- python/rcsss/envs/factories.py | 2 +- python/rcsss/hand/__init__.py | 0 python/rcsss/hand/config/calibration.json | 1 + python/rcsss/hand/config/config.json | 8 ++ python/rcsss/hand/hand.py | 144 +++------------------- python/rcsss/hand/tilburg_hand_control.py | 136 ++++++++++++++++++++ 6 files changed, 162 insertions(+), 129 deletions(-) create mode 100644 python/rcsss/hand/__init__.py create mode 100644 python/rcsss/hand/config/calibration.json create mode 100644 python/rcsss/hand/config/config.json create mode 100644 python/rcsss/hand/tilburg_hand_control.py diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index cd10b9c1..3428de37 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -227,7 +227,7 @@ def fr3_sim_env( #gripper = sim.FrankaHand(simulation, "0", gripper_cfg) #env = GripperWrapper(env, gripper, binary=True) from rcsss.hand.hand import Hand - from rcsss.hand.hand import TilburgHandControl + from rcsss.hand.tilburg_hand_control import TilburgHandControl from rcsss.envs.base import HandWrapper hand = Hand(TilburgHandControl()) env = HandWrapper(env, hand, binary=True) diff --git a/python/rcsss/hand/__init__.py b/python/rcsss/hand/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/python/rcsss/hand/config/calibration.json b/python/rcsss/hand/config/calibration.json new file mode 100644 index 00000000..a5a7ed7c --- /dev/null +++ b/python/rcsss/hand/config/calibration.json @@ -0,0 +1 @@ +{"motor_calib_min_range_deg": [-5, 0, 0, 0, -5, -5, 0, -25, -5, -5, 0, -25, -5, -5, 0, -25, 0, 0], "motor_calib_max_range_deg": [95, 90, 100, 90, 95, 95, 95, 25, 95, 95, 95, 25, 95, 95, 95, 25, 0, 0], "motor_calib_min_range_ticks": [1991, 2048, 2048, 2048, 1991, 1991, 2048, 2332, 1991, 1991, 2048, 2332, 1991, 1991, 2048, 2332, 0, 0], "motor_calib_max_range_ticks": [3128, 1024, 910, 3072, 3128, 3128, 967, 1763, 3128, 3128, 967, 1763, 3128, 3128, 967, 1763, 0, 0], "motor_calib_zero_pos_ticks": [3088, 1351, 2044, 2918, 2048, 2060, 1860, 2089, 2051, 2053, 1876, 2082, 2047, 2047, 1929, 2031, 512, 512], "hand_orientation": "right"} \ No newline at end of file diff --git a/python/rcsss/hand/config/config.json b/python/rcsss/hand/config/config.json new file mode 100644 index 00000000..53bc3d0b --- /dev/null +++ b/python/rcsss/hand/config/config.json @@ -0,0 +1,8 @@ +{ + "motors_dynamixelsdk_port" : "", + "motors_dynamixelsdk_VID_PID" : "0403:6014", + + "joint_motor_mapping" : [23, 22, 21, 20, 27, 26, 25, 24, 31, 30, 29, 28, 35, 34, 33, 32, 255, 255], + + "_comments": "If usb port names are left blank, then all ports are scanned for the pre-set VID:PID. If a port is set, it will override the automatic search. Motors will show as disabled if the app can't connect to the control board. All motors are pinged at the beginning, and non-responding / non-connected ones are set as disabled. joint_motor_mapping is used as, e.g., motor_id = joint_motor_mapping[Finger.INDEX_ABD]." +} diff --git a/python/rcsss/hand/hand.py b/python/rcsss/hand/hand.py index 49ace76a..5de07dea 100644 --- a/python/rcsss/hand/hand.py +++ b/python/rcsss/hand/hand.py @@ -1,140 +1,29 @@ -import sys -import os -import logging -from time import sleep -import copy -from tilburg_hand import TilburgHandMotorInterface, Finger, Wrist, Unit -# Setup logger -logging.basicConfig(level=logging.INFO) -logger = logging.getLogger(__name__) - -class TilburgHandControl: - def __init__(self, config_folder_path="config", verbose=False): - """ - Initializes the Tilburg Hand Control interface. - """ - self.config_folder_path = config_folder_path - self.tilburg_hand_config_file_path = os.path.join(config_folder_path, "config.json") - self.tilburg_hand_calibration_file_path = os.path.join(config_folder_path, "calibration.json") - - self._motors = TilburgHandMotorInterface( - config_file=self.tilburg_hand_config_file_path, - calibration_file=self.tilburg_hand_calibration_file_path, - verbose=verbose - ) - - self.pos_value_unit = Unit.NORMALIZED - self._pos_normalized = [0] * self._motors.n_motors - if self._motors.connect() < 0: - logger.error("Failed to connect to the motors' board.") - sys.exit(1) - - logger.info("Connected to the motors' board.") - - self.fingers_joints = Finger - self.fingers_joints_list = [ - Finger.INDEX_ABD, Finger.MIDDLE_ABD, Finger.THUMB_ABD, Finger.RING_ABD, - Finger.INDEX_DIP, Finger.MIDDLE_DIP, Finger.RING_DIP, Finger.THUMB_IP, - Finger.INDEX_PIP, Finger.MIDDLE_PIP, Finger.RING_PIP, - Finger.INDEX_MCP, Finger.MIDDLE_MCP, Finger.RING_MCP, Finger.THUMB_MCP, - Finger.THUMB_CMC - ] - self.pos_vector_names_list = [ - "THUMB_IP", "THUMB_MCP", "THUMB_ABD", "THUMB_CMC", - "INDEX_DIP", "INDEX_PIP", "INDEX_MCP", "INDEX_ABD", - "MIDDLE_DIP", "MIDDLE_PIP", "MIDDLE_MCP", "MIDDLE_ABD", - "RING_DIP", "RING_PIP", "RING_MCP", "RING_ABD", - "WRIST_PITCH", "WRIST_YAW" - ] - - self.wrist_joints_list = [Wrist.PITCH, Wrist.YAW] - self.set_zero_pos() - logger.info("Setting all joints to zero position.") - - def grasp(self, value: float, template: list = None): - """ - Performs a grasp with a specified intensity (0.0 to 1.0). - """ - if template is None: - template = [ - 0.0, 0.0, 1.0, 0.0, - 1.0, 1.0, 1.0, 1.0, - 1.0, 1.0, 1.0, - 1.0, 1.0, 1.0, 1.0, - 1.0 - ] - - if len(template) != len(self.fingers_joints_list): - logger.warning("Grasp template length mismatch. Aborting grasp.") - return - - fingers_joints_pos_values_list = [ - val * value for val in template - ] - - for joint, pos in zip(self.fingers_joints_list, fingers_joints_pos_values_list): - self._pos_normalized[joint] = pos +class HandControl: + """ + Base class for hand control. + This class provides an interface for hand control, but does not implement any functionality. + """ + def __init__(self): + pass - self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) - logger.info(f"Grasp command sent with value: {value}") + def grasp(self, value: float): + raise NotImplementedError("This method should be overridden by subclasses.") def set_zero_pos(self): - """ - Sets all finger joint positions to zero. - """ - for joint in self.fingers_joints_list: - self._pos_normalized[joint] = 0 - self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) - logger.info("All joints reset to zero position.") - - def sleep(self, seconds: float): - """ - Delays the program for a specified number of seconds. - """ - sleep(seconds) - - def set_joint_pos(self, finger_joint, pos_value: float): - """ - Sets a single joint to a specific normalized position. - """ - self._motors.set_pos_single(finger_joint, pos_value, unit=self.pos_value_unit) - self._pos_normalized[finger_joint] = pos_value - logger.info(f"Set joint {finger_joint.name} to {pos_value:.2f}") - - def reset_joint_pos(self, finger_joint): - """ - Resets a specific joint to zero. - """ - self._motors.set_pos_single(finger_joint, 0, unit=self.pos_value_unit) - self._pos_normalized[finger_joint] = 0 - logger.info(f"Reset joint {finger_joint.name} to 0") + raise NotImplementedError("This method should be overridden by subclasses.") def disconnect(self): - """ - Gracefully disconnects from the motor interface. - """ - self._motors.disconnect() - logger.info("Disconnected from the motors' board") + raise NotImplementedError("This method should be overridden by subclasses.") + - def get_pos_vector(self): - """ - Returns the current position vector of the motors. - """ - return self._pos_normalized - - def get_pos_single(self, finger_joint): - """ - Returns the current position of a single joint. - """ - return self._pos_normalized[finger_joint] - class Hand: """ - Dummy class to represent the hand. In a real implementation, this would - interface with the actual hand hardware. + Hand Class + This class provides an interface for hand control. + It allows for grasping, resetting, and disconnecting from the hand. """ - def __init__(self, hand: TilburgHandControl): + def __init__(self, hand: HandControl): self._hand = hand def grasp(self): @@ -157,7 +46,6 @@ def open(self): def __del__(self): self._hand.disconnect() - logger.info("Hand object deleted and disconnected from motors' board.") if __name__ == "__main__": # try: diff --git a/python/rcsss/hand/tilburg_hand_control.py b/python/rcsss/hand/tilburg_hand_control.py new file mode 100644 index 00000000..7ed8449e --- /dev/null +++ b/python/rcsss/hand/tilburg_hand_control.py @@ -0,0 +1,136 @@ +import sys +import os +import copy +from time import sleep +from rcsss.hand.hand import HandControl +import logging +from tilburg_hand import TilburgHandMotorInterface, Finger, Wrist, Unit + +# Setup logger +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger(__name__) + +class TilburgHandControl(HandControl): + """ + Tilburg Hand Control Class + This class provides an interface for controlling the Tilburg Hand. + It allows for grasping, resetting, and disconnecting from the hand. + """ + def __init__(self, config_folder_path="config", verbose=False): + """ + Initializes the Tilburg Hand Control interface. + """ + self.config_folder_path = config_folder_path + self.tilburg_hand_config_file_path = os.path.join(config_folder_path, "config.json") + self.tilburg_hand_calibration_file_path = os.path.join(config_folder_path, "calibration.json") + + self._motors = TilburgHandMotorInterface( + config_file=self.tilburg_hand_config_file_path, + calibration_file=self.tilburg_hand_calibration_file_path, + verbose=verbose + ) + + self.pos_value_unit = Unit.NORMALIZED + self._pos_normalized = [0] * self._motors.n_motors + if self._motors.connect() < 0: + logger.error("Failed to connect to the motors' board.") + sys.exit(1) + + logger.info("Connected to the motors' board.") + + self.fingers_joints = Finger + self.fingers_joints_list = [ + Finger.INDEX_ABD, Finger.MIDDLE_ABD, Finger.THUMB_ABD, Finger.RING_ABD, + Finger.INDEX_DIP, Finger.MIDDLE_DIP, Finger.RING_DIP, Finger.THUMB_IP, + Finger.INDEX_PIP, Finger.MIDDLE_PIP, Finger.RING_PIP, + Finger.INDEX_MCP, Finger.MIDDLE_MCP, Finger.RING_MCP, Finger.THUMB_MCP, + Finger.THUMB_CMC + ] + self.pos_vector_names_list = [ + "THUMB_IP", "THUMB_MCP", "THUMB_ABD", "THUMB_CMC", + "INDEX_DIP", "INDEX_PIP", "INDEX_MCP", "INDEX_ABD", + "MIDDLE_DIP", "MIDDLE_PIP", "MIDDLE_MCP", "MIDDLE_ABD", + "RING_DIP", "RING_PIP", "RING_MCP", "RING_ABD", + "WRIST_PITCH", "WRIST_YAW" + ] + + self.wrist_joints_list = [Wrist.PITCH, Wrist.YAW] + self.set_zero_pos() + logger.info("Setting all joints to zero position.") + + def grasp(self, value: float, template: list = None): + """ + Performs a grasp with a specified intensity (0.0 to 1.0). + """ + if template is None: + template = [ + 0.0, 0.0, 1.0, 0.0, + 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, + 1.0 + ] + + if len(template) != len(self.fingers_joints_list): + logger.warning("Grasp template length mismatch. Aborting grasp.") + return + + fingers_joints_pos_values_list = [ + val * value for val in template + ] + + for joint, pos in zip(self.fingers_joints_list, fingers_joints_pos_values_list): + self._pos_normalized[joint] = pos + + self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) + logger.info(f"Grasp command sent with value: {value}") + + def set_zero_pos(self): + """ + Sets all finger joint positions to zero. + """ + for joint in self.fingers_joints_list: + self._pos_normalized[joint] = 0 + self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) + logger.info("All joints reset to zero position.") + + def sleep(self, seconds: float): + """ + Delays the program for a specified number of seconds. + """ + sleep(seconds) + + def set_joint_pos(self, finger_joint, pos_value: float): + """ + Sets a single joint to a specific normalized position. + """ + self._motors.set_pos_single(finger_joint, pos_value, unit=self.pos_value_unit) + self._pos_normalized[finger_joint] = pos_value + logger.info(f"Set joint {finger_joint.name} to {pos_value:.2f}") + + def reset_joint_pos(self, finger_joint): + """ + Resets a specific joint to zero. + """ + self._motors.set_pos_single(finger_joint, 0, unit=self.pos_value_unit) + self._pos_normalized[finger_joint] = 0 + logger.info(f"Reset joint {finger_joint.name} to 0") + + def disconnect(self): + """ + Gracefully disconnects from the motor interface. + """ + self._motors.disconnect() + logger.info("Disconnected from the motors' board") + + def get_pos_vector(self): + """ + Returns the current position vector of the motors. + """ + return self._pos_normalized + + def get_pos_single(self, finger_joint): + """ + Returns the current position of a single joint. + """ + return self._pos_normalized[finger_joint] \ No newline at end of file From cb70eedb02753d87849ea173869f2dfecf68919a Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Tue, 15 Apr 2025 18:19:04 +0200 Subject: [PATCH 03/24] refactor(hand integration) --- python/examples/env_cartesian_control.py | 5 +++-- python/rcsss/envs/factories.py | 19 ++++++++++++------- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index f2fde943..e36aff33 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -50,7 +50,7 @@ def main(): resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) else: resource_manger = DummyResourceManager() - + use_hand = True with resource_manger: if ROBOT_INSTANCE == RobotInstance.HARDWARE: env_rel = fr3_hw_env( @@ -67,10 +67,11 @@ def main(): control_mode=ControlMode.CARTESIAN_TQuart, robot_cfg=default_fr3_sim_robot_cfg(), collision_guard=False, - gripper_cfg=default_fr3_sim_gripper_cfg(), + gripper_cfg=None, camera_set_cfg=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, + use_hand=use_hand, ) env_rel.get_wrapper_attr("sim").open_gui() diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 3428de37..c6fb6106 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -13,6 +13,8 @@ from rcsss.camera.interface import BaseCameraConfig from rcsss.camera.realsense import RealSenseCameraSet, RealSenseSetConfig from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig +from rcsss.hand.hand import Hand +from rcsss.hand.tilburg_hand_control import TilburgHandControl from rcsss.envs.base import ( CameraSetWrapper, ControlMode, @@ -20,6 +22,7 @@ GripperWrapper, RelativeActionSpace, RelativeTo, + HandWrapper ) from rcsss.envs.hw import FR3HW from rcsss.envs.sim import ( @@ -184,6 +187,7 @@ def fr3_sim_env( urdf_path: str | PathLike | None = None, mjcf: str | PathLike = "fr3_empty_world", sim_wrapper: Type[SimWrapper] | None = None, + use_hand: bool = False, ) -> gym.Env: """ Creates a simulation environment for the FR3 robot. @@ -224,13 +228,14 @@ def fr3_sim_env( env = CameraSetWrapper(env, camera_set, include_depth=True) if gripper_cfg is not None: - #gripper = sim.FrankaHand(simulation, "0", gripper_cfg) - #env = GripperWrapper(env, gripper, binary=True) - from rcsss.hand.hand import Hand - from rcsss.hand.tilburg_hand_control import TilburgHandControl - from rcsss.envs.base import HandWrapper - hand = Hand(TilburgHandControl()) - env = HandWrapper(env, hand, binary=True) + gripper = sim.FrankaHand(simulation, "0", gripper_cfg) + env = GripperWrapper(env, gripper, binary=True) + + else: + if use_hand: + hand = Hand(TilburgHandControl()) + env = HandWrapper(env, hand, binary=True) + if collision_guard: env = CollisionGuard.env_from_xml_paths( env, From 14f7f3a2427a202bfde8ca523e257ba936d1a11a Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Tue, 15 Apr 2025 19:08:08 +0200 Subject: [PATCH 04/24] refactor(hand module) --- python/rcsss/hand/hand.py | 7 ++-- python/rcsss/hand/tilburg_hand_control.py | 44 +++++++++++++---------- 2 files changed, 30 insertions(+), 21 deletions(-) diff --git a/python/rcsss/hand/hand.py b/python/rcsss/hand/hand.py index 5de07dea..64725395 100644 --- a/python/rcsss/hand/hand.py +++ b/python/rcsss/hand/hand.py @@ -38,8 +38,11 @@ def get_state(self): def disconnect(self): self._hand.disconnect() - def get_normalized_width(self): - pass + def get_normalized_joints_poses(self): + self._hand.get_pos_vector() + + def set_normalized_joints_poses(self, values: list): + self._hand.set_pos_vector(values) def open(self): self.reset() diff --git a/python/rcsss/hand/tilburg_hand_control.py b/python/rcsss/hand/tilburg_hand_control.py index 7ed8449e..aa089d4b 100644 --- a/python/rcsss/hand/tilburg_hand_control.py +++ b/python/rcsss/hand/tilburg_hand_control.py @@ -52,7 +52,7 @@ def __init__(self, config_folder_path="config", verbose=False): "MIDDLE_DIP", "MIDDLE_PIP", "MIDDLE_MCP", "MIDDLE_ABD", "RING_DIP", "RING_PIP", "RING_MCP", "RING_ABD", "WRIST_PITCH", "WRIST_YAW" - ] + ] self.wrist_joints_list = [Wrist.PITCH, Wrist.YAW] self.set_zero_pos() @@ -63,34 +63,40 @@ def grasp(self, value: float, template: list = None): Performs a grasp with a specified intensity (0.0 to 1.0). """ if template is None: - template = [ - 0.0, 0.0, 1.0, 0.0, - 1.0, 1.0, 1.0, 1.0, - 1.0, 1.0, 1.0, - 1.0, 1.0, 1.0, 1.0, - 1.0 - ] - - if len(template) != len(self.fingers_joints_list): - logger.warning("Grasp template length mismatch. Aborting grasp.") - return + template = [ + 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 0.0, + 1.0, 1.0, 1.0, 0.0, + 1.0, 1.0, 1.0, 0.0, + 0.0, 0.0 + ] + - fingers_joints_pos_values_list = [ - val * value for val in template - ] - for joint, pos in zip(self.fingers_joints_list, fingers_joints_pos_values_list): - self._pos_normalized[joint] = pos + self._pos_normalized = [ + val * value for val in template + ] self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) logger.info(f"Grasp command sent with value: {value}") + def set_pose_vector(self, pos_vector: list): + """ + Sets the position vector for the motors. + """ + if len(pos_vector) != len(self._pos_normalized): + logger.error(f"Invalid position vector length: {len(pos_vector)}. Expected: {len(self._pos_normalized)}") + return + self._pos_normalized = pos_vector + + self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) + logger.info(f"Set pose vector: {pos_vector}") + def set_zero_pos(self): """ Sets all finger joint positions to zero. """ - for joint in self.fingers_joints_list: - self._pos_normalized[joint] = 0 + self._pos_normalized = [0] * self._motors.n_motors self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) logger.info("All joints reset to zero position.") From e1db29541f038abb162402be0817e73b53113b1c Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Tue, 15 Apr 2025 19:43:39 +0200 Subject: [PATCH 05/24] feat(hand): enable Binary and vector action --- python/examples/env_cartesian_control.py | 23 +++++++++++++++++++---- python/rcsss/envs/base.py | 10 +++++----- python/rcsss/envs/factories.py | 6 +++--- python/rcsss/hand/tilburg_hand_control.py | 2 +- 4 files changed, 28 insertions(+), 13 deletions(-) diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index e36aff33..207d2bf2 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -50,8 +50,8 @@ def main(): resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) else: resource_manger = DummyResourceManager() - use_hand = True with resource_manger: + binary_action = False if ROBOT_INSTANCE == RobotInstance.HARDWARE: env_rel = fr3_hw_env( ip=ROBOT_IP, @@ -71,17 +71,32 @@ def main(): camera_set_cfg=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, - use_hand=use_hand, + hand_cfg={"Binary": binary_action}, ) env_rel.get_wrapper_attr("sim").open_gui() env_rel.reset() print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore + if binary_action: + close_action = 0 + open_action = 1 + else: + template = [ + 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 0.0, + 1.0, 1.0, 1.0, 0.0, + 1.0, 1.0, 1.0, 0.0, + 0.0, 0.0 + ] + value = 0.5 + close_action = [value * val for val in template ] + open_action = [0.0] * len(template) + for _ in range(10): for _ in range(10): # move 1cm in x direction (forward) and close gripper - act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "hand": 0} + act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "hand": close_action} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") @@ -90,7 +105,7 @@ def main(): sleep(5) for _ in range(10): # move 1cm in negative x direction (backward) and open gripper - act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "hand": 1} + act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "hand": open_action} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index d4e213a8..a5557a79 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -611,7 +611,7 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl self._last_hand_cmd if self._last_hand_cmd is not None else self.BINARY_HAND_OPEN ) else: - observation[self.hand_key] = self._hand.get_normalized_width() + observation[self.hand_key] = self._hand.get_normalized_joints_poses() info = False return observation, info @@ -624,16 +624,16 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: hand_action = np.round(action[self.hand_key]) if self.binary else action[self.hand_key] hand_action = np.clip(hand_action, 0.0, 1.0) - if self._last_hand_cmd is None or self._last_hand_cmd != hand_action: - if self.binary: + if self.binary: + if self._last_hand_cmd is None or self._last_hand_cmd != hand_action: if hand_action == self.BINARY_HAND_CLOSED: self._hand.grasp() print("Hand closed") else: self._hand.open() print("Hand opened") - else: - self._hand.set_normalized_width(hand_action) + else: + self._hand.set_normalized_joints_poses(hand_action) self._last_hand_cmd = hand_action del action[self.hand_key] return action \ No newline at end of file diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index c6fb6106..f07a1860 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -187,7 +187,7 @@ def fr3_sim_env( urdf_path: str | PathLike | None = None, mjcf: str | PathLike = "fr3_empty_world", sim_wrapper: Type[SimWrapper] | None = None, - use_hand: bool = False, + hand_cfg: dict = None, ) -> gym.Env: """ Creates a simulation environment for the FR3 robot. @@ -232,9 +232,9 @@ def fr3_sim_env( env = GripperWrapper(env, gripper, binary=True) else: - if use_hand: + if hand_cfg is not None: hand = Hand(TilburgHandControl()) - env = HandWrapper(env, hand, binary=True) + env = HandWrapper(env, hand, binary=hand_cfg["Binary"]) if collision_guard: env = CollisionGuard.env_from_xml_paths( diff --git a/python/rcsss/hand/tilburg_hand_control.py b/python/rcsss/hand/tilburg_hand_control.py index aa089d4b..1828a41e 100644 --- a/python/rcsss/hand/tilburg_hand_control.py +++ b/python/rcsss/hand/tilburg_hand_control.py @@ -80,7 +80,7 @@ def grasp(self, value: float, template: list = None): self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) logger.info(f"Grasp command sent with value: {value}") - def set_pose_vector(self, pos_vector: list): + def set_pos_vector(self, pos_vector: list): """ Sets the position vector for the motors. """ From d5403a8968082482835082b20782800e5eb3a081 Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Tue, 15 Apr 2025 20:36:31 +0200 Subject: [PATCH 06/24] refafix(hand) : refactor and fix action and observation space for both binary and vector --- python/examples/env_cartesian_control.py | 2 +- python/rcsss/envs/base.py | 40 ++++++++++++++++++----- python/rcsss/envs/space_utils.py | 1 + python/rcsss/hand/hand.py | 27 +-------------- python/rcsss/hand/tilburg_hand_control.py | 3 +- 5 files changed, 36 insertions(+), 37 deletions(-) diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index 207d2bf2..a62a8daa 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -51,7 +51,7 @@ def main(): else: resource_manger = DummyResourceManager() with resource_manger: - binary_action = False + binary_action = True if ROBOT_INSTANCE == RobotInstance.HARDWARE: env_rel = fr3_hw_env( ip=ROBOT_IP, diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index a5557a79..ae609e3b 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -14,6 +14,7 @@ RCSpaceType, Vec6Type, Vec7Type, + Vec18Type, get_space, get_space_keys, ) @@ -96,10 +97,20 @@ class GripperDictType(RCSpaceType): # 0 for closed, 1 for open (>=0.5 for open) gripper: Annotated[float, gym.spaces.Box(low=0, high=1, dtype=np.float32)] -class HandDictType(RCSpaceType): +class HandBinDictType(RCSpaceType): # 0 for closed, 1 for open (>=0.5 for open) hand: Annotated[float, gym.spaces.Box(low=0, high=1, dtype=np.float32)] +class HandVecDictType(RCSpaceType): + hand: Annotated[ + Vec18Type, + gym.spaces.Box( + low=np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), + high=np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), + dtype=np.float32, + ), + ] + class CameraDictType(RCSpaceType): frames: dict[ Annotated[str, "camera_names"], @@ -582,7 +593,17 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: return action class HandWrapper(ActObsInfoWrapper): - # TODO: sticky gripper, like in aloha + """ + This wrapper allows for controlling the hand of the robot + using either binary or continuous actions. + The binary action space allows for opening and closing the hand, + while the continuous action space allows for setting the hand + to a specific pose. + The wrapper also provides an observation space that includes + the hand state. + The hand state is represented as a binary value (0 for closed, + 1 for open) or as a continuous value (normalized joint positions). + """ BINARY_HAND_CLOSED = 0 BINARY_HAND_OPEN = 1 @@ -591,12 +612,17 @@ def __init__(self, env, hand, binary: bool = True): super().__init__(env) self.unwrapped: FR3Env self.observation_space: gym.spaces.Dict - self.observation_space.spaces.update(get_space(HandDictType).spaces) self.action_space: gym.spaces.Dict - self.action_space.spaces.update(get_space(HandDictType).spaces) - self.hand_key = get_space_keys(HandDictType)[0] - self._hand = hand self.binary = binary + if self.binary: + self.observation_space.spaces.update(get_space(HandBinDictType).spaces) + self.action_space.spaces.update(get_space(HandBinDictType).spaces) + self.hand_key = get_space_keys(HandBinDictType)[0] + else: + self.observation_space.spaces.update(get_space(HandVecDictType).spaces) + self.action_space.spaces.update(get_space(HandVecDictType).spaces) + self.hand_key = get_space_keys(HandVecDictType)[0] + self._hand = hand self._last_hand_cmd = None def reset(self, **kwargs) -> tuple[dict[str, Any], dict[str, Any]]: @@ -628,10 +654,8 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: if self._last_hand_cmd is None or self._last_hand_cmd != hand_action: if hand_action == self.BINARY_HAND_CLOSED: self._hand.grasp() - print("Hand closed") else: self._hand.open() - print("Hand opened") else: self._hand.set_normalized_joints_poses(hand_action) self._last_hand_cmd = hand_action diff --git a/python/rcsss/envs/space_utils.py b/python/rcsss/envs/space_utils.py index cf9c28b1..69d97d40 100644 --- a/python/rcsss/envs/space_utils.py +++ b/python/rcsss/envs/space_utils.py @@ -17,6 +17,7 @@ Vec7Type: TypeAlias = np.ndarray[Literal[7], np.dtype[np.float64]] Vec3Type: TypeAlias = np.ndarray[Literal[3], np.dtype[np.float64]] Vec6Type: TypeAlias = np.ndarray[Literal[6], np.dtype[np.float64]] +Vec18Type: TypeAlias = np.ndarray[Literal[18], np.dtype[np.float64]] class RCSpaceType(TypedDict): ... diff --git a/python/rcsss/hand/hand.py b/python/rcsss/hand/hand.py index 64725395..b0bbc1e5 100644 --- a/python/rcsss/hand/hand.py +++ b/python/rcsss/hand/hand.py @@ -48,29 +48,4 @@ def open(self): self.reset() def __del__(self): - self._hand.disconnect() - -if __name__ == "__main__": - # try: - # hand = TilburgHandControl(verbose=True) - # hand.grasp(0.9) - # logger.info(f"current joints poses: {hand.get_pos_vector()}") - # logger.info(hand.pos_vector_names_list) - # hand.sleep(5) - # hand.set_zero_pos() - # hand.sleep(1) - # hand.set_joint_pos(hand.fingers_joints.THUMB_CMC, 0.5) - # hand.sleep(2) - # logger.info(f"current joint {hand.fingers_joints.THUMB_CMC.name}: {hand.get_pos_single(hand.fingers_joints.THUMB_CMC)}") - # hand.reset_joint_pos(hand.fingers_joints.THUMB_CMC) - # except Exception as e: - # logger.exception(f"An error occurred: {e}") - # finally: - # hand.disconnect() - hand = Hand(TilburgHandControl(verbose=True)) - hand.reset() - hand.grasp() - sleep(5) - hand.reset() - sleep(2) - hand.disconnect() \ No newline at end of file + self._hand.disconnect() \ No newline at end of file diff --git a/python/rcsss/hand/tilburg_hand_control.py b/python/rcsss/hand/tilburg_hand_control.py index 1828a41e..ed475b06 100644 --- a/python/rcsss/hand/tilburg_hand_control.py +++ b/python/rcsss/hand/tilburg_hand_control.py @@ -9,6 +9,7 @@ # Setup logger logging.basicConfig(level=logging.INFO) logger = logging.getLogger(__name__) +logger.disabled = False class TilburgHandControl(HandControl): """ @@ -71,8 +72,6 @@ def grasp(self, value: float, template: list = None): 0.0, 0.0 ] - - self._pos_normalized = [ val * value for val in template ] From 8855f06e6bce0d551bf10bf84fa57789ee970a29 Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Wed, 16 Apr 2025 17:46:24 +0200 Subject: [PATCH 07/24] fix(hand): add get_pos_vector and set_pos_vector to Handcontol class --- python/rcsss/hand/hand.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/python/rcsss/hand/hand.py b/python/rcsss/hand/hand.py index b0bbc1e5..f713817d 100644 --- a/python/rcsss/hand/hand.py +++ b/python/rcsss/hand/hand.py @@ -16,7 +16,13 @@ def set_zero_pos(self): def disconnect(self): raise NotImplementedError("This method should be overridden by subclasses.") + def get_pos_vector(self): + raise NotImplementedError("This method should be overridden by subclasses.") + + def set_pos_vector(self, values: list): + raise NotImplementedError("This method should be overridden by subclasses.") + class Hand: """ Hand Class From 2a0a00f2d775c2ce2cfe6610c905c8bf13d9d5f1 Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Thu, 17 Apr 2025 14:45:38 +0200 Subject: [PATCH 08/24] refactor(hand): fix pyformat, pylint and pytest --- python/examples/env_cartesian_control.py | 24 +++--- python/rcsss/envs/base.py | 18 +++-- python/rcsss/envs/factories.py | 14 ++-- python/rcsss/hand/hand.py | 34 +++++---- python/rcsss/hand/tilburg_hand_control.py | 89 ++++++++++++++--------- 5 files changed, 103 insertions(+), 76 deletions(-) diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index a62a8daa..5d5a01e8 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -1,4 +1,5 @@ import logging +from typing import Union from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager from rcsss.control.utils import load_creds_fr3_desk @@ -6,7 +7,6 @@ from rcsss.envs.factories import ( default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg, - default_fr3_sim_gripper_cfg, default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, fr3_hw_env, @@ -77,22 +77,17 @@ def main(): env_rel.reset() print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore - + close_action: Union[int, list[float]] + open_action: Union[int, list[float]] if binary_action: close_action = 0 - open_action = 1 + open_action = 1 else: - template = [ - 1.0, 1.0, 1.0, 1.0, - 1.0, 1.0, 1.0, 0.0, - 1.0, 1.0, 1.0, 0.0, - 1.0, 1.0, 1.0, 0.0, - 0.0, 0.0 - ] + template = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0] value = 0.5 - close_action = [value * val for val in template ] + close_action = [value * val for val in template] open_action = [0.0] * len(template) - + for _ in range(10): for _ in range(10): # move 1cm in x direction (forward) and close gripper @@ -102,7 +97,8 @@ def main(): logger.info("Truncated or terminated!") return from time import sleep - sleep(5) + + sleep(5) for _ in range(10): # move 1cm in negative x direction (backward) and open gripper act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "hand": open_action} @@ -111,6 +107,6 @@ def main(): logger.info("Truncated or terminated!") return - + if __name__ == "__main__": main() diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index ae609e3b..1b9abdb8 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -97,10 +97,12 @@ class GripperDictType(RCSpaceType): # 0 for closed, 1 for open (>=0.5 for open) gripper: Annotated[float, gym.spaces.Box(low=0, high=1, dtype=np.float32)] + class HandBinDictType(RCSpaceType): # 0 for closed, 1 for open (>=0.5 for open) hand: Annotated[float, gym.spaces.Box(low=0, high=1, dtype=np.float32)] + class HandVecDictType(RCSpaceType): hand: Annotated[ Vec18Type, @@ -111,6 +113,7 @@ class HandVecDictType(RCSpaceType): ), ] + class CameraDictType(RCSpaceType): frames: dict[ Annotated[str, "camera_names"], @@ -592,6 +595,7 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: del action[self.gripper_key] return action + class HandWrapper(ActObsInfoWrapper): """ This wrapper allows for controlling the hand of the robot @@ -602,7 +606,7 @@ class HandWrapper(ActObsInfoWrapper): The wrapper also provides an observation space that includes the hand state. The hand state is represented as a binary value (0 for closed, - 1 for open) or as a continuous value (normalized joint positions). + 1 for open) or as a continuous value (normalized joint positions). """ BINARY_HAND_CLOSED = 0 @@ -639,7 +643,7 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl else: observation[self.hand_key] = self._hand.get_normalized_joints_poses() - info = False + info = {} return observation, info def action(self, action: dict[str, Any]) -> dict[str, Any]: @@ -652,12 +656,12 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: if self.binary: if self._last_hand_cmd is None or self._last_hand_cmd != hand_action: - if hand_action == self.BINARY_HAND_CLOSED: - self._hand.grasp() - else: + if hand_action == self.BINARY_HAND_CLOSED: + self._hand.grasp() + else: self._hand.open() else: - self._hand.set_normalized_joints_poses(hand_action) + self._hand.set_normalized_joints_poses(hand_action) self._last_hand_cmd = hand_action del action[self.hand_key] - return action \ No newline at end of file + return action diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index f07a1860..b351d1a0 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -13,16 +13,14 @@ from rcsss.camera.interface import BaseCameraConfig from rcsss.camera.realsense import RealSenseCameraSet, RealSenseSetConfig from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig -from rcsss.hand.hand import Hand -from rcsss.hand.tilburg_hand_control import TilburgHandControl from rcsss.envs.base import ( CameraSetWrapper, ControlMode, FR3Env, GripperWrapper, + HandWrapper, RelativeActionSpace, RelativeTo, - HandWrapper ) from rcsss.envs.hw import FR3HW from rcsss.envs.sim import ( @@ -32,6 +30,8 @@ RandomCubePos, SimWrapper, ) +from rcsss.hand.hand import Hand +from rcsss.hand.tilburg_hand_control import TilburgHandControl logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -187,7 +187,7 @@ def fr3_sim_env( urdf_path: str | PathLike | None = None, mjcf: str | PathLike = "fr3_empty_world", sim_wrapper: Type[SimWrapper] | None = None, - hand_cfg: dict = None, + hand_cfg: dict | None = None, ) -> gym.Env: """ Creates a simulation environment for the FR3 robot. @@ -230,12 +230,12 @@ def fr3_sim_env( if gripper_cfg is not None: gripper = sim.FrankaHand(simulation, "0", gripper_cfg) env = GripperWrapper(env, gripper, binary=True) - - else: + + else: if hand_cfg is not None: hand = Hand(TilburgHandControl()) env = HandWrapper(env, hand, binary=hand_cfg["Binary"]) - + if collision_guard: env = CollisionGuard.env_from_xml_paths( env, diff --git a/python/rcsss/hand/hand.py b/python/rcsss/hand/hand.py index f713817d..9a821de6 100644 --- a/python/rcsss/hand/hand.py +++ b/python/rcsss/hand/hand.py @@ -1,46 +1,54 @@ - class HandControl: """ Base class for hand control. This class provides an interface for hand control, but does not implement any functionality. """ + def __init__(self): pass def grasp(self, value: float): - raise NotImplementedError("This method should be overridden by subclasses.") + self.value = value # to pass pylint + message = "This method should be overridden by subclasses." + raise NotImplementedError(message) def set_zero_pos(self): - raise NotImplementedError("This method should be overridden by subclasses.") + message = "This method should be overridden by subclasses." + raise NotImplementedError(message) def disconnect(self): - raise NotImplementedError("This method should be overridden by subclasses.") - + message = "This method should be overridden by subclasses." + raise NotImplementedError(message) + def get_pos_vector(self): - raise NotImplementedError("This method should be overridden by subclasses.") - + message = "This method should be overridden by subclasses." + raise NotImplementedError(message) + def set_pos_vector(self, values: list): - raise NotImplementedError("This method should be overridden by subclasses.") + self.values = values # to pass pylint + message = "This method should be overridden by subclasses." + raise NotImplementedError(message) + - class Hand: """ Hand Class This class provides an interface for hand control. It allows for grasping, resetting, and disconnecting from the hand. """ + def __init__(self, hand: HandControl): self._hand = hand def grasp(self): - self._hand.grasp(value = 0.9) + self._hand.grasp(value=0.9) def reset(self): self._hand.set_zero_pos() def get_state(self): pass - + def disconnect(self): self._hand.disconnect() @@ -48,10 +56,10 @@ def get_normalized_joints_poses(self): self._hand.get_pos_vector() def set_normalized_joints_poses(self, values: list): - self._hand.set_pos_vector(values) + self._hand.set_pos_vector(values) def open(self): self.reset() def __del__(self): - self._hand.disconnect() \ No newline at end of file + self._hand.disconnect() diff --git a/python/rcsss/hand/tilburg_hand_control.py b/python/rcsss/hand/tilburg_hand_control.py index ed475b06..263b83a2 100644 --- a/python/rcsss/hand/tilburg_hand_control.py +++ b/python/rcsss/hand/tilburg_hand_control.py @@ -1,22 +1,25 @@ -import sys -import os import copy +import logging +import os +import sys from time import sleep + from rcsss.hand.hand import HandControl -import logging -from tilburg_hand import TilburgHandMotorInterface, Finger, Wrist, Unit +from tilburg_hand import Finger, TilburgHandMotorInterface, Unit, Wrist # Setup logger logging.basicConfig(level=logging.INFO) logger = logging.getLogger(__name__) logger.disabled = False + class TilburgHandControl(HandControl): """ Tilburg Hand Control Class This class provides an interface for controlling the Tilburg Hand. It allows for grasping, resetting, and disconnecting from the hand. """ + def __init__(self, config_folder_path="config", verbose=False): """ Initializes the Tilburg Hand Control interface. @@ -28,11 +31,11 @@ def __init__(self, config_folder_path="config", verbose=False): self._motors = TilburgHandMotorInterface( config_file=self.tilburg_hand_config_file_path, calibration_file=self.tilburg_hand_calibration_file_path, - verbose=verbose + verbose=verbose, ) self.pos_value_unit = Unit.NORMALIZED - self._pos_normalized = [0] * self._motors.n_motors + self._pos_normalized = [0.0] * self._motors.n_motors if self._motors.connect() < 0: logger.error("Failed to connect to the motors' board.") sys.exit(1) @@ -41,40 +44,56 @@ def __init__(self, config_folder_path="config", verbose=False): self.fingers_joints = Finger self.fingers_joints_list = [ - Finger.INDEX_ABD, Finger.MIDDLE_ABD, Finger.THUMB_ABD, Finger.RING_ABD, - Finger.INDEX_DIP, Finger.MIDDLE_DIP, Finger.RING_DIP, Finger.THUMB_IP, - Finger.INDEX_PIP, Finger.MIDDLE_PIP, Finger.RING_PIP, - Finger.INDEX_MCP, Finger.MIDDLE_MCP, Finger.RING_MCP, Finger.THUMB_MCP, - Finger.THUMB_CMC + Finger.INDEX_ABD, + Finger.MIDDLE_ABD, + Finger.THUMB_ABD, + Finger.RING_ABD, + Finger.INDEX_DIP, + Finger.MIDDLE_DIP, + Finger.RING_DIP, + Finger.THUMB_IP, + Finger.INDEX_PIP, + Finger.MIDDLE_PIP, + Finger.RING_PIP, + Finger.INDEX_MCP, + Finger.MIDDLE_MCP, + Finger.RING_MCP, + Finger.THUMB_MCP, + Finger.THUMB_CMC, ] self.pos_vector_names_list = [ - "THUMB_IP", "THUMB_MCP", "THUMB_ABD", "THUMB_CMC", - "INDEX_DIP", "INDEX_PIP", "INDEX_MCP", "INDEX_ABD", - "MIDDLE_DIP", "MIDDLE_PIP", "MIDDLE_MCP", "MIDDLE_ABD", - "RING_DIP", "RING_PIP", "RING_MCP", "RING_ABD", - "WRIST_PITCH", "WRIST_YAW" - ] + "THUMB_IP", + "THUMB_MCP", + "THUMB_ABD", + "THUMB_CMC", + "INDEX_DIP", + "INDEX_PIP", + "INDEX_MCP", + "INDEX_ABD", + "MIDDLE_DIP", + "MIDDLE_PIP", + "MIDDLE_MCP", + "MIDDLE_ABD", + "RING_DIP", + "RING_PIP", + "RING_MCP", + "RING_ABD", + "WRIST_PITCH", + "WRIST_YAW", + ] self.wrist_joints_list = [Wrist.PITCH, Wrist.YAW] self.set_zero_pos() logger.info("Setting all joints to zero position.") - def grasp(self, value: float, template: list = None): + def grasp(self, value: float, template: list | None = None): """ Performs a grasp with a specified intensity (0.0 to 1.0). """ if template is None: - template = [ - 1.0, 1.0, 1.0, 1.0, - 1.0, 1.0, 1.0, 0.0, - 1.0, 1.0, 1.0, 0.0, - 1.0, 1.0, 1.0, 0.0, - 0.0, 0.0 - ] - - self._pos_normalized = [ - val * value for val in template - ] + template = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0] + + self._pos_normalized = [val * value for val in template] self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) logger.info(f"Grasp command sent with value: {value}") @@ -95,7 +114,7 @@ def set_zero_pos(self): """ Sets all finger joint positions to zero. """ - self._pos_normalized = [0] * self._motors.n_motors + self._pos_normalized = [0.0] * self._motors.n_motors self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) logger.info("All joints reset to zero position.") @@ -105,15 +124,15 @@ def sleep(self, seconds: float): """ sleep(seconds) - def set_joint_pos(self, finger_joint, pos_value: float): + def set_joint_pos(self, finger_joint : Finger, pos_value: float): """ Sets a single joint to a specific normalized position. """ self._motors.set_pos_single(finger_joint, pos_value, unit=self.pos_value_unit) - self._pos_normalized[finger_joint] = pos_value + self._pos_normalized[int(finger_joint)] = pos_value logger.info(f"Set joint {finger_joint.name} to {pos_value:.2f}") - def reset_joint_pos(self, finger_joint): + def reset_joint_pos(self, finger_joint : Finger): """ Resets a specific joint to zero. """ @@ -133,9 +152,9 @@ def get_pos_vector(self): Returns the current position vector of the motors. """ return self._pos_normalized - + def get_pos_single(self, finger_joint): """ Returns the current position of a single joint. """ - return self._pos_normalized[finger_joint] \ No newline at end of file + return self._pos_normalized[finger_joint] From 2511e7bbad793ae46ad17c3957729abef9d8098c Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Thu, 17 Apr 2025 17:02:11 +0200 Subject: [PATCH 09/24] fix(hand): make default value in TilburgHandControl class instead of HandControl class --- python/rcsss/hand/hand.py | 2 +- python/rcsss/hand/tilburg_hand_control.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/python/rcsss/hand/hand.py b/python/rcsss/hand/hand.py index 9a821de6..301c76ca 100644 --- a/python/rcsss/hand/hand.py +++ b/python/rcsss/hand/hand.py @@ -41,7 +41,7 @@ def __init__(self, hand: HandControl): self._hand = hand def grasp(self): - self._hand.grasp(value=0.9) + self._hand.grasp() def reset(self): self._hand.set_zero_pos() diff --git a/python/rcsss/hand/tilburg_hand_control.py b/python/rcsss/hand/tilburg_hand_control.py index 263b83a2..134f3339 100644 --- a/python/rcsss/hand/tilburg_hand_control.py +++ b/python/rcsss/hand/tilburg_hand_control.py @@ -86,7 +86,7 @@ def __init__(self, config_folder_path="config", verbose=False): self.set_zero_pos() logger.info("Setting all joints to zero position.") - def grasp(self, value: float, template: list | None = None): + def grasp(self, value: float = 0.9, template: list | None = None): """ Performs a grasp with a specified intensity (0.0 to 1.0). """ From a1e829415d1208667d694d90a70d81f634d4a4a1 Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Thu, 17 Apr 2025 20:25:21 +0200 Subject: [PATCH 10/24] refactor(hand): fix pyformat, pylint and pytest --- python/rcsss/envs/factories.py | 4 ++-- python/rcsss/hand/hand.py | 8 ++++---- python/rcsss/hand/tilburg_hand_control.py | 4 ++-- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 4b67d1c5..8b80ff98 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -27,8 +27,6 @@ RandomCubePos, SimWrapper, ) -from rcsss.hand.hand import Hand -from rcsss.hand.tilburg_hand_control import TilburgHandControl from rcsss.envs.space_utils import Vec7Type from rcsss.envs.utils import ( default_fr3_hw_gripper_cfg, @@ -38,6 +36,8 @@ default_realsense, get_urdf_path, ) +from rcsss.hand.hand import Hand +from rcsss.hand.tilburg_hand_control import TilburgHandControl logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/python/rcsss/hand/hand.py b/python/rcsss/hand/hand.py index 301c76ca..99aed207 100644 --- a/python/rcsss/hand/hand.py +++ b/python/rcsss/hand/hand.py @@ -8,7 +8,7 @@ def __init__(self): pass def grasp(self, value: float): - self.value = value # to pass pylint + self.value = value # to pass pylint message = "This method should be overridden by subclasses." raise NotImplementedError(message) @@ -25,7 +25,7 @@ def get_pos_vector(self): raise NotImplementedError(message) def set_pos_vector(self, values: list): - self.values = values # to pass pylint + self.values = values # to pass pylint message = "This method should be overridden by subclasses." raise NotImplementedError(message) @@ -40,8 +40,8 @@ class Hand: def __init__(self, hand: HandControl): self._hand = hand - def grasp(self): - self._hand.grasp() + def grasp(self, value): + self._hand.grasp(value) def reset(self): self._hand.set_zero_pos() diff --git a/python/rcsss/hand/tilburg_hand_control.py b/python/rcsss/hand/tilburg_hand_control.py index 134f3339..3841f8f3 100644 --- a/python/rcsss/hand/tilburg_hand_control.py +++ b/python/rcsss/hand/tilburg_hand_control.py @@ -124,7 +124,7 @@ def sleep(self, seconds: float): """ sleep(seconds) - def set_joint_pos(self, finger_joint : Finger, pos_value: float): + def set_joint_pos(self, finger_joint: Finger, pos_value: float): """ Sets a single joint to a specific normalized position. """ @@ -132,7 +132,7 @@ def set_joint_pos(self, finger_joint : Finger, pos_value: float): self._pos_normalized[int(finger_joint)] = pos_value logger.info(f"Set joint {finger_joint.name} to {pos_value:.2f}") - def reset_joint_pos(self, finger_joint : Finger): + def reset_joint_pos(self, finger_joint: Finger): """ Resets a specific joint to zero. """ From 5ab655c102de5bd7443fd9699f945c0802ab0c6d Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Tue, 22 Apr 2025 16:59:32 +0200 Subject: [PATCH 11/24] refactor(hand): beteter handling of default grasp value and add tilburg-hand library to requieremnts_dev.txt --- python/rcsss/hand/hand.py | 7 +++++-- python/rcsss/hand/tilburg_hand_control.py | 9 +++++---- requirements_dev.txt | 1 + 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/python/rcsss/hand/hand.py b/python/rcsss/hand/hand.py index 99aed207..1a196999 100644 --- a/python/rcsss/hand/hand.py +++ b/python/rcsss/hand/hand.py @@ -1,3 +1,6 @@ +from typing import Optional + + class HandControl: """ Base class for hand control. @@ -7,7 +10,7 @@ class HandControl: def __init__(self): pass - def grasp(self, value: float): + def grasp(self, value: Optional[float] = None): self.value = value # to pass pylint message = "This method should be overridden by subclasses." raise NotImplementedError(message) @@ -40,7 +43,7 @@ class Hand: def __init__(self, hand: HandControl): self._hand = hand - def grasp(self, value): + def grasp(self, value: Optional[float] = None): self._hand.grasp(value) def reset(self): diff --git a/python/rcsss/hand/tilburg_hand_control.py b/python/rcsss/hand/tilburg_hand_control.py index 3841f8f3..5a3cc931 100644 --- a/python/rcsss/hand/tilburg_hand_control.py +++ b/python/rcsss/hand/tilburg_hand_control.py @@ -3,6 +3,7 @@ import os import sys from time import sleep +from typing import Optional from rcsss.hand.hand import HandControl from tilburg_hand import Finger, TilburgHandMotorInterface, Unit, Wrist @@ -86,13 +87,13 @@ def __init__(self, config_folder_path="config", verbose=False): self.set_zero_pos() logger.info("Setting all joints to zero position.") - def grasp(self, value: float = 0.9, template: list | None = None): + def grasp(self, value: Optional[float] = None): """ Performs a grasp with a specified intensity (0.0 to 1.0). """ - if template is None: - template = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0] - + template = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0] + if value is None: + value = 0.9 self._pos_normalized = [val * value for val in template] self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) diff --git a/requirements_dev.txt b/requirements_dev.txt index 67752008..7851d1de 100644 --- a/requirements_dev.txt +++ b/requirements_dev.txt @@ -12,3 +12,4 @@ pybind11 mujoco==3.2.6 pin==2.7.0 wheel +tilburg-hand From ac8d35f04fcbe1750ef499b20a85a6d6bf35f463 Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Tue, 22 Apr 2025 18:19:02 +0200 Subject: [PATCH 12/24] fix(env): fix some of the requersted changes --- .../examples/env_cartesian_control_tilburg.py | 111 ++++++++++++++++++ python/rcsss/envs/base.py | 3 +- python/rcsss/hand/hand.py | 2 +- 3 files changed, 114 insertions(+), 2 deletions(-) create mode 100644 python/examples/env_cartesian_control_tilburg.py diff --git a/python/examples/env_cartesian_control_tilburg.py b/python/examples/env_cartesian_control_tilburg.py new file mode 100644 index 00000000..cf82de1a --- /dev/null +++ b/python/examples/env_cartesian_control_tilburg.py @@ -0,0 +1,111 @@ +import logging +from typing import Union + +from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager +from rcsss.control.utils import load_creds_fr3_desk +from rcsss.envs.base import ControlMode, RelativeTo, RobotInstance +from rcsss.envs.factories import fr3_hw_env, fr3_sim_env +from rcsss.envs.utils import ( + default_fr3_hw_gripper_cfg, + default_fr3_hw_robot_cfg, + default_fr3_sim_robot_cfg, + default_mujoco_cameraset_cfg, +) + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +ROBOT_IP = "192.168.101.1" +ROBOT_INSTANCE = RobotInstance.SIMULATION + + +""" +Create a .env file in the same directory as this file with the following content: +FR3_USERNAME= +FR3_PASSWORD= + +When you use a real FR3 you first need to unlock its joints using the following cli script: + +python -m rcsss fr3 unlock + +or put it into guiding mode using: + +python -m rcsss fr3 guiding-mode + +When you are done you lock it again using: + +python -m rcsss fr3 lock + +or even shut it down using: + +python -m rcsss fr3 shutdown +""" + + +def main(): + resource_manger: FCI | DummyResourceManager + if ROBOT_INSTANCE == RobotInstance.HARDWARE: + user, pw = load_creds_fr3_desk() + resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) + else: + resource_manger = DummyResourceManager() + with resource_manger: + binary_action = False + if ROBOT_INSTANCE == RobotInstance.HARDWARE: + env_rel = fr3_hw_env( + ip=ROBOT_IP, + control_mode=ControlMode.CARTESIAN_TQuart, + robot_cfg=default_fr3_hw_robot_cfg(), + collision_guard="lab", + gripper_cfg=default_fr3_hw_gripper_cfg(), + max_relative_movement=0.5, + relative_to=RelativeTo.LAST_STEP, + ) + else: + env_rel = fr3_sim_env( + control_mode=ControlMode.CARTESIAN_TQuart, + robot_cfg=default_fr3_sim_robot_cfg(), + collision_guard=False, + gripper_cfg=None, + camera_set_cfg=default_mujoco_cameraset_cfg(), + max_relative_movement=0.5, + relative_to=RelativeTo.LAST_STEP, + hand_cfg={"Binary": binary_action}, + ) + env_rel.get_wrapper_attr("sim").open_gui() + + env_rel.reset() + print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore + close_action: Union[int, list[float]] + open_action: Union[int, list[float]] + if binary_action: + close_action = 0 + open_action = 1 + else: + template = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0] + value = 0.5 + close_action = [value * val for val in template] + open_action = [0.0] * len(template) + + for _ in range(10): + for _ in range(10): + # move 1cm in x direction (forward) and close gripper + act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "hand": close_action} + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + from time import sleep + + sleep(5) + for _ in range(10): + # move 1cm in negative x direction (backward) and open gripper + act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "hand": open_action} + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + + +if __name__ == "__main__": + main() diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index 1b9abdb8..a6b553ae 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -18,6 +18,7 @@ get_space, get_space_keys, ) +from rcsss.hand.hand import Hand _logger = logging.getLogger() @@ -612,7 +613,7 @@ class HandWrapper(ActObsInfoWrapper): BINARY_HAND_CLOSED = 0 BINARY_HAND_OPEN = 1 - def __init__(self, env, hand, binary: bool = True): + def __init__(self, env, hand: Hand, binary: bool = True): super().__init__(env) self.unwrapped: FR3Env self.observation_space: gym.spaces.Dict diff --git a/python/rcsss/hand/hand.py b/python/rcsss/hand/hand.py index 1a196999..ecd45851 100644 --- a/python/rcsss/hand/hand.py +++ b/python/rcsss/hand/hand.py @@ -27,7 +27,7 @@ def get_pos_vector(self): message = "This method should be overridden by subclasses." raise NotImplementedError(message) - def set_pos_vector(self, values: list): + def set_pos_vector(self, values: list[float]): self.values = values # to pass pylint message = "This method should be overridden by subclasses." raise NotImplementedError(message) From 00ba9f1d3ccd673bca596b673455f8e8390f9f74 Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Wed, 23 Apr 2025 19:52:04 +0200 Subject: [PATCH 13/24] refactor(envs, hand, examples): address the requested changes - Add dataclass for configuration of Tilburg hand - Remove config folder from hand module --- python/examples/env_cartesian_control.py | 1 - python/examples/env_cartesian_control_tilburg.py | 7 +++---- python/rcsss/envs/factories.py | 14 ++++++-------- python/rcsss/envs/utils.py | 3 +++ python/rcsss/hand/config/calibration.json | 1 - python/rcsss/hand/config/config.json | 8 -------- python/rcsss/hand/interface.py | 11 +++++++++++ python/rcsss/hand/tilburg_hand_control.py | 9 +++------ 8 files changed, 26 insertions(+), 28 deletions(-) delete mode 100644 python/rcsss/hand/config/calibration.json delete mode 100644 python/rcsss/hand/config/config.json create mode 100644 python/rcsss/hand/interface.py diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index dc81a830..7e3daf3f 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -70,7 +70,6 @@ def main(): camera_set_cfg=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, - hand_cfg={"Binary": binary_action}, ) env_rel.get_wrapper_attr("sim").open_gui() diff --git a/python/examples/env_cartesian_control_tilburg.py b/python/examples/env_cartesian_control_tilburg.py index cf82de1a..ac3ca749 100644 --- a/python/examples/env_cartesian_control_tilburg.py +++ b/python/examples/env_cartesian_control_tilburg.py @@ -10,6 +10,7 @@ default_fr3_hw_robot_cfg, default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, + default_tilburg_hw_hand_cfg, ) logger = logging.getLogger(__name__) @@ -50,7 +51,6 @@ def main(): else: resource_manger = DummyResourceManager() with resource_manger: - binary_action = False if ROBOT_INSTANCE == RobotInstance.HARDWARE: env_rel = fr3_hw_env( ip=ROBOT_IP, @@ -66,11 +66,10 @@ def main(): control_mode=ControlMode.CARTESIAN_TQuart, robot_cfg=default_fr3_sim_robot_cfg(), collision_guard=False, - gripper_cfg=None, + gripper_cfg=default_tilburg_hw_hand_cfg(), camera_set_cfg=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, - hand_cfg={"Binary": binary_action}, ) env_rel.get_wrapper_attr("sim").open_gui() @@ -78,7 +77,7 @@ def main(): print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore close_action: Union[int, list[float]] open_action: Union[int, list[float]] - if binary_action: + if default_tilburg_hw_hand_cfg().binary_action: close_action = 0 open_action = 1 else: diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 8b80ff98..3fc34093 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -48,7 +48,7 @@ def fr3_hw_env( control_mode: ControlMode, robot_cfg: rcsss.hw.FR3Config, collision_guard: str | PathLike | None = None, - gripper_cfg: rcsss.hw.FHConfig | None = None, + gripper_cfg: rcsss.hw.FHConfig | rcsss.hand.interface.THConfig | None = None, camera_set: BaseHardwareCameraSet | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, @@ -84,7 +84,7 @@ def fr3_hw_env( env: gym.Env = FR3Env(robot, ControlMode.JOINTS if collision_guard is not None else control_mode) env = FR3HW(env) - if gripper_cfg is not None: + if isinstance(gripper_cfg, rcsss.sim.FHConfig): gripper = rcsss.hw.FrankaHand(ip, gripper_cfg) env = GripperWrapper(env, gripper, binary=True) @@ -124,7 +124,6 @@ def fr3_sim_env( urdf_path: str | PathLike | None = None, mjcf: str | PathLike = "fr3_empty_world", sim_wrapper: Type[SimWrapper] | None = None, - hand_cfg: dict | None = None, ) -> gym.Env: """ Creates a simulation environment for the FR3 robot. @@ -165,14 +164,13 @@ def fr3_sim_env( camera_set = SimCameraSet(simulation, camera_set_cfg) env = CameraSetWrapper(env, camera_set, include_depth=True) - if gripper_cfg is not None: + if isinstance(gripper_cfg, rcsss.sim.FHConfig): gripper = sim.FrankaHand(simulation, "0", gripper_cfg) env = GripperWrapper(env, gripper, binary=True) - else: - if hand_cfg is not None: - hand = Hand(TilburgHandControl()) - env = HandWrapper(env, hand, binary=hand_cfg["Binary"]) + elif isinstance(gripper_cfg, rcsss.hand.interface.THConfig): + hand = Hand(TilburgHandControl(gripper_cfg)) + env = HandWrapper(env, hand, binary=gripper_cfg.binary_action) if collision_guard: env = CollisionGuard.env_from_xml_paths( diff --git a/python/rcsss/envs/utils.py b/python/rcsss/envs/utils.py index 9b9590e2..8f843726 100644 --- a/python/rcsss/envs/utils.py +++ b/python/rcsss/envs/utils.py @@ -39,6 +39,9 @@ def default_fr3_hw_gripper_cfg(): return gripper_cfg +def default_tilburg_hw_hand_cfg(): + return rcsss.hand.interface.THConfig() + def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | None: if name2id is None: return None diff --git a/python/rcsss/hand/config/calibration.json b/python/rcsss/hand/config/calibration.json deleted file mode 100644 index a5a7ed7c..00000000 --- a/python/rcsss/hand/config/calibration.json +++ /dev/null @@ -1 +0,0 @@ -{"motor_calib_min_range_deg": [-5, 0, 0, 0, -5, -5, 0, -25, -5, -5, 0, -25, -5, -5, 0, -25, 0, 0], "motor_calib_max_range_deg": [95, 90, 100, 90, 95, 95, 95, 25, 95, 95, 95, 25, 95, 95, 95, 25, 0, 0], "motor_calib_min_range_ticks": [1991, 2048, 2048, 2048, 1991, 1991, 2048, 2332, 1991, 1991, 2048, 2332, 1991, 1991, 2048, 2332, 0, 0], "motor_calib_max_range_ticks": [3128, 1024, 910, 3072, 3128, 3128, 967, 1763, 3128, 3128, 967, 1763, 3128, 3128, 967, 1763, 0, 0], "motor_calib_zero_pos_ticks": [3088, 1351, 2044, 2918, 2048, 2060, 1860, 2089, 2051, 2053, 1876, 2082, 2047, 2047, 1929, 2031, 512, 512], "hand_orientation": "right"} \ No newline at end of file diff --git a/python/rcsss/hand/config/config.json b/python/rcsss/hand/config/config.json deleted file mode 100644 index 53bc3d0b..00000000 --- a/python/rcsss/hand/config/config.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "motors_dynamixelsdk_port" : "", - "motors_dynamixelsdk_VID_PID" : "0403:6014", - - "joint_motor_mapping" : [23, 22, 21, 20, 27, 26, 25, 24, 31, 30, 29, 28, 35, 34, 33, 32, 255, 255], - - "_comments": "If usb port names are left blank, then all ports are scanned for the pre-set VID:PID. If a port is set, it will override the automatic search. Motors will show as disabled if the app can't connect to the control board. All motors are pinged at the beginning, and non-responding / non-connected ones are set as disabled. joint_motor_mapping is used as, e.g., motor_id = joint_motor_mapping[Finger.INDEX_ABD]." -} diff --git a/python/rcsss/hand/interface.py b/python/rcsss/hand/interface.py new file mode 100644 index 00000000..68fe7c71 --- /dev/null +++ b/python/rcsss/hand/interface.py @@ -0,0 +1,11 @@ +import os +from pathlib import Path + +from pydantic import BaseModel + + +class THConfig(BaseModel): + """Config for the Tilburg hand""" + + binary_action: bool = True + calibration_file_path: str = os.path.join(Path.home(), "tilburg_hand/calibration.json") diff --git a/python/rcsss/hand/tilburg_hand_control.py b/python/rcsss/hand/tilburg_hand_control.py index 5a3cc931..5d83310d 100644 --- a/python/rcsss/hand/tilburg_hand_control.py +++ b/python/rcsss/hand/tilburg_hand_control.py @@ -1,11 +1,11 @@ import copy import logging -import os import sys from time import sleep from typing import Optional from rcsss.hand.hand import HandControl +from rcsss.hand.interface import THConfig from tilburg_hand import Finger, TilburgHandMotorInterface, Unit, Wrist # Setup logger @@ -21,16 +21,13 @@ class TilburgHandControl(HandControl): It allows for grasping, resetting, and disconnecting from the hand. """ - def __init__(self, config_folder_path="config", verbose=False): + def __init__(self, tilburghand_cfg: Optional[THConfig] = None, verbose: bool = False): """ Initializes the Tilburg Hand Control interface. """ - self.config_folder_path = config_folder_path - self.tilburg_hand_config_file_path = os.path.join(config_folder_path, "config.json") - self.tilburg_hand_calibration_file_path = os.path.join(config_folder_path, "calibration.json") + self.tilburg_hand_calibration_file_path = tilburghand_cfg.calibration_file_path if tilburghand_cfg else None self._motors = TilburgHandMotorInterface( - config_file=self.tilburg_hand_config_file_path, calibration_file=self.tilburg_hand_calibration_file_path, verbose=verbose, ) From f6a2e65f3f9167756480a8eb2f5a5b3cbefb063f Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Wed, 23 Apr 2025 20:01:56 +0200 Subject: [PATCH 14/24] refactor(envs): fix pyformat, pylint and pytest --- python/rcsss/envs/utils.py | 1 + 1 file changed, 1 insertion(+) diff --git a/python/rcsss/envs/utils.py b/python/rcsss/envs/utils.py index 8f843726..43a17074 100644 --- a/python/rcsss/envs/utils.py +++ b/python/rcsss/envs/utils.py @@ -42,6 +42,7 @@ def default_fr3_hw_gripper_cfg(): def default_tilburg_hw_hand_cfg(): return rcsss.hand.interface.THConfig() + def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | None: if name2id is None: return None From 7bb2d36c42bc61926c91e2c6ce12675e7db30004 Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Tue, 6 May 2025 18:09:46 +0200 Subject: [PATCH 15/24] refactor(examples): - revert env_cartesian_control.py to master version - Put default_tilburg_hw_hand_cfg in HARDWARE instead of SIMULATION --- python/examples/env_cartesian_control.py | 25 +++++-------------- .../examples/env_cartesian_control_tilburg.py | 8 +++--- 2 files changed, 10 insertions(+), 23 deletions(-) diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index 7e3daf3f..2f383c08 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -1,5 +1,4 @@ import logging -from typing import Union from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager from rcsss.control.utils import load_creds_fr3_desk @@ -8,6 +7,7 @@ from rcsss.envs.utils import ( default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg, + default_fr3_sim_gripper_cfg, default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, ) @@ -49,8 +49,8 @@ def main(): resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) else: resource_manger = DummyResourceManager() + with resource_manger: - binary_action = True if ROBOT_INSTANCE == RobotInstance.HARDWARE: env_rel = fr3_hw_env( ip=ROBOT_IP, @@ -66,7 +66,7 @@ def main(): control_mode=ControlMode.CARTESIAN_TQuart, robot_cfg=default_fr3_sim_robot_cfg(), collision_guard=False, - gripper_cfg=None, + gripper_cfg=default_fr3_sim_gripper_cfg(), camera_set_cfg=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, @@ -75,31 +75,18 @@ def main(): env_rel.reset() print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore - close_action: Union[int, list[float]] - open_action: Union[int, list[float]] - if binary_action: - close_action = 0 - open_action = 1 - else: - template = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0] - value = 0.5 - close_action = [value * val for val in template] - open_action = [0.0] * len(template) for _ in range(10): for _ in range(10): # move 1cm in x direction (forward) and close gripper - act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "hand": close_action} + act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") return - from time import sleep - - sleep(5) for _ in range(10): # move 1cm in negative x direction (backward) and open gripper - act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "hand": open_action} + act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") @@ -107,4 +94,4 @@ def main(): if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/python/examples/env_cartesian_control_tilburg.py b/python/examples/env_cartesian_control_tilburg.py index ac3ca749..1867ee79 100644 --- a/python/examples/env_cartesian_control_tilburg.py +++ b/python/examples/env_cartesian_control_tilburg.py @@ -6,11 +6,11 @@ from rcsss.envs.base import ControlMode, RelativeTo, RobotInstance from rcsss.envs.factories import fr3_hw_env, fr3_sim_env from rcsss.envs.utils import ( - default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg, + default_tilburg_hw_hand_cfg, + default_fr3_sim_gripper_cfg, default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, - default_tilburg_hw_hand_cfg, ) logger = logging.getLogger(__name__) @@ -57,7 +57,7 @@ def main(): control_mode=ControlMode.CARTESIAN_TQuart, robot_cfg=default_fr3_hw_robot_cfg(), collision_guard="lab", - gripper_cfg=default_fr3_hw_gripper_cfg(), + gripper_cfg=default_tilburg_hw_hand_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, ) @@ -66,7 +66,7 @@ def main(): control_mode=ControlMode.CARTESIAN_TQuart, robot_cfg=default_fr3_sim_robot_cfg(), collision_guard=False, - gripper_cfg=default_tilburg_hw_hand_cfg(), + gripper_cfg=default_fr3_sim_gripper_cfg(), camera_set_cfg=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, From 75fc5ca633bc632f96520d60e0586320b9762b58 Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Tue, 6 May 2025 18:13:59 +0200 Subject: [PATCH 16/24] refactor(examples): pyformat, pylint and pytest --- python/examples/env_cartesian_control.py | 2 +- python/examples/env_cartesian_control_tilburg.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index 2f383c08..d8e9c5f7 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -94,4 +94,4 @@ def main(): if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/python/examples/env_cartesian_control_tilburg.py b/python/examples/env_cartesian_control_tilburg.py index 1867ee79..d36a305d 100644 --- a/python/examples/env_cartesian_control_tilburg.py +++ b/python/examples/env_cartesian_control_tilburg.py @@ -7,10 +7,10 @@ from rcsss.envs.factories import fr3_hw_env, fr3_sim_env from rcsss.envs.utils import ( default_fr3_hw_robot_cfg, - default_tilburg_hw_hand_cfg, default_fr3_sim_gripper_cfg, default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, + default_tilburg_hw_hand_cfg, ) logger = logging.getLogger(__name__) From 9a20d99f5e737e7123b1053c0024b38e8afaf627 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 19 May 2025 10:16:16 +0200 Subject: [PATCH 17/24] refactor(hand): python protocol interface - make architecture leaner: just a python protocol class that defines the interface, removed bridge pattern to removed interface overhead - removed example support of non binary grasps - added useful configurations to the config such as control unit - moved tilburg hand dependency to pyproject.toml --- pyproject.toml | 3 +- .../examples/env_cartesian_control_tilburg.py | 13 +- python/rcsss/__init__.py | 4 +- python/rcsss/envs/base.py | 4 +- python/rcsss/envs/factories.py | 10 +- python/rcsss/envs/utils.py | 12 +- python/rcsss/hand/hand.py | 68 ------- python/rcsss/hand/interface.py | 34 +++- python/rcsss/hand/tilburg_hand_control.py | 188 +++++++++--------- requirements_dev.txt | 1 - 10 files changed, 145 insertions(+), 192 deletions(-) delete mode 100644 python/rcsss/hand/hand.py diff --git a/pyproject.toml b/pyproject.toml index f2fde75e..1c63919f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -25,7 +25,8 @@ dependencies = ["websockets>=11.0", # NOTE: when changing the mujoco version, also change it in requirements_dev.txt "mujoco==3.2.6", # NOTE: Same for pinocchio (pin) - "pin==2.7.0" + "pin==2.7.0", + "tilburg-hand", ] readme = "README.md" maintainers = [ diff --git a/python/examples/env_cartesian_control_tilburg.py b/python/examples/env_cartesian_control_tilburg.py index d36a305d..c2fcc5a9 100644 --- a/python/examples/env_cartesian_control_tilburg.py +++ b/python/examples/env_cartesian_control_tilburg.py @@ -1,5 +1,4 @@ import logging -from typing import Union from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager from rcsss.control.utils import load_creds_fr3_desk @@ -75,16 +74,8 @@ def main(): env_rel.reset() print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore - close_action: Union[int, list[float]] - open_action: Union[int, list[float]] - if default_tilburg_hw_hand_cfg().binary_action: - close_action = 0 - open_action = 1 - else: - template = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0] - value = 0.5 - close_action = [value * val for val in template] - open_action = [0.0] * len(template) + close_action = 0 + open_action = 1 for _ in range(10): for _ in range(10): diff --git a/python/rcsss/__init__.py b/python/rcsss/__init__.py index 63d4cf6f..45edce33 100644 --- a/python/rcsss/__init__.py +++ b/python/rcsss/__init__.py @@ -4,7 +4,7 @@ import site from gymnasium import register -from rcsss import camera, control, envs, sim +from rcsss import camera, control, envs, hand, sim from rcsss._core import __version__, common, hw from rcsss.envs.factories import ( FR3LabPickUpSimDigitHand, @@ -19,7 +19,7 @@ } # make submodules available -__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs"] +__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs", "hand"] # register gymnasium environments register( diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index a6b553ae..e7c7a409 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -18,7 +18,7 @@ get_space, get_space_keys, ) -from rcsss.hand.hand import Hand +from rcsss.hand.interface import BaseHandControl _logger = logging.getLogger() @@ -613,7 +613,7 @@ class HandWrapper(ActObsInfoWrapper): BINARY_HAND_CLOSED = 0 BINARY_HAND_OPEN = 1 - def __init__(self, env, hand: Hand, binary: bool = True): + def __init__(self, env, hand: BaseHandControl, binary: bool = True): super().__init__(env) self.unwrapped: FR3Env self.observation_space: gym.spaces.Dict diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 3fc34093..c18859c7 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -5,6 +5,7 @@ import gymnasium as gym import numpy as np import rcsss +import rcsss.hand.tilburg_hand_control from gymnasium.envs.registration import EnvCreator from rcsss import sim from rcsss._core.sim import CameraType @@ -36,7 +37,6 @@ default_realsense, get_urdf_path, ) -from rcsss.hand.hand import Hand from rcsss.hand.tilburg_hand_control import TilburgHandControl logger = logging.getLogger(__name__) @@ -48,7 +48,7 @@ def fr3_hw_env( control_mode: ControlMode, robot_cfg: rcsss.hw.FR3Config, collision_guard: str | PathLike | None = None, - gripper_cfg: rcsss.hw.FHConfig | rcsss.hand.interface.THConfig | None = None, + gripper_cfg: rcsss.hw.FHConfig | rcsss.hand.tilburg_hand_control.THConfig | None = None, camera_set: BaseHardwareCameraSet | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, @@ -168,9 +168,9 @@ def fr3_sim_env( gripper = sim.FrankaHand(simulation, "0", gripper_cfg) env = GripperWrapper(env, gripper, binary=True) - elif isinstance(gripper_cfg, rcsss.hand.interface.THConfig): - hand = Hand(TilburgHandControl(gripper_cfg)) - env = HandWrapper(env, hand, binary=gripper_cfg.binary_action) + elif isinstance(gripper_cfg, rcsss.hand.tilburg_hand_control.THConfig): + hand = TilburgHandControl(gripper_cfg) + env = HandWrapper(env, hand, binary=True) if collision_guard: env = CollisionGuard.env_from_xml_paths( diff --git a/python/rcsss/envs/utils.py b/python/rcsss/envs/utils.py index 43a17074..cb086445 100644 --- a/python/rcsss/envs/utils.py +++ b/python/rcsss/envs/utils.py @@ -11,6 +11,7 @@ from rcsss.camera.interface import BaseCameraConfig from rcsss.camera.realsense import RealSenseCameraSet, RealSenseSetConfig from rcsss.camera.sim import SimCameraConfig, SimCameraSetConfig +from rcsss.hand.tilburg_hand_control import THConfig logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -23,7 +24,7 @@ def default_fr3_sim_robot_cfg(mjcf: str | Path = "fr3_empty_world") -> sim.FR3Co return cfg -def default_fr3_hw_robot_cfg(): +def default_fr3_hw_robot_cfg() -> rcsss.hw.FR3Config: robot_cfg = FR3Config() robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) robot_cfg.speed_factor = 0.1 @@ -31,7 +32,7 @@ def default_fr3_hw_robot_cfg(): return robot_cfg -def default_fr3_hw_gripper_cfg(): +def default_fr3_hw_gripper_cfg() -> rcsss.hw.FHConfig: gripper_cfg = rcsss.hw.FHConfig() gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 gripper_cfg.speed = 0.1 @@ -39,8 +40,11 @@ def default_fr3_hw_gripper_cfg(): return gripper_cfg -def default_tilburg_hw_hand_cfg(): - return rcsss.hand.interface.THConfig() +def default_tilburg_hw_hand_cfg(file: str | PathLike | None = None) -> THConfig: + hand_cfg = THConfig() + hand_cfg.grasp_percentage = 1.0 + hand_cfg.calibration_file = str(file) if isinstance(file, PathLike) else file + return hand_cfg def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | None: diff --git a/python/rcsss/hand/hand.py b/python/rcsss/hand/hand.py deleted file mode 100644 index ecd45851..00000000 --- a/python/rcsss/hand/hand.py +++ /dev/null @@ -1,68 +0,0 @@ -from typing import Optional - - -class HandControl: - """ - Base class for hand control. - This class provides an interface for hand control, but does not implement any functionality. - """ - - def __init__(self): - pass - - def grasp(self, value: Optional[float] = None): - self.value = value # to pass pylint - message = "This method should be overridden by subclasses." - raise NotImplementedError(message) - - def set_zero_pos(self): - message = "This method should be overridden by subclasses." - raise NotImplementedError(message) - - def disconnect(self): - message = "This method should be overridden by subclasses." - raise NotImplementedError(message) - - def get_pos_vector(self): - message = "This method should be overridden by subclasses." - raise NotImplementedError(message) - - def set_pos_vector(self, values: list[float]): - self.values = values # to pass pylint - message = "This method should be overridden by subclasses." - raise NotImplementedError(message) - - -class Hand: - """ - Hand Class - This class provides an interface for hand control. - It allows for grasping, resetting, and disconnecting from the hand. - """ - - def __init__(self, hand: HandControl): - self._hand = hand - - def grasp(self, value: Optional[float] = None): - self._hand.grasp(value) - - def reset(self): - self._hand.set_zero_pos() - - def get_state(self): - pass - - def disconnect(self): - self._hand.disconnect() - - def get_normalized_joints_poses(self): - self._hand.get_pos_vector() - - def set_normalized_joints_poses(self, values: list): - self._hand.set_pos_vector(values) - - def open(self): - self.reset() - - def __del__(self): - self._hand.disconnect() diff --git a/python/rcsss/hand/interface.py b/python/rcsss/hand/interface.py index 68fe7c71..a02b7645 100644 --- a/python/rcsss/hand/interface.py +++ b/python/rcsss/hand/interface.py @@ -1,11 +1,31 @@ -import os -from pathlib import Path +from typing import Protocol -from pydantic import BaseModel +import numpy as np -class THConfig(BaseModel): - """Config for the Tilburg hand""" +class BaseHandControl(Protocol): + """ + Hand Class + This class provides an interface for hand control. + """ - binary_action: bool = True - calibration_file_path: str = os.path.join(Path.home(), "tilburg_hand/calibration.json") + def grasp(self): + pass + + def open(self): + pass + + def reset(self): + pass + + def close(self): + pass + + def get_state(self) -> np.ndarray: + pass + + def get_normalized_joints_poses(self) -> np.ndarray: + pass + + def set_normalized_joints_poses(self, values: np.ndarray): + pass diff --git a/python/rcsss/hand/tilburg_hand_control.py b/python/rcsss/hand/tilburg_hand_control.py index 5d83310d..81073bc8 100644 --- a/python/rcsss/hand/tilburg_hand_control.py +++ b/python/rcsss/hand/tilburg_hand_control.py @@ -1,12 +1,12 @@ import copy import logging -import sys from time import sleep -from typing import Optional -from rcsss.hand.hand import HandControl -from rcsss.hand.interface import THConfig -from tilburg_hand import Finger, TilburgHandMotorInterface, Unit, Wrist +import numpy as np +from pydantic import BaseModel +from rcsss.envs.space_utils import Vec18Type +from rcsss.hand.interface import BaseHandControl +from tilburg_hand import Finger, TilburgHandMotorInterface, Unit # Setup logger logging.basicConfig(level=logging.INFO) @@ -14,128 +14,83 @@ logger.disabled = False -class TilburgHandControl(HandControl): +class THConfig(BaseModel): + """Config for the Tilburg hand""" + + calibration_file: str | None = None + grasp_percentage: float = 1.0 + control_unit: Unit = Unit.NORMALIZED + hand_orientation: str = "right" + + +class TilburgHandControl(BaseHandControl): """ Tilburg Hand Control Class This class provides an interface for controlling the Tilburg Hand. It allows for grasping, resetting, and disconnecting from the hand. """ - def __init__(self, tilburghand_cfg: Optional[THConfig] = None, verbose: bool = False): + MAX_GRASP_JOINTS_VALS = np.array( + [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0] + ) + + def __init__(self, cfg: THConfig, verbose: bool = False): """ Initializes the Tilburg Hand Control interface. """ - self.tilburg_hand_calibration_file_path = tilburghand_cfg.calibration_file_path if tilburghand_cfg else None + self._cfg = cfg self._motors = TilburgHandMotorInterface( - calibration_file=self.tilburg_hand_calibration_file_path, - verbose=verbose, + calibration_file=self._cfg.calibration_file, hand_orientation=self._cfg.hand_orientation, verbose=verbose ) - self.pos_value_unit = Unit.NORMALIZED - self._pos_normalized = [0.0] * self._motors.n_motors - if self._motors.connect() < 0: - logger.error("Failed to connect to the motors' board.") - sys.exit(1) + re = self._motors.connect() + assert re >= 0, "Failed to connect to the motors' board." logger.info("Connected to the motors' board.") - self.fingers_joints = Finger - self.fingers_joints_list = [ - Finger.INDEX_ABD, - Finger.MIDDLE_ABD, - Finger.THUMB_ABD, - Finger.RING_ABD, - Finger.INDEX_DIP, - Finger.MIDDLE_DIP, - Finger.RING_DIP, - Finger.THUMB_IP, - Finger.INDEX_PIP, - Finger.MIDDLE_PIP, - Finger.RING_PIP, - Finger.INDEX_MCP, - Finger.MIDDLE_MCP, - Finger.RING_MCP, - Finger.THUMB_MCP, - Finger.THUMB_CMC, - ] - self.pos_vector_names_list = [ - "THUMB_IP", - "THUMB_MCP", - "THUMB_ABD", - "THUMB_CMC", - "INDEX_DIP", - "INDEX_PIP", - "INDEX_MCP", - "INDEX_ABD", - "MIDDLE_DIP", - "MIDDLE_PIP", - "MIDDLE_MCP", - "MIDDLE_ABD", - "RING_DIP", - "RING_PIP", - "RING_MCP", - "RING_ABD", - "WRIST_PITCH", - "WRIST_YAW", - ] - - self.wrist_joints_list = [Wrist.PITCH, Wrist.YAW] - self.set_zero_pos() - logger.info("Setting all joints to zero position.") - - def grasp(self, value: Optional[float] = None): + @property + def config(self): """ - Performs a grasp with a specified intensity (0.0 to 1.0). + Returns the configuration of the Tilburg Hand Control. """ - template = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0] - if value is None: - value = 0.9 - self._pos_normalized = [val * value for val in template] + return copy.deepcopy(self._cfg) - self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) - logger.info(f"Grasp command sent with value: {value}") + @config.setter + def config(self, cfg: THConfig): + """ + Sets the configuration of the Tilburg Hand Control. + """ + self._cfg = cfg - def set_pos_vector(self, pos_vector: list): + def set_pos_vector(self, pos_vector: Vec18Type): """ Sets the position vector for the motors. """ - if len(pos_vector) != len(self._pos_normalized): - logger.error(f"Invalid position vector length: {len(pos_vector)}. Expected: {len(self._pos_normalized)}") - return - self._pos_normalized = pos_vector - - self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) + assert len(pos_vector) == len( + self._motors.n_motors + ), f"Invalid position vector length: {len(pos_vector)}. Expected: {len(self._motors.n_motors)}" + self._motors.set_pos_vector(copy.deepcopy(pos_vector), unit=self._cfg.control_unit) logger.info(f"Set pose vector: {pos_vector}") def set_zero_pos(self): """ Sets all finger joint positions to zero. """ - self._pos_normalized = [0.0] * self._motors.n_motors - self._motors.set_pos_vector(copy.deepcopy(self._pos_normalized), unit=self.pos_value_unit) + self._motors.goto_zero_position() logger.info("All joints reset to zero position.") - def sleep(self, seconds: float): - """ - Delays the program for a specified number of seconds. - """ - sleep(seconds) - def set_joint_pos(self, finger_joint: Finger, pos_value: float): """ Sets a single joint to a specific normalized position. """ - self._motors.set_pos_single(finger_joint, pos_value, unit=self.pos_value_unit) - self._pos_normalized[int(finger_joint)] = pos_value - logger.info(f"Set joint {finger_joint.name} to {pos_value:.2f}") + self._motors.set_pos_single(finger_joint, pos_value, unit=self._cfg.control_unit) def reset_joint_pos(self, finger_joint: Finger): """ Resets a specific joint to zero. """ - self._motors.set_pos_single(finger_joint, 0, unit=self.pos_value_unit) - self._pos_normalized[finger_joint] = 0 + self._motors.set_pos_single(finger_joint, 0, unit=self._cfg.control_unit) logger.info(f"Reset joint {finger_joint.name} to 0") def disconnect(self): @@ -145,14 +100,65 @@ def disconnect(self): self._motors.disconnect() logger.info("Disconnected from the motors' board") - def get_pos_vector(self): + def get_pos_vector(self) -> Vec18Type: """ Returns the current position vector of the motors. """ - return self._pos_normalized + return np.array(self._motors.get_encoder_vector(self._cfg.control_unit)) - def get_pos_single(self, finger_joint): + def get_pos_single(self, finger_joint: Finger) -> float: """ Returns the current position of a single joint. """ - return self._pos_normalized[finger_joint] + return self._motors.get_encoder_single(finger_joint, self._cfg.control_unit) + + def _grasp(self): + pos_normalized = self._cfg.grasp_percentage * self.MAX_GRASP_JOINTS_VALS + self._motors.set_pos_vector(pos_normalized, unit=self._cfg.control_unit) + logger.info(f"Grasp command sent with value: {self._cfg.grasp_percentage:.2f}") + + def auto_recovery(self): + if not np.array(self._motors.check_enabled_motors()).all(): + logger.warning("Some motors are not enabled. Attempting to enable them.") + self._motors.disconnect() + sleep(1) + re = self._motors.connect() + assert re >= 0, "Failed to reconnect to the motors' board." + + #### BaseHandControl Interface methods #### + + def grasp(self): + """ + Performs a grasp with a specified intensity (0.0 to 1.0). + """ + self._grasp() + + def open(self): + self.set_zero_pos() + + def reset(self): + """ + Resets the hand to its initial state. + """ + self.auto_recovery() + self.set_zero_pos() + logger.info("Hand reset to initial state.") + + def get_state(self) -> np.ndarray: + """ + Returns the current state of the hand. + """ + return self.get_pos_vector() + + def close(self): + """ + Closes the hand control interface. + """ + self.disconnect() + logger.info("Hand control interface closed.") + + def get_normalized_joints_poses(self) -> np.ndarray: + return self.get_pos_vector() + + def set_normalized_joints_poses(self, values: np.ndarray): + self.set_pos_vector(values) diff --git a/requirements_dev.txt b/requirements_dev.txt index 7851d1de..67752008 100644 --- a/requirements_dev.txt +++ b/requirements_dev.txt @@ -12,4 +12,3 @@ pybind11 mujoco==3.2.6 pin==2.7.0 wheel -tilburg-hand From c35304f07c623d01c2a4b650f3e3166c2917a11f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 19 May 2025 10:21:22 +0200 Subject: [PATCH 18/24] refactor(hand): rename to remove control in name --- python/rcsss/envs/factories.py | 8 ++++---- python/rcsss/envs/utils.py | 2 +- .../hand/{tilburg_hand_control.py => tilburg_hand.py} | 0 3 files changed, 5 insertions(+), 5 deletions(-) rename python/rcsss/hand/{tilburg_hand_control.py => tilburg_hand.py} (100%) diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index c18859c7..bee152a1 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -5,7 +5,7 @@ import gymnasium as gym import numpy as np import rcsss -import rcsss.hand.tilburg_hand_control +import rcsss.hand.tilburg_hand from gymnasium.envs.registration import EnvCreator from rcsss import sim from rcsss._core.sim import CameraType @@ -37,7 +37,7 @@ default_realsense, get_urdf_path, ) -from rcsss.hand.tilburg_hand_control import TilburgHandControl +from rcsss.hand.tilburg_hand import TilburgHandControl logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -48,7 +48,7 @@ def fr3_hw_env( control_mode: ControlMode, robot_cfg: rcsss.hw.FR3Config, collision_guard: str | PathLike | None = None, - gripper_cfg: rcsss.hw.FHConfig | rcsss.hand.tilburg_hand_control.THConfig | None = None, + gripper_cfg: rcsss.hw.FHConfig | rcsss.hand.tilburg_hand.THConfig | None = None, camera_set: BaseHardwareCameraSet | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, @@ -168,7 +168,7 @@ def fr3_sim_env( gripper = sim.FrankaHand(simulation, "0", gripper_cfg) env = GripperWrapper(env, gripper, binary=True) - elif isinstance(gripper_cfg, rcsss.hand.tilburg_hand_control.THConfig): + elif isinstance(gripper_cfg, rcsss.hand.tilburg_hand.THConfig): hand = TilburgHandControl(gripper_cfg) env = HandWrapper(env, hand, binary=True) diff --git a/python/rcsss/envs/utils.py b/python/rcsss/envs/utils.py index cb086445..f5702737 100644 --- a/python/rcsss/envs/utils.py +++ b/python/rcsss/envs/utils.py @@ -11,7 +11,7 @@ from rcsss.camera.interface import BaseCameraConfig from rcsss.camera.realsense import RealSenseCameraSet, RealSenseSetConfig from rcsss.camera.sim import SimCameraConfig, SimCameraSetConfig -from rcsss.hand.tilburg_hand_control import THConfig +from rcsss.hand.tilburg_hand import THConfig logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/python/rcsss/hand/tilburg_hand_control.py b/python/rcsss/hand/tilburg_hand.py similarity index 100% rename from python/rcsss/hand/tilburg_hand_control.py rename to python/rcsss/hand/tilburg_hand.py From a1518d2dff9661ebcc7999a83d4eccf6a8f9d709 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 19 May 2025 10:23:12 +0200 Subject: [PATCH 19/24] refactor(hand): previous renaming in code --- python/rcsss/envs/base.py | 4 ++-- python/rcsss/envs/factories.py | 4 ++-- python/rcsss/hand/interface.py | 2 +- python/rcsss/hand/tilburg_hand.py | 8 ++++---- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index e7c7a409..21fe0a1b 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -18,7 +18,7 @@ get_space, get_space_keys, ) -from rcsss.hand.interface import BaseHandControl +from rcsss.hand.interface import BaseHand _logger = logging.getLogger() @@ -613,7 +613,7 @@ class HandWrapper(ActObsInfoWrapper): BINARY_HAND_CLOSED = 0 BINARY_HAND_OPEN = 1 - def __init__(self, env, hand: BaseHandControl, binary: bool = True): + def __init__(self, env, hand: BaseHand, binary: bool = True): super().__init__(env) self.unwrapped: FR3Env self.observation_space: gym.spaces.Dict diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index bee152a1..afbed6d5 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -37,7 +37,7 @@ default_realsense, get_urdf_path, ) -from rcsss.hand.tilburg_hand import TilburgHandControl +from rcsss.hand.tilburg_hand import TilburgHand logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -169,7 +169,7 @@ def fr3_sim_env( env = GripperWrapper(env, gripper, binary=True) elif isinstance(gripper_cfg, rcsss.hand.tilburg_hand.THConfig): - hand = TilburgHandControl(gripper_cfg) + hand = TilburgHand(gripper_cfg) env = HandWrapper(env, hand, binary=True) if collision_guard: diff --git a/python/rcsss/hand/interface.py b/python/rcsss/hand/interface.py index a02b7645..2ce5b700 100644 --- a/python/rcsss/hand/interface.py +++ b/python/rcsss/hand/interface.py @@ -3,7 +3,7 @@ import numpy as np -class BaseHandControl(Protocol): +class BaseHand(Protocol): """ Hand Class This class provides an interface for hand control. diff --git a/python/rcsss/hand/tilburg_hand.py b/python/rcsss/hand/tilburg_hand.py index 81073bc8..29ca1e32 100644 --- a/python/rcsss/hand/tilburg_hand.py +++ b/python/rcsss/hand/tilburg_hand.py @@ -5,7 +5,7 @@ import numpy as np from pydantic import BaseModel from rcsss.envs.space_utils import Vec18Type -from rcsss.hand.interface import BaseHandControl +from rcsss.hand.interface import BaseHand from tilburg_hand import Finger, TilburgHandMotorInterface, Unit # Setup logger @@ -23,9 +23,9 @@ class THConfig(BaseModel): hand_orientation: str = "right" -class TilburgHandControl(BaseHandControl): +class TilburgHand(BaseHand): """ - Tilburg Hand Control Class + Tilburg Hand Class This class provides an interface for controlling the Tilburg Hand. It allows for grasping, resetting, and disconnecting from the hand. """ @@ -36,7 +36,7 @@ class TilburgHandControl(BaseHandControl): def __init__(self, cfg: THConfig, verbose: bool = False): """ - Initializes the Tilburg Hand Control interface. + Initializes the Tilburg Hand interface. """ self._cfg = cfg From 51fec0db0693837575ea74d9e34e68b128f98a44 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 19 May 2025 16:46:45 +0200 Subject: [PATCH 20/24] fix: imports for renamed rcs package --- .../examples/env_cartesian_control_tilburg.py | 18 +++++++++--------- python/rcs/envs/base.py | 2 +- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/python/examples/env_cartesian_control_tilburg.py b/python/examples/env_cartesian_control_tilburg.py index c2fcc5a9..00f7f361 100644 --- a/python/examples/env_cartesian_control_tilburg.py +++ b/python/examples/env_cartesian_control_tilburg.py @@ -1,10 +1,10 @@ import logging -from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager -from rcsss.control.utils import load_creds_fr3_desk -from rcsss.envs.base import ControlMode, RelativeTo, RobotInstance -from rcsss.envs.factories import fr3_hw_env, fr3_sim_env -from rcsss.envs.utils import ( +from rcs.control.fr3_desk import FCI, Desk, DummyResourceManager +from rcs.control.utils import load_creds_fr3_desk +from rcs.envs.base import ControlMode, RelativeTo, RobotInstance +from rcs.envs.factories import fr3_hw_env, fr3_sim_env +from rcs.envs.utils import ( default_fr3_hw_robot_cfg, default_fr3_sim_gripper_cfg, default_fr3_sim_robot_cfg, @@ -26,19 +26,19 @@ When you use a real FR3 you first need to unlock its joints using the following cli script: -python -m rcsss fr3 unlock +python -m rcs fr3 unlock or put it into guiding mode using: -python -m rcsss fr3 guiding-mode +python -m rcs fr3 guiding-mode When you are done you lock it again using: -python -m rcsss fr3 lock +python -m rcs fr3 lock or even shut it down using: -python -m rcsss fr3 shutdown +python -m rcs fr3 shutdown """ diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index c8051d39..85558a91 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -19,7 +19,7 @@ get_space, get_space_keys, ) -from rcsss.hand.interface import BaseHand +from rcs.hand.interface import BaseHand _logger = logging.getLogger(__name__) From f53303df61d6187e390623a3d72163d41bf82aea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 19 May 2025 17:04:32 +0200 Subject: [PATCH 21/24] fix: remaining qarts renamed --- python/examples/env_cartesian_control_tilburg.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/python/examples/env_cartesian_control_tilburg.py b/python/examples/env_cartesian_control_tilburg.py index 00f7f361..216a9e25 100644 --- a/python/examples/env_cartesian_control_tilburg.py +++ b/python/examples/env_cartesian_control_tilburg.py @@ -53,7 +53,7 @@ def main(): if ROBOT_INSTANCE == RobotInstance.HARDWARE: env_rel = fr3_hw_env( ip=ROBOT_IP, - control_mode=ControlMode.CARTESIAN_TQuart, + control_mode=ControlMode.CARTESIAN_TQuat, robot_cfg=default_fr3_hw_robot_cfg(), collision_guard="lab", gripper_cfg=default_tilburg_hw_hand_cfg(), @@ -62,7 +62,7 @@ def main(): ) else: env_rel = fr3_sim_env( - control_mode=ControlMode.CARTESIAN_TQuart, + control_mode=ControlMode.CARTESIAN_TQuat, robot_cfg=default_fr3_sim_robot_cfg(), collision_guard=False, gripper_cfg=default_fr3_sim_gripper_cfg(), @@ -80,7 +80,7 @@ def main(): for _ in range(10): for _ in range(10): # move 1cm in x direction (forward) and close gripper - act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "hand": close_action} + act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "hand": close_action} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") @@ -90,7 +90,7 @@ def main(): sleep(5) for _ in range(10): # move 1cm in negative x direction (backward) and open gripper - act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "hand": open_action} + act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "hand": open_action} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") From a0f42600a72df5b6ccf021214c36a8b50a41eead Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 19 May 2025 17:42:02 +0200 Subject: [PATCH 22/24] fix(examples): import of hand example --- .../examples/env_cartesian_control_tilburg.py | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/python/examples/env_cartesian_control_tilburg.py b/python/examples/env_cartesian_control_tilburg.py index 216a9e25..d6b350b9 100644 --- a/python/examples/env_cartesian_control_tilburg.py +++ b/python/examples/env_cartesian_control_tilburg.py @@ -1,9 +1,9 @@ import logging -from rcs.control.fr3_desk import FCI, Desk, DummyResourceManager -from rcs.control.utils import load_creds_fr3_desk -from rcs.envs.base import ControlMode, RelativeTo, RobotInstance -from rcs.envs.factories import fr3_hw_env, fr3_sim_env +from rcs.envs.creators import RCSFR3EnvCreator, RCSSimEnvCreator +from rcs._core.common import RobotPlatform +from rcs.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk +from rcs.envs.base import ControlMode, RelativeTo from rcs.envs.utils import ( default_fr3_hw_robot_cfg, default_fr3_sim_gripper_cfg, @@ -16,7 +16,7 @@ logger.setLevel(logging.INFO) ROBOT_IP = "192.168.101.1" -ROBOT_INSTANCE = RobotInstance.SIMULATION +ROBOT_INSTANCE = RobotPlatform.SIMULATION """ @@ -43,15 +43,15 @@ def main(): - resource_manger: FCI | DummyResourceManager - if ROBOT_INSTANCE == RobotInstance.HARDWARE: + resource_manger: FCI | ContextManager + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: user, pw = load_creds_fr3_desk() resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) else: - resource_manger = DummyResourceManager() + resource_manger = ContextManager() with resource_manger: - if ROBOT_INSTANCE == RobotInstance.HARDWARE: - env_rel = fr3_hw_env( + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + env_rel = RCSFR3EnvCreator( ip=ROBOT_IP, control_mode=ControlMode.CARTESIAN_TQuat, robot_cfg=default_fr3_hw_robot_cfg(), @@ -61,7 +61,7 @@ def main(): relative_to=RelativeTo.LAST_STEP, ) else: - env_rel = fr3_sim_env( + env_rel = RCSSimEnvCreator( control_mode=ControlMode.CARTESIAN_TQuat, robot_cfg=default_fr3_sim_robot_cfg(), collision_guard=False, From a7c02328c2afb0f079296b1dc2dbe489721f5b26 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 19 May 2025 18:16:57 +0200 Subject: [PATCH 23/24] style: python format --- python/examples/env_cartesian_control_tilburg.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/examples/env_cartesian_control_tilburg.py b/python/examples/env_cartesian_control_tilburg.py index d6b350b9..052920a7 100644 --- a/python/examples/env_cartesian_control_tilburg.py +++ b/python/examples/env_cartesian_control_tilburg.py @@ -1,9 +1,9 @@ import logging -from rcs.envs.creators import RCSFR3EnvCreator, RCSSimEnvCreator from rcs._core.common import RobotPlatform from rcs.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import RCSFR3EnvCreator, RCSSimEnvCreator from rcs.envs.utils import ( default_fr3_hw_robot_cfg, default_fr3_sim_gripper_cfg, @@ -51,7 +51,7 @@ def main(): resource_manger = ContextManager() with resource_manger: if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - env_rel = RCSFR3EnvCreator( + env_rel = RCSFR3EnvCreator()( ip=ROBOT_IP, control_mode=ControlMode.CARTESIAN_TQuat, robot_cfg=default_fr3_hw_robot_cfg(), @@ -61,7 +61,7 @@ def main(): relative_to=RelativeTo.LAST_STEP, ) else: - env_rel = RCSSimEnvCreator( + env_rel = RCSSimEnvCreator()( control_mode=ControlMode.CARTESIAN_TQuat, robot_cfg=default_fr3_sim_robot_cfg(), collision_guard=False, From 3b0e9f846ab01f3661a70066eea620ca37943dcd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 19 May 2025 19:08:42 +0200 Subject: [PATCH 24/24] fix(env creators): gripper config types --- python/rcs/envs/creators.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 53a2486e..84339ccb 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -92,7 +92,7 @@ def __call__( # type: ignore env: gym.Env = RobotEnv(robot, ControlMode.JOINTS if collision_guard is not None else control_mode) env = FR3HW(env) - if isinstance(gripper_cfg, rcs.sim.SimRobotConfig): + if isinstance(gripper_cfg, rcs.hw.FHConfig): gripper = rcs.hw.FrankaHand(ip, gripper_cfg) env = GripperWrapper(env, gripper, binary=True) elif isinstance(gripper_cfg, rcs.hand.tilburg_hand.THConfig): @@ -199,7 +199,7 @@ def __call__( # type: ignore camera_set = SimCameraSet(simulation, camera_set_cfg) env = CameraSetWrapper(env, camera_set, include_depth=True) - if isinstance(gripper_cfg, rcs.sim.SimRobotConfig): + if gripper_cfg is not None and isinstance(gripper_cfg, rcs.sim.SimGripperConfig): gripper = sim.SimGripper(simulation, "0", gripper_cfg) env = GripperWrapper(env, gripper, binary=True) env = GripperWrapperSim(env, gripper)