diff --git a/examples/simple_tilburg_test.py b/examples/simple_tilburg_test.py new file mode 100644 index 00000000..af0c45f1 --- /dev/null +++ b/examples/simple_tilburg_test.py @@ -0,0 +1,89 @@ +from time import sleep + +import numpy as np +from rcs._core.common import GraspType +from rcs.hand.tilburg_hand import THConfig, TilburgHand + + +def sim_real_index_flip(q: np.ndarray): + if len(q) == 18: + # This is the qpos for the real robot + q = q[:16] + assert len(q) == 16, "Expected 16 joint positions for the Tilburg hand" + q2 = q.copy() + for i in range(4): + for j in range(4): + q2[i * 4 + j] = q[i * 4 + (3 - j)] + return q2 + + +def sim_real_name_flip(names): + names2 = names.copy() + for i in range(4): + for j in range(4): + names2[i * 4 + j] = names[i * 4 + (3 - j)] + return names2 + + +def pad_qpos(q: np.ndarray): + if len(q) == 16: + # This is the qpos for the real robot + q = np.concatenate((q, np.zeros(2))) + assert len(q) == 18, "Expected 18 joint positions for the Tilburg hand" + return q + + +if __name__ == "__main__": + config = { + "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, 1, 1], + } + power_grasp_values = [ + 0.5, + 0.5, + 0.5, + 1.4, # THUMB_(IP, MCP, ABD, CMC) + 0.5, + 0.5, + 1.0, + 0.7, # INDEX_(DIP, PIP, MCP, ABD) + 0.5, + 0.5, + 1.0, + 0.3, + 0.5, + 0.5, + 1.0, + 0.0, + 0, + 0, + ] + rads = [] + min_joints_radians = [np.deg2rad(angle) for angle in config["motor_calib_min_range_deg"]] + max_joints_radians = [np.deg2rad(angle) for angle in config["motor_calib_max_range_deg"]] + + for i in range(len(config["motor_calib_min_range_deg"])): + joint_angle = ( + power_grasp_values[i] * (config["motor_calib_max_range_deg"][i] - config["motor_calib_min_range_deg"][i]) + + config["motor_calib_min_range_deg"][i] + ) + rads.append(np.deg2rad(joint_angle)) + + print(f"Motor {i}: {joint_angle:.2f} degrees") + print("power_grasp_radians=[", ", ".join(f"{rad:.2f}" for rad in rads), "]") + print("min_joints_radians=[", ", ".join(f"{rad:.2f}" for rad in min_joints_radians), "]") + print("max_joints_radians=[", ", ".join(f"{rad:.2f}" for rad in max_joints_radians), "]") + hand_cfg = THConfig( + calibration_file="/home/sbien/Documents/Development/RCS/robot-control-stack/python/rcs/hand/calibration_og.json", + grasp_percentage=1, + hand_orientation="right", + ) + hand = TilburgHand(cfg=hand_cfg, verbose=True) + hand.set_grasp_type(GraspType.POWER_GRASP) + hand.grasp() + sleep(2) + hand.open() + sleep(5) + hand.reset() + hand.close() + hand.disconnect() diff --git a/examples/tilburg_grasp.py b/examples/tilburg_grasp.py new file mode 100644 index 00000000..c3c0b540 --- /dev/null +++ b/examples/tilburg_grasp.py @@ -0,0 +1,42 @@ +import logging + +import numpy as np +from rcs._core.common import RobotPlatform +from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import SimEnvCreator +from rcs.envs.utils import default_sim_robot_cfg, default_sim_tilburg_hand_cfg + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +ROBOT_INSTANCE = RobotPlatform.SIMULATION + + +def main(): + mjcf = "/home/sbien/Documents/Development/RCS/models/scenes/fr3_tilburg_empty_world/scene.xml" + if ROBOT_INSTANCE == RobotPlatform.SIMULATION: + env_rel = SimEnvCreator()( + control_mode=ControlMode.JOINTS, + collision_guard=False, + robot_cfg=default_sim_robot_cfg(), + hand_cfg=default_sim_tilburg_hand_cfg(), + # cameras=default_mujoco_cameraset_cfg(), + mjcf=mjcf, + max_relative_movement=np.deg2rad(5), + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() + + for _ in range(10): + obs, info = env_rel.reset() + for _ in range(10): + # sample random relative action and execute it + act = env_rel.action_space.sample() + 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/include/rcs/Robot.h b/include/rcs/Robot.h index 793ca6e5..fb759fe5 100644 --- a/include/rcs/Robot.h +++ b/include/rcs/Robot.h @@ -91,6 +91,17 @@ struct GripperState { virtual ~GripperState(){}; }; +enum GraspType { POWER_GRASP = 0, + PRECISION_GRASP, + LATERAL_GRASP, + TRIPOD_GRASP }; +struct HandConfig { + virtual ~HandConfig(){}; +}; +struct HandState { + virtual ~HandState(){}; +}; + class Robot { public: virtual ~Robot(){}; @@ -158,6 +169,36 @@ class Gripper { virtual void reset() = 0; }; +class Hand { + public: + virtual ~Hand(){}; + // TODO: Add low-level control interface for the hand with internal state updates + // Also add an implementation specific set_parameters function that takes + // a deduced config type + // bool set_parameters(const GConfig& cfg); + + virtual HandConfig* get_parameters() = 0; + virtual HandState* get_state() = 0; + + // set width of the hand, 0 is closed, 1 is open + virtual void set_normalized_joint_poses(const VectorXd& q) = 0; + virtual VectorXd get_normalized_joint_poses() = 0; + + virtual bool is_grasped() = 0; + + // close hand with force, return true if the object is grasped successfully + virtual void grasp() = 0; + + // open hand + virtual void open() = 0; + + // close hand without applying force + virtual void shut() = 0; + + // puts the hand to max position + virtual void reset() = 0; +}; + } // namespace common } // namespace rcs #endif // RCS_ROBOT_H diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index 3238eb55..0d9d01d7 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -13,14 +13,21 @@ __all__ = [ "BaseCameraConfig", "FR3", "FrankaHandTCPOffset", + "GraspType", "Gripper", "GripperConfig", "GripperState", "HARDWARE", + "Hand", + "HandConfig", + "HandState", "IK", "IdentityRotMatrix", "IdentityRotQuatVec", "IdentityTranslation", + "LATERAL_GRASP", + "POWER_GRASP", + "PRECISION_GRASP", "Pin", "Pose", "RL", @@ -33,6 +40,7 @@ __all__ = [ "RobotType", "SIMULATION", "SO101", + "TRIPOD_GRASP", "UR5e", "robots_meta_config", ] @@ -46,6 +54,41 @@ class BaseCameraConfig: resolution_width: int def __init__(self, identifier: str, frame_rate: int, resolution_width: int, resolution_height: int) -> None: ... +class GraspType: + """ + Members: + + POWER_GRASP + + PRECISION_GRASP + + LATERAL_GRASP + + TRIPOD_GRASP + """ + + LATERAL_GRASP: typing.ClassVar[GraspType] # value = + POWER_GRASP: typing.ClassVar[GraspType] # value = + PRECISION_GRASP: typing.ClassVar[GraspType] # value = + TRIPOD_GRASP: typing.ClassVar[GraspType] # value = + __members__: typing.ClassVar[ + dict[str, GraspType] + ] # value = {'POWER_GRASP': , 'PRECISION_GRASP': , 'LATERAL_GRASP': , 'TRIPOD_GRASP': } + def __eq__(self, other: typing.Any) -> bool: ... + def __getstate__(self) -> int: ... + def __hash__(self) -> int: ... + def __index__(self) -> int: ... + def __init__(self, value: int) -> None: ... + def __int__(self) -> int: ... + def __ne__(self, other: typing.Any) -> bool: ... + def __repr__(self) -> str: ... + def __setstate__(self, state: int) -> None: ... + def __str__(self) -> str: ... + @property + def name(self) -> str: ... + @property + def value(self) -> int: ... + class Gripper: def get_normalized_width(self) -> float: ... def get_parameters(self) -> GripperConfig: ... @@ -63,6 +106,24 @@ class GripperConfig: class GripperState: pass +class Hand: + def __init__(self) -> None: ... + def get_normalized_joint_poses(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... + def get_parameters(self) -> HandConfig: ... + def get_state(self) -> HandState: ... + def grasp(self) -> None: ... + def is_grasped(self) -> bool: ... + def open(self) -> None: ... + def reset(self) -> None: ... + def set_normalized_joint_poses(self, q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]) -> None: ... + def shut(self) -> None: ... + +class HandConfig: + def __init__(self) -> None: ... + +class HandState: + def __init__(self) -> None: ... + class IK: def forward(self, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ... def ik( @@ -252,6 +313,10 @@ def robots_meta_config(robot_type: RobotType) -> RobotMetaConfig: ... FR3: RobotType # value = HARDWARE: RobotPlatform # value = +LATERAL_GRASP: GraspType # value = +POWER_GRASP: GraspType # value = +PRECISION_GRASP: GraspType # value = SIMULATION: RobotPlatform # value = SO101: RobotType # value = +TRIPOD_GRASP: GraspType # value = UR5e: RobotType # value = diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index 6c6e34fe..1a9eb28b 100644 --- a/python/rcs/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -22,6 +22,9 @@ __all__ = [ "SimRobot", "SimRobotConfig", "SimRobotState", + "SimTilburgHand", + "SimTilburgHandConfig", + "SimTilburgHandState", "default_free", "fixed", "free", @@ -177,6 +180,36 @@ class SimRobotState(rcs._core.common.RobotState): @property def target_angles(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... +class SimTilburgHand(rcs._core.common.Hand): + def __init__(self, sim: Sim, cfg: SimTilburgHandConfig) -> None: ... + def get_parameters(self) -> SimTilburgHandConfig: ... + def get_state(self) -> SimTilburgHandState: ... + def set_parameters(self, cfg: SimTilburgHandConfig) -> bool: ... + +class SimTilburgHandConfig(rcs._core.common.HandConfig, typing.Generic[M]): + actuators: list[str] + collision_geoms: list[str] + collision_geoms_fingers: list[str] + grasp_type: rcs._core.common.GraspType + ignored_collision_geoms: list[str] + joints: list[str] + max_joint_position: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]] + min_joint_position: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]] + seconds_between_callbacks: float + def __init__(self) -> None: ... + def add_id(self, id: str) -> None: ... + +class SimTilburgHandState(rcs._core.common.HandState): + def __init__(self) -> None: ... + @property + def collision(self) -> bool: ... + @property + def is_moving(self) -> bool: ... + @property + def last_commanded_qpos(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... + @property + def last_qpos(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... + default_free: CameraType # value = fixed: CameraType # value = free: CameraType # value = diff --git a/python/rcs/camera/hw.py b/python/rcs/camera/hw.py index a04e5289..460f26f7 100644 --- a/python/rcs/camera/hw.py +++ b/python/rcs/camera/hw.py @@ -83,7 +83,7 @@ def calibrate( return True def get_extrinsics(self) -> np.ndarray[tuple[typing.Literal[4], typing.Literal[4]], np.dtype[np.float64]] | None: - return np.eye(4) + return np.eye(4) # type: ignore[return-value] class HardwareCameraSet(BaseCameraSet): diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 4eee1be9..4d910036 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -7,6 +7,7 @@ import gymnasium as gym import numpy as np +from rcs._core.common import Hand from rcs.camera.interface import BaseCameraSet from rcs.envs.space_utils import ( ActObsInfoWrapper, @@ -18,7 +19,6 @@ get_space, get_space_keys, ) -from rcs.hand.interface import BaseHand from rcs import common @@ -749,7 +749,7 @@ class HandWrapper(ActObsInfoWrapper): BINARY_HAND_CLOSED = 0 BINARY_HAND_OPEN = 1 - def __init__(self, env, hand: BaseHand, binary: bool = True): + def __init__(self, env, hand: Hand, binary: bool = True): super().__init__(env) self.unwrapped: RobotEnv self.observation_space: gym.spaces.Dict @@ -778,7 +778,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_joints_poses() + observation[self.hand_key] = self._hand.get_normalized_joint_poses() info = {} return observation, info @@ -798,7 +798,7 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: else: self._hand.open() else: - self._hand.set_normalized_joints_poses(hand_action) + self._hand.set_normalized_joint_poses(hand_action) self._last_hand_cmd = hand_action del action[self.hand_key] return action diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index cde695ae..59ebab95 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -13,6 +13,7 @@ CameraSetWrapper, ControlMode, GripperWrapper, + HandWrapper, RelativeActionSpace, RelativeTo, RobotEnv, @@ -20,6 +21,7 @@ from rcs.envs.sim import ( CollisionGuard, GripperWrapperSim, + HandWrapperSim, PickCubeSuccessWrapper, RandomCubePos, RobotSimWrapper, @@ -45,6 +47,7 @@ def __call__( # type: ignore robot_cfg: rcs.sim.SimRobotConfig, collision_guard: bool = False, gripper_cfg: rcs.sim.SimGripperConfig | None = None, + hand_cfg: rcs.sim.SimTilburgHandConfig | None = None, cameras: dict[str, SimCameraConfig] | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, @@ -60,6 +63,9 @@ def __call__( # type: ignore robot_cfg (rcs.sim.SimRobotConfig): Configuration for the FR3 robot. collision_guard (bool): Whether to use collision guarding. If True, the same mjcf scene is used for collision guarding. gripper_cfg (rcs.sim.SimGripperConfig | None): Configuration for the gripper. If None, no gripper is used. + Cannot be used together with hand_cfg. + hand_cfg (rcs.sim.SimHandConfig | None): Configuration for the hand. If None, no hand is used. + Cannot be used together with gripper_cfg. camera_set_cfg (SimCameraSetConfig | None): Configuration for the camera set. If None, no cameras are used. max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts translational movement in meters. If tuple, it restricts both translational (in meters) and rotational @@ -97,6 +103,15 @@ def __call__( # type: ignore ) env = CameraSetWrapper(env, camera_set, include_depth=True) + assert not ( + hand_cfg is not None and gripper_cfg is not None + ), "Hand and gripper configurations cannot be used together." + + if hand_cfg is not None and isinstance(hand_cfg, rcs.sim.SimTilburgHandConfig): + hand = sim.SimTilburgHand(simulation, hand_cfg) + env = HandWrapper(env, hand, binary=True) + env = HandWrapperSim(env, hand) + if gripper_cfg is not None and isinstance(gripper_cfg, rcs.sim.SimGripperConfig): gripper = sim.SimGripper(simulation, gripper_cfg) env = GripperWrapper(env, gripper, binary=True) @@ -128,20 +143,39 @@ def __call__( # type: ignore control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, delta_actions: bool = True, cameras: dict[str, SimCameraConfig] | None = None, + hand_cfg: rcs.sim.SimTilburgHandConfig | None = None, + gripper_cfg: rcs.sim.SimGripperConfig | None = None, ) -> gym.Env: - + mode = "gripper" + if gripper_cfg is None and hand_cfg is None: + _gripper_cfg = default_sim_gripper_cfg() + _hand_cfg = None + logger.info("Using default gripper configuration.") + elif hand_cfg is not None: + _gripper_cfg = None + _hand_cfg = hand_cfg + mode = "hand" + logger.info("Using hand configuration.") + else: + # Either both cfgs are set, or only gripper_cfg is set + _gripper_cfg = gripper_cfg + _hand_cfg = None + logger.info("Using gripper configuration.") env_rel = SimEnvCreator()( control_mode=control_mode, robot_cfg=default_sim_robot_cfg(mjcf), collision_guard=False, - gripper_cfg=default_sim_gripper_cfg(), + gripper_cfg=_gripper_cfg, + hand_cfg=_hand_cfg, cameras=cameras, max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, relative_to=RelativeTo.LAST_STEP, mjcf=mjcf, sim_wrapper=RandomCubePos, ) - env_rel = PickCubeSuccessWrapper(env_rel) + if mode == "gripper": + env_rel = PickCubeSuccessWrapper(env_rel) + if render_mode == "human": env_rel.get_wrapper_attr("sim").open_gui() diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 283b246b..125f66cd 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -114,6 +114,26 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl return observation, info +class HandWrapperSim(ActObsInfoWrapper): + def __init__(self, env, hand: sim.SimTilburgHand): + super().__init__(env) + self._hand = hand + + def action(self, action: dict[str, Any]) -> dict[str, Any]: + if len(action["hand"]) == 18: + action["hand"] = action["hand"][:16] + assert len(action["hand"]) == 16 or len(action["hand"]) == 1, "Hand action must be of length 16 or 1" + return action + + def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: + state = self._hand.get_state() + if "collision" not in info or not info["collision"]: + info["collision"] = state.collision + info["hand_position"] = self._hand.get_normalized_joint_poses() + # info["is_grasped"] = self._hand.get_normalized_joint_poses() > 0.01 and self._hand.get_normalized_joint_poses() < 0.99 + return observation, info + + class CollisionGuard(gym.Wrapper[dict[str, Any], dict[str, Any], dict[str, Any], dict[str, Any]]): """ - Gripper Wrapper has to be added before this as it removes the gripper action diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index eea965c9..8da70f36 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -39,6 +39,10 @@ def default_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: return cfg +def default_sim_tilburg_hand_cfg() -> sim.SimTilburgHandConfig: + return sim.SimTilburgHandConfig() + + def default_digit(name2id: dict[str, str] | None, stream_name: str = "QVGA") -> DigitCam | None: if name2id is None: return None @@ -94,8 +98,8 @@ def get_tcp_offset(mjcf: str | Path) -> rcs.common.Pose: tcp_offset_translation = np.array(model.numeric("tcp_offset_translation").data) tcp_offset_rotation_matrix = np.array(model.numeric("tcp_offset_rotation_matrix").data) return rcs.common.Pose( - translation=tcp_offset_translation, rotation=tcp_offset_rotation_matrix.reshape((3, 3)) - ) # type: ignore + translation=tcp_offset_translation, rotation=tcp_offset_rotation_matrix.reshape((3, 3)) # type: ignore + ) except KeyError: msg = "No tcp offset found in the model. Using the default tcp offset." logging.info(msg) diff --git a/python/rcs/hand/interface.py b/python/rcs/hand/interface.py deleted file mode 100644 index 2ce5b700..00000000 --- a/python/rcs/hand/interface.py +++ /dev/null @@ -1,31 +0,0 @@ -from typing import Protocol - -import numpy as np - - -class BaseHand(Protocol): - """ - Hand Class - This class provides an interface for hand control. - """ - - 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/rcs/hand/tilburg_hand.py b/python/rcs/hand/tilburg_hand.py index 7c0a9b6b..d48001fd 100644 --- a/python/rcs/hand/tilburg_hand.py +++ b/python/rcs/hand/tilburg_hand.py @@ -4,8 +4,8 @@ from time import sleep import numpy as np +from rcs._core import common from rcs.envs.space_utils import Vec18Type -from rcs.hand.interface import BaseHand from tilburg_hand import Finger, TilburgHandMotorInterface, Unit # Setup logger @@ -15,16 +15,29 @@ @dataclass(kw_only=True) -class THConfig: +class THConfig(common.HandConfig): """Config for the Tilburg hand""" calibration_file: str | None = None grasp_percentage: float = 1.0 control_unit: Unit = Unit.NORMALIZED hand_orientation: str = "right" + grasp_type: common.GraspType = common.GraspType.POWER_GRASP + def __post_init__(self): + # 👇 satisfy pybind11 by actually calling the C++ constructor + super().__init__() -class TilburgHand(BaseHand): + +@dataclass +class TilburgHandState(common.HandState): + joint_positions: Vec18Type + + def __post_init__(self): + super().__init__() + + +class TilburgHand(common.Hand): """ Tilburg Hand Class This class provides an interface for controlling the Tilburg Hand. @@ -35,10 +48,58 @@ class TilburgHand(BaseHand): [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] ) + # TODO: Control mode for position control and pos+effort control + + POWER_GRASP_VALUES = np.array( + [ + 0.5, + 0.5, + 0.5, + 1.4, # THUMB_(IP, MCP, ABD, CMC) + 0.5, + 0.5, + 1.0, + 0.7, # INDEX_(DIP, PIP, MCP, ABD) + 0.5, + 0.5, + 1.0, + 0.3, + 0.5, + 0.5, + 1.0, + 0.0, + 0.0, + 0.0, + ] + ) + OPEN_VALUES = np.array( + [ + 0.0, + 0.0, + 0.5, + 1.4, # THUMB_(IP, MCP, ABD, CMC) + 0.2, + 0.2, + 0.2, + 0.7, # INDEX_(DIP, PIP, MCP, ABD) + 0.2, + 0.2, + 0.2, + 0.3, + 0.2, + 0.2, + 0.2, + 0.0, + 0.0, + 0.0, + ] + ) + def __init__(self, cfg: THConfig, verbose: bool = False): """ Initializes the Tilburg Hand interface. """ + super().__init__() self._cfg = cfg self._motors = TilburgHandMotorInterface( @@ -115,9 +176,13 @@ def get_pos_single(self, finger_joint: Finger) -> float: 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 + if self._cfg.grasp_type == common.GraspType.POWER_GRASP: + pos_normalized = self.POWER_GRASP_VALUES * self._cfg.grasp_percentage + else: + logger.warning(f"Grasp type {self._cfg.grasp_type} is not implemented. Defaulting to power grasp.") + pos_normalized = self.POWER_GRASP_VALUES * self._cfg.grasp_percentage 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}") + logger.info(f"Performing {self._cfg.grasp_type} grasp with intensity: {self._cfg.grasp_percentage:.2f}") def auto_recovery(self): if not np.array(self._motors.check_enabled_motors()).all(): @@ -127,6 +192,30 @@ def auto_recovery(self): re = self._motors.connect() assert re >= 0, "Failed to reconnect to the motors' board." + def set_grasp_type(self, grasp_type: common.GraspType): + """ + Sets the grasp type for the hand. + """ + if not isinstance(grasp_type, common.GraspType): + error_msg = f"Invalid grasp type: {grasp_type}. Must be an instance of common.GraspType." + raise ValueError(error_msg) + if grasp_type == common.GraspType.POWER_GRASP: + self._cfg.grasp_type = common.GraspType.POWER_GRASP + elif grasp_type == common.GraspType.PRECISION_GRASP: + logger.warning("Precision grasp is not implemented yet. Defaulting to power grasp.") + self._cfg.grasp_type = common.GraspType.POWER_GRASP + elif grasp_type == common.GraspType.LATERAL_GRASP: + logger.warning("Lateral grasp is not implemented yet. Defaulting to power grasp.") + self._cfg.grasp_type = common.GraspType.POWER_GRASP + elif grasp_type == common.GraspType.TRIPOD_GRASP: + logger.warning("Tripod grasp is not implemented yet. Defaulting to power grasp.") + self._cfg.grasp_type = common.GraspType.POWER_GRASP + else: + error_msg = f"Unknown grasp type: {grasp_type}." + raise ValueError(error_msg) + + logger.info(f"Grasp type set to: {self._cfg.grasp_type}") + #### BaseHandControl Interface methods #### def grasp(self): @@ -136,7 +225,7 @@ def grasp(self): self._grasp() def open(self): - self.set_zero_pos() + self._motors.set_pos_vector(self.OPEN_VALUES, unit=self._cfg.control_unit) def reset(self): """ @@ -146,11 +235,11 @@ def reset(self): self.set_zero_pos() logger.info("Hand reset to initial state.") - def get_state(self) -> np.ndarray: + def get_state(self) -> TilburgHandState: """ Returns the current state of the hand. """ - return self.get_pos_vector() + return TilburgHandState(joint_positions=self.get_pos_vector()) def close(self): """ @@ -159,8 +248,8 @@ def close(self): self.disconnect() logger.info("Hand control interface closed.") - def get_normalized_joints_poses(self) -> np.ndarray: + def get_normalized_joint_poses(self) -> np.ndarray: return self.get_pos_vector() - def set_normalized_joints_poses(self, values: np.ndarray): + def set_normalized_joint_poses(self, values: np.ndarray): self.set_pos_vector(values) diff --git a/python/rcs/sim/__init__.py b/python/rcs/sim/__init__.py index c9476131..036f5bb5 100644 --- a/python/rcs/sim/__init__.py +++ b/python/rcs/sim/__init__.py @@ -6,6 +6,9 @@ SimRobot, SimRobotConfig, SimRobotState, + SimTilburgHand, + SimTilburgHandConfig, + SimTilburgHandState, ) from rcs.sim.sim import Sim, gui_loop @@ -17,6 +20,9 @@ "SimGripper", "SimGripperConfig", "SimGripperState", + "SimTilburgHand", + "SimTilburgHandConfig", + "SimTilburgHandState", "gui_loop", "SimCameraConfig", ] diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 2c55ed72..c335ef0e 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -117,6 +118,52 @@ class PyGripper : public rcs::common::Gripper { } }; +/** + * @brief Hand trampoline class for python bindings + */ +class PyHand : public rcs::common::Hand { + public: + using rcs::common::Hand::Hand; // Inherit constructors + + rcs::common::HandConfig *get_parameters() override { + PYBIND11_OVERRIDE_PURE(rcs::common::HandConfig *, rcs::common::Hand, + get_parameters, ); + } + + rcs::common::HandState *get_state() override { + PYBIND11_OVERRIDE_PURE(rcs::common::HandState *, rcs::common::Hand, + get_state, ); + } + + void set_normalized_joint_poses(const rcs::common::VectorXd &q) override { + PYBIND11_OVERRIDE_PURE(void, rcs::common::Hand, set_normalized_joint_poses, + q); + } + + rcs::common::VectorXd get_normalized_joint_poses() override { + PYBIND11_OVERRIDE_PURE(rcs::common::VectorXd, rcs::common::Hand, get_normalized_joint_poses, ); + } + + bool is_grasped() override { + PYBIND11_OVERRIDE_PURE(bool, rcs::common::Hand, is_grasped, ); + } + + void grasp() override { + PYBIND11_OVERRIDE_PURE(void, rcs::common::Hand, grasp, ); + } + + void open() override { + PYBIND11_OVERRIDE_PURE(void, rcs::common::Hand, open, ); + } + + void shut() override { + PYBIND11_OVERRIDE_PURE(void, rcs::common::Hand, shut, ); + } + void reset() override { + PYBIND11_OVERRIDE_PURE(void, rcs::common::Hand, reset, ); + } +}; + PYBIND11_MODULE(_core, m) { m.doc() = R"pbdoc( Robot Control Stack Python Bindings @@ -269,6 +316,16 @@ PYBIND11_MODULE(_core, m) { py::class_(common, "RobotState"); py::class_(common, "GripperConfig"); py::class_(common, "GripperState"); + py::enum_(common, "GraspType") + .value("POWER_GRASP", rcs::common::GraspType::POWER_GRASP) + .value("PRECISION_GRASP", rcs::common::GraspType::PRECISION_GRASP) + .value("LATERAL_GRASP", rcs::common::GraspType::LATERAL_GRASP) + .value("TRIPOD_GRASP", rcs::common::GraspType::TRIPOD_GRASP) + .export_values(); + py::class_(common, "HandConfig") + .def(py::init<>()); + py::class_(common, "HandState") + .def(py::init<>()); // holder type should be smart pointer as we deal with smart pointer // instances of this class @@ -314,6 +371,24 @@ PYBIND11_MODULE(_core, m) { .def("reset", &rcs::common::Gripper::reset, py::call_guard()); + py::class_>(common, "Hand") + .def(py::init<>()) + .def("get_parameters", &rcs::common::Hand::get_parameters) + .def("get_state", &rcs::common::Hand::get_state) + .def("set_normalized_joint_poses", &rcs::common::Hand::set_normalized_joint_poses, + py::arg("q")) + .def("get_normalized_joint_poses", &rcs::common::Hand::get_normalized_joint_poses) + .def("grasp", &rcs::common::Hand::grasp, + py::call_guard()) + .def("is_grasped", &rcs::common::Hand::is_grasped) + .def("open", &rcs::common::Hand::open, + py::call_guard()) + .def("shut", &rcs::common::Hand::shut, + py::call_guard()) + .def("reset", &rcs::common::Hand::reset, + py::call_guard()); + // SIM MODULE auto sim = m.def_submodule("sim", "sim module"); py::class_( @@ -417,6 +492,48 @@ PYBIND11_MODULE(_core, m) { .def("set_joints_hard", &rcs::sim::SimRobot::set_joints_hard, py::arg("q")) .def("get_state", &rcs::sim::SimRobot::get_state); + + // SimTilburgHandState + py::class_( + sim, "SimTilburgHandState") + .def(py::init<>()) + .def_readonly("last_commanded_qpos", + &rcs::sim::SimTilburgHandState::last_commanded_qpos) + .def_readonly("is_moving", &rcs::sim::SimTilburgHandState::is_moving) + .def_readonly("last_qpos", &rcs::sim::SimTilburgHandState::last_qpos) + .def_readonly("collision", &rcs::sim::SimTilburgHandState::collision); + // SimTilburgHandConfig + py::class_( + sim, "SimTilburgHandConfig") + .def(py::init<>()) + .def_readwrite("max_joint_position", + &rcs::sim::SimTilburgHandConfig::max_joint_position) + .def_readwrite("min_joint_position", + &rcs::sim::SimTilburgHandConfig::min_joint_position) + .def_readwrite("ignored_collision_geoms", + &rcs::sim::SimTilburgHandConfig::ignored_collision_geoms) + .def_readwrite("collision_geoms", + &rcs::sim::SimTilburgHandConfig::collision_geoms) + .def_readwrite("collision_geoms_fingers", + &rcs::sim::SimTilburgHandConfig::collision_geoms_fingers) + .def_readwrite("joints", &rcs::sim::SimTilburgHandConfig::joints) + .def_readwrite("actuators", &rcs::sim::SimTilburgHandConfig::actuators) + .def_readwrite("grasp_type", + &rcs::sim::SimTilburgHandConfig::grasp_type) + .def_readwrite("seconds_between_callbacks", + &rcs::sim::SimTilburgHandConfig::seconds_between_callbacks) + .def("add_id", &rcs::sim::SimTilburgHandConfig::add_id, py::arg("id")); + // SimTilburgHand + py::class_>(sim, "SimTilburgHand") + .def(py::init, + const rcs::sim::SimTilburgHandConfig &>(), + py::arg("sim"), py::arg("cfg")) + .def("get_parameters", &rcs::sim::SimTilburgHand::get_parameters) + .def("get_state", &rcs::sim::SimTilburgHand::get_state) + .def("set_parameters", &rcs::sim::SimTilburgHand::set_parameters, + py::arg("cfg")); + py::enum_(sim, "CameraType") .value("free", rcs::sim::CameraType::free) .value("tracking", rcs::sim::CameraType::tracking) diff --git a/src/sim/CMakeLists.txt b/src/sim/CMakeLists.txt index 0768d92f..5c57a61c 100644 --- a/src/sim/CMakeLists.txt +++ b/src/sim/CMakeLists.txt @@ -1,6 +1,6 @@ add_library(sim) target_sources(sim - PRIVATE sim.cpp SimRobot.cpp renderer.cpp camera.cpp SimGripper.cpp gui_server.cpp gui_client.cpp + PRIVATE sim.cpp SimRobot.cpp renderer.cpp camera.cpp SimGripper.cpp SimTilburgHand.cpp gui_server.cpp gui_client.cpp ) target_link_libraries(sim PUBLIC rcs MuJoCo::MuJoCo) diff --git a/src/sim/SimTilburgHand.cpp b/src/sim/SimTilburgHand.cpp new file mode 100644 index 00000000..7168364c --- /dev/null +++ b/src/sim/SimTilburgHand.cpp @@ -0,0 +1,208 @@ + +#include "SimTilburgHand.h" + +#include +#include +#include +#include +#include + +namespace rcs { +namespace sim { + +SimTilburgHand::SimTilburgHand(std::shared_ptr sim, + const SimTilburgHandConfig &cfg) + : sim{sim}, cfg{cfg} { + this->state = SimTilburgHandState(); + + // Initialize actuator and joint IDs + for (int i = 0; i < this->n_joints; ++i) { + // actuators + this->actuator_ids[i] = mj_name2id(this->sim->m, mjOBJ_ACTUATOR, + this->cfg.actuators[i].c_str()); + if (this->actuator_ids[i] == -1) { + throw std::runtime_error( + std::string("No actuator named " + this->cfg.actuators[i])); + } + // joints + this->joint_ids[i] = this->sim->m->jnt_qposadr[mj_name2id( + this->sim->m, mjOBJ_JOINT, this->cfg.joints[i].c_str())]; + if (this->joint_ids[i] == -1) { + throw std::runtime_error( + std::string("No joint named " + this->cfg.joints[i])); + } + } + + // Collision geoms + // this->add_collision_geoms(this->cfg.collision_geoms, this->cgeom, false); + // this->add_collision_geoms(this->cfg.collision_geoms_fingers, this->cfgeom, + // false); + + this->sim->register_all_cb( + std::bind(&SimTilburgHand::convergence_callback, this), + this->cfg.seconds_between_callbacks); + this->sim->register_any_cb( + std::bind(&SimTilburgHand::collision_callback, this), + this->cfg.seconds_between_callbacks); + this->m_reset(); +} + +SimTilburgHand::~SimTilburgHand() {} + +// void SimTilburgHand::add_collision_geoms( +// const std::vector &cgeoms_str, std::set &cgeoms_set, +// bool clear_before) { +// if (clear_before) { +// cgeoms_set.clear(); +// } +// for (size_t i = 0; i < std::size(cgeoms_str); ++i) { +// std::string name = cgeoms_str[i]; + +// int coll_id = mj_name2id(this->sim->m, mjOBJ_GEOM, name.c_str()); +// if (coll_id == -1) { +// throw std::runtime_error(std::string("No geom named " + name)); +// } +// cgeoms_set.insert(coll_id); +// } +// } + +bool SimTilburgHand::set_parameters(const SimTilburgHandConfig &cfg) { + auto current_grasp_type = this->cfg.grasp_type; + this->cfg = cfg; + if (!this->set_grasp_type(current_grasp_type)) { + std::cerr << "Provided grasp type is not supported." << std::endl; + this->cfg.grasp_type = current_grasp_type; + } + // this->add_collision_geoms(cfg.ignored_collision_geoms, + // this->ignored_collision_geoms, true); + return true; +} + +bool SimTilburgHand::set_grasp_type(common::GraspType grasp_type){ + switch (grasp_type) { + case common::GraspType::POWER_GRASP: + this->cfg.grasp_type = common::GraspType::POWER_GRASP; + break; + default: + return false; + } + return true; +} + +SimTilburgHandConfig *SimTilburgHand::get_parameters() { + // copy config to heap + SimTilburgHandConfig *cfg = new SimTilburgHandConfig(); + *cfg = this->cfg; + return cfg; +} + +SimTilburgHandState *SimTilburgHand::get_state() { + SimTilburgHandState *state = new SimTilburgHandState(); + *state = this->state; + return state; +} +void SimTilburgHand::set_normalized_joint_poses( + const rcs::common::VectorXd &q) { + if (q.size() != this->n_joints) { + throw std::invalid_argument("Invalid joint pose vector size, expected 16."); + } + this->state.last_commanded_qpos = q; + for (int i = 0; i < this->n_joints; ++i) { + this->sim->d->ctrl[this->actuator_ids[i]] = + (mjtNum)q[i] * (this->cfg.max_joint_position[i] - + this->cfg.min_joint_position[i]) + + this->cfg.min_joint_position[i]; + } + + // we ignore force for now + // this->sim->d->actuator_force[this->gripper_id] = 0; +} +rcs::common::VectorXd SimTilburgHand::get_normalized_joint_poses() { + // TODO: maybe we should use the mujoco sensors? Not sure what the difference + // is between reading out from qpos and reading from the sensors. + rcs::common::VectorXd q(this->n_joints); + for (int i = 0; i < this->n_joints; ++i) { + q[i] = (this->sim->d->qpos[this->joint_ids[i]] - + this->cfg.min_joint_position[i]) / + (this->cfg.max_joint_position[i] - this->cfg.min_joint_position[i]); + } + return q; +} + +bool SimTilburgHand::collision_callback() { + this->state.collision = false; + // for (size_t i = 0; i < this->sim->d->ncon; ++i) { + // if (this->cfgeom.contains(this->sim->d->contact[i].geom[0]) && + // this->cfgeom.contains(this->sim->d->contact[i].geom[1])) { + // // ignore collisions between fingers + // continue; + // } + + // if ((this->cgeom.contains(this->sim->d->contact[i].geom[0]) or + // this->cgeom.contains(this->sim->d->contact[i].geom[1])) and + // // ignore all collision with ignored objects with frankahand + // // not just fingers + // not(this->ignored_collision_geoms.contains( + // this->sim->d->contact[i].geom[1]) or + // this->ignored_collision_geoms.contains( + // this->sim->d->contact[i].geom[1]))) { + // this->state.collision = true; + // break; + // } + // } + return this->state.collision; +} + +bool SimTilburgHand::is_grasped() { + // double width = this->get_normalized_width(); + + // if (this->state.last_commanded_width - this->cfg.epsilon_inner < width && + // width < this->state.last_commanded_width + this->cfg.epsilon_outer) { + // // this is the libfranka logic to decide whether an object is grasped + // return true; + // } + return false; +} + +bool SimTilburgHand::convergence_callback() { + auto qpos = get_normalized_joint_poses(); + this->state.is_moving = + (this->state.last_qpos - qpos).norm() > + 0.001 * (this->cfg.max_joint_position - this->cfg.min_joint_position) + .norm(); // 0.1% tolerance + this->state.last_qpos = qpos; + return not this->state.is_moving; +} + +void SimTilburgHand::grasp() { + switch (this->cfg.grasp_type) { + case common::GraspType::POWER_GRASP: + this->set_normalized_joint_poses(this->power_grasp_pose); + break; + default: + std::cerr << "Grasp type not implemented, using power grasp as default." + << std::endl; + this->set_normalized_joint_poses(this->power_grasp_pose); + break; + } +} + +void SimTilburgHand::open() { + this->set_normalized_joint_poses(this->open_pose); +} +void SimTilburgHand::shut() { + this->set_normalized_joint_poses(rcs::common::VectorXd::Ones(16)); +} + +void SimTilburgHand::m_reset() { + this->state = SimTilburgHandState(); + // reset state hard + for (int i = 0; i < this->n_joints; ++i) { + this->sim->d->qpos[this->joint_ids[i]] = this->cfg.min_joint_position[i]; + this->sim->d->ctrl[this->actuator_ids[i]] = this->cfg.min_joint_position[i]; + } +} + +void SimTilburgHand::reset() { this->m_reset(); } +} // namespace sim +} // namespace rcs diff --git a/src/sim/SimTilburgHand.h b/src/sim/SimTilburgHand.h new file mode 100644 index 00000000..25db0013 --- /dev/null +++ b/src/sim/SimTilburgHand.h @@ -0,0 +1,126 @@ +#ifndef RCS_TILBURG_HAND_SIM_H +#define RCS_TILBURG_HAND_SIM_H + +#include +#include +#include +#include + +#include "rcs/Robot.h" +#include "sim/sim.h" + +namespace rcs { +namespace sim { + +struct SimTilburgHandConfig : common::HandConfig { + rcs::common::VectorXd max_joint_position = + (rcs::common::VectorXd(16) << 1.6581, 1.5708, 0.0000, 1.5708, 1.6581, + 1.6581, 1.6581, 0.4363, 1.6581, 1.6581, 1.6581, 0.4363, 1.6581, 1.6581, + 1.6581, 0.4363) + .finished(); + rcs::common::VectorXd min_joint_position = + (rcs::common::VectorXd(16) << 0.0000, 0.0000, -1.7453, 0.0000, -0.0873, + -0.0873, -0.0873, -0.4363, -0.0873, -0.0873, -0.0873, -0.4363, -0.0873, + -0.0873, -0.0873, -0.4363) + .finished(); + std::vector ignored_collision_geoms = {}; + std::vector collision_geoms = + {}; //{"hand_c", "d435i_collision", + // "finger_0_left", "finger_0_right"}; + + std::vector collision_geoms_fingers = {}; //{"finger_0_left", + //"finger_0_right"}; + // following the real robot motor order convention + std::vector joints = { + "thumb_ip", "thumb_mcp", "thumb_mcp_rot", "thumb_cmc", + "index_dip", "index_pip", "index_mcp", "index_mcp_abadd", + "middle_dip", "middle_pip", "middle_mcp", "middle_mcp_abadd", + "ring_dip", "ring_pip", "ring_mcp", "ring_mcp_abadd"}; + std::vector actuators = { + // following the real robot motor order convention + "thumb_ip", "thumb_mcp", "thumb_mcp_rot", "thumb_cmc", + "index_dip", "index_pip", "index_mcp", "index_mcp_abadd", + "middle_dip", "middle_pip", "middle_mcp", "middle_mcp_abadd", + "ring_dip", "ring_pip", "ring_mcp", "ring_mcp_abadd"}; + + common::GraspType grasp_type = common::GraspType::POWER_GRASP; + + double seconds_between_callbacks = 0.0167; // 60 Hz + void add_id(const std::string &id) { + for (auto &s : this->collision_geoms) { + s = s + "_" + id; + } + for (auto &s : this->collision_geoms_fingers) { + s = s + "_" + id; + } + for (auto &s : this->ignored_collision_geoms) { + s = s + "_" + id; + } + for (auto &s : this->joints) { + s = s + "_" + id; + } + for (auto &s : this->actuators) { + s = s + "_" + id; + } + } +}; + +struct SimTilburgHandState : common::HandState { + rcs::common::VectorXd last_commanded_qpos = rcs::common::VectorXd::Zero(16); + bool is_moving = false; + rcs::common::VectorXd last_qpos = rcs::common::VectorXd::Zero(16); + bool collision = false; +}; + +class SimTilburgHand : public common::Hand { + private: + SimTilburgHandConfig cfg; + std::shared_ptr sim; + const int n_joints = 16; + std::vector actuator_ids = std::vector(n_joints); + std::vector joint_ids = std::vector(n_joints); + SimTilburgHandState state; + bool convergence_callback(); + bool collision_callback(); + std::set cgeom; + std::set cfgeom; + std::set ignored_collision_geoms; + void add_collision_geoms(const std::vector &cgeoms_str, + std::set &cgeoms_set, bool clear_before); + void m_reset(); + + const rcs::common::VectorXd open_pose = + (rcs::common::VectorXd(16) << 0.0, 0.0, 0.5, 1.4, 0.2, 0.2, 0.2, 0.7, 0.2, + 0.2, 0.2, 0.3, 0.2, 0.2, 0.2, 0.0) + .finished(); + const rcs::common::VectorXd power_grasp_pose = + (rcs::common::VectorXd(16) << 0.5, 0.5, 0.5, 1.4, 0.5, 0.5, 1.0, 0.7, 0.5, + 0.5, 1.0, 0.3, 0.5, 0.5, 1.0, 0.0) + .finished(); + bool set_grasp_type(common::GraspType grasp_type); + + public: + SimTilburgHand(std::shared_ptr sim, const SimTilburgHandConfig &cfg); + ~SimTilburgHand() override; + + bool set_parameters(const SimTilburgHandConfig &cfg); + + SimTilburgHandConfig *get_parameters() override; + + SimTilburgHandState *get_state() override; + + // normalized joints of the hand, 0 is closed, 1 is open + void set_normalized_joint_poses(const rcs::common::VectorXd &q) override; + rcs::common::VectorXd get_normalized_joint_poses() override; + + void reset() override; + + bool is_grasped() override; + + void grasp() override; + void open() override; + void shut() override; +}; +} // namespace sim +} // namespace rcs +#endif // RCS_TILBURG_HAND_SIM_H \ No newline at end of file