From da202799fd32bff9721f76bd756ee149894fc657 Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Tue, 5 Aug 2025 18:01:34 +0200 Subject: [PATCH 01/74] chg: Created a C++ side base class for generic Hand, and refactored Tilburg Hand python code to inherit from it --- include/rcs/Robot.h | 37 ++++++++++++++++ python/rcs/_core/common.pyi | 21 +++++++++ python/rcs/envs/base.py | 4 +- python/rcs/hand/interface.py | 31 -------------- python/rcs/hand/tilburg_hand.py | 76 ++++++++++++++++++++++++++++++--- src/pybind/rcs.cpp | 68 +++++++++++++++++++++++++++++ 6 files changed, 197 insertions(+), 40 deletions(-) delete mode 100644 python/rcs/hand/interface.py diff --git a/include/rcs/Robot.h b/include/rcs/Robot.h index 793ca6e5..e55dccc4 100644 --- a/include/rcs/Robot.h +++ b/include/rcs/Robot.h @@ -91,6 +91,13 @@ struct GripperState { virtual ~GripperState(){}; }; +struct HandConfig { + virtual ~HandConfig(){}; +}; +struct HandState { + virtual ~HandState(){}; +}; + class Robot { public: virtual ~Robot(){}; @@ -158,6 +165,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..497d415c 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -17,6 +17,9 @@ __all__ = [ "GripperConfig", "GripperState", "HARDWARE", + "Hand", + "HandConfig", + "HandState", "IK", "IdentityRotMatrix", "IdentityRotQuatVec", @@ -63,6 +66,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( diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 4eee1be9..8855c378 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -18,7 +18,7 @@ get_space, get_space_keys, ) -from rcs.hand.interface import BaseHand +from rcs._core.common import Hand 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 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..9c69b16b 100644 --- a/python/rcs/hand/tilburg_hand.py +++ b/python/rcs/hand/tilburg_hand.py @@ -1,11 +1,12 @@ import copy +import enum import logging from dataclasses import dataclass from time import sleep +from rcs._core import common import numpy as np from rcs.envs.space_utils import Vec18Type -from rcs.hand.interface import BaseHand from tilburg_hand import Finger, TilburgHandMotorInterface, Unit # Setup logger @@ -15,16 +16,26 @@ @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" + def __post_init__(self): + # 👇 satisfy pybind11 by actually calling the C++ constructor + super().__init__() -class TilburgHand(BaseHand): +class GRASP_TYPES(enum.Enum): + """Grasp types for the Tilburg Hand""" + POWER_GRASP = "power_grasp" + PRECISION_GRASP = "precision_grasp" + LATERAL_GRASP = "lateral_grasp" + TRIPOD_GRASP = "tripod_grasp" + +class TilburgHand(common.Hand): """ Tilburg Hand Class This class provides an interface for controlling the Tilburg Hand. @@ -35,10 +46,35 @@ 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, + ] + ) + + GRASP_TYPE = GRASP_TYPES.POWER_GRASP + def __init__(self, cfg: THConfig, verbose: bool = False): """ Initializes the Tilburg Hand interface. """ + super().__init__() self._cfg = cfg self._motors = TilburgHandMotorInterface( @@ -115,9 +151,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.GRASP_TYPE == GRASP_TYPES.POWER_GRASP): + pos_normalized = self.POWER_GRASP_VALUES * self._cfg.grasp_percentage + else: + logger.warning(f"Grasp type {self.GRASP_TYPE.value} 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.GRASP_TYPE.value} grasp with intensity: {self._cfg.grasp_percentage:.2f}") def auto_recovery(self): if not np.array(self._motors.check_enabled_motors()).all(): @@ -127,6 +167,28 @@ 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: GRASP_TYPES): + """ + Sets the grasp type for the hand. + """ + if not isinstance(grasp_type, GRASP_TYPES): + raise ValueError(f"Invalid grasp type: {grasp_type}. Must be an instance of GRASP_TYPES.") + if grasp_type == GRASP_TYPES.POWER_GRASP: + self.GRASP_TYPE = GRASP_TYPES.POWER_GRASP + elif grasp_type == GRASP_TYPES.PRECISION_GRASP: + logger.warning("Precision grasp is not implemented yet. Defaulting to power grasp.") + self.GRASP_TYPE = GRASP_TYPES.POWER_GRASP + elif grasp_type == GRASP_TYPES.LATERAL_GRASP: + logger.warning("Lateral grasp is not implemented yet. Defaulting to power grasp.") + self.GRASP_TYPE = GRASP_TYPES.POWER_GRASP + elif grasp_type == GRASP_TYPES.TRIPOD_GRASP: + logger.warning("Tripod grasp is not implemented yet. Defaulting to power grasp.") + self.GRASP_TYPE = GRASP_TYPES.POWER_GRASP + else: + raise ValueError(f"Unknown grasp type: {grasp_type}.") + + logger.info(f"Grasp type set to: {self.GRASP_TYPE.value}") + #### BaseHandControl Interface methods #### def grasp(self): @@ -136,7 +198,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): """ @@ -163,4 +225,4 @@ 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) + self.set_pos_vector(values) \ No newline at end of file diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 2c55ed72..ea365267 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -117,6 +117,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 +315,10 @@ PYBIND11_MODULE(_core, m) { py::class_(common, "RobotState"); py::class_(common, "GripperConfig"); py::class_(common, "GripperState"); + 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 +364,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_( From 492c982006c3bd38a8a7ae9d625c3a9c62be7170 Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Wed, 6 Aug 2025 10:30:44 +0200 Subject: [PATCH 02/74] feat: added a grasp type enum for the hand --- include/rcs/Robot.h | 4 +++ python/rcs/_core/common.pyi | 44 +++++++++++++++++++++++++++++++++ python/rcs/hand/tilburg_hand.py | 41 +++++++++++++----------------- src/pybind/rcs.cpp | 6 +++++ 4 files changed, 71 insertions(+), 24 deletions(-) diff --git a/include/rcs/Robot.h b/include/rcs/Robot.h index e55dccc4..fb759fe5 100644 --- a/include/rcs/Robot.h +++ b/include/rcs/Robot.h @@ -91,6 +91,10 @@ struct GripperState { virtual ~GripperState(){}; }; +enum GraspType { POWER_GRASP = 0, + PRECISION_GRASP, + LATERAL_GRASP, + TRIPOD_GRASP }; struct HandConfig { virtual ~HandConfig(){}; }; diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index 497d415c..0d9d01d7 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -13,6 +13,7 @@ __all__ = [ "BaseCameraConfig", "FR3", "FrankaHandTCPOffset", + "GraspType", "Gripper", "GripperConfig", "GripperState", @@ -24,6 +25,9 @@ __all__ = [ "IdentityRotMatrix", "IdentityRotQuatVec", "IdentityTranslation", + "LATERAL_GRASP", + "POWER_GRASP", + "PRECISION_GRASP", "Pin", "Pose", "RL", @@ -36,6 +40,7 @@ __all__ = [ "RobotType", "SIMULATION", "SO101", + "TRIPOD_GRASP", "UR5e", "robots_meta_config", ] @@ -49,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: ... @@ -273,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/hand/tilburg_hand.py b/python/rcs/hand/tilburg_hand.py index 9c69b16b..1501d245 100644 --- a/python/rcs/hand/tilburg_hand.py +++ b/python/rcs/hand/tilburg_hand.py @@ -23,18 +23,12 @@ class THConfig(common.HandConfig): 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 GRASP_TYPES(enum.Enum): - """Grasp types for the Tilburg Hand""" - POWER_GRASP = "power_grasp" - PRECISION_GRASP = "precision_grasp" - LATERAL_GRASP = "lateral_grasp" - TRIPOD_GRASP = "tripod_grasp" - class TilburgHand(common.Hand): """ Tilburg Hand Class @@ -68,7 +62,6 @@ class TilburgHand(common.Hand): ] ) - GRASP_TYPE = GRASP_TYPES.POWER_GRASP def __init__(self, cfg: THConfig, verbose: bool = False): """ @@ -151,13 +144,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): - if(self.GRASP_TYPE == GRASP_TYPES.POWER_GRASP): + 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.GRASP_TYPE.value} is not implemented. Defaulting to power grasp.") + 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"Performing {self.GRASP_TYPE.value} grasp with intensity: {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(): @@ -167,27 +160,27 @@ 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: GRASP_TYPES): + def set_grasp_type(self, grasp_type: common.GraspType): """ Sets the grasp type for the hand. """ - if not isinstance(grasp_type, GRASP_TYPES): - raise ValueError(f"Invalid grasp type: {grasp_type}. Must be an instance of GRASP_TYPES.") - if grasp_type == GRASP_TYPES.POWER_GRASP: - self.GRASP_TYPE = GRASP_TYPES.POWER_GRASP - elif grasp_type == GRASP_TYPES.PRECISION_GRASP: + if not isinstance(grasp_type, common.GraspType): + raise ValueError(f"Invalid grasp type: {grasp_type}. Must be an instance of common.GraspType.") + 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.GRASP_TYPE = GRASP_TYPES.POWER_GRASP - elif grasp_type == GRASP_TYPES.LATERAL_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.GRASP_TYPE = GRASP_TYPES.POWER_GRASP - elif grasp_type == GRASP_TYPES.TRIPOD_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.GRASP_TYPE = GRASP_TYPES.POWER_GRASP + self._cfg.grasp_type = common.GraspType.POWER_GRASP else: raise ValueError(f"Unknown grasp type: {grasp_type}.") - logger.info(f"Grasp type set to: {self.GRASP_TYPE.value}") + logger.info(f"Grasp type set to: {self._cfg.grasp_type}") #### BaseHandControl Interface methods #### diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index ea365267..816c2c25 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -315,6 +315,12 @@ 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") From 3f285223cf49af7edf2f69fbedc9c39e758051e3 Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Wed, 6 Aug 2025 10:31:05 +0200 Subject: [PATCH 03/74] feat: added a basic compiling sim tilburg hand --- src/sim/CMakeLists.txt | 2 +- src/sim/SimTilburgHand.cpp | 208 +++++++++++++++++++++++++++++++++++++ src/sim/SimTilburgHand.h | 120 +++++++++++++++++++++ 3 files changed, 329 insertions(+), 1 deletion(-) create mode 100644 src/sim/SimTilburgHand.cpp create mode 100644 src/sim/SimTilburgHand.h 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..2d8944dc --- /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_commanded_qpos - qpos).norm() > + 0.001 * (this->cfg.max_joint_position - this->cfg.min_joint_position) + .norm(); // 0.1% tolerance + this->state.last_commanded_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..61aa827f --- /dev/null +++ b/src/sim/SimTilburgHand.h @@ -0,0 +1,120 @@ +#ifndef RCS_FRANKA_HAND_SIM_H +#define RCS_FRANKA_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.66, 1.57, 1.75, 1.57, 1.66, 1.66, 1.66, 0.44, + 1.66, 1.66, 1.66, 0.44, 1.66, 1.66, 1.66, 0.44).finished(); + rcs::common::VectorXd min_joint_position = (rcs::common::VectorXd(16) << + -0.09, 0.00, 0.00, 0.00, -0.09, -0.09, 0.00, -0.44, + -0.09, -0.09, 0.00, -0.44, -0.09, -0.09, 0.00, -0.44).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"}; + std::vector joints = { + "thumb_cmc", "thumb_mcp_rot", "thumb_mcp", "thumb_ip", + "index_mcp_abadd", "index_mcp", "index_pip", "index_dip", + "middle_mcp_abadd", "middle_mcp", "middle_pip", "middle_dip", + "ring_mcp_abadd", "ring_mcp", "ring_pip", "ring_dip"}; + std::vector actuators = { + "thumb_cmc", "thumb_mcp_rot", "thumb_mcp", "thumb_ip", + "index_mcp_abadd", "index_mcp", "index_pip", "index_dip", + "middle_mcp_abadd", "middle_mcp", "middle_pip", "middle_dip", + "ring_mcp_abadd", "ring_mcp", "ring_pip", "ring_dip"}; + + 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_width = 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_FRANKA_HAND_SIM_H \ No newline at end of file From 3065fa527c38840196590d8713d6eef3a3b959cd Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Wed, 6 Aug 2025 10:56:49 +0200 Subject: [PATCH 04/74] fix: syntax consistency --- src/sim/SimTilburgHand.cpp | 22 +++++++++++----------- src/sim/SimTilburgHand.h | 8 ++++---- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/sim/SimTilburgHand.cpp b/src/sim/SimTilburgHand.cpp index 2d8944dc..7168364c 100644 --- a/src/sim/SimTilburgHand.cpp +++ b/src/sim/SimTilburgHand.cpp @@ -153,24 +153,24 @@ bool SimTilburgHand::collision_callback() { return this->state.collision; } -// bool SimTilburgHand::is_grasped() { -// double width = this->get_normalized_width(); +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; -// } + // 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_commanded_qpos - qpos).norm() > + (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_commanded_qpos = qpos; + this->state.last_qpos = qpos; return not this->state.is_moving; } diff --git a/src/sim/SimTilburgHand.h b/src/sim/SimTilburgHand.h index 61aa827f..66259952 100644 --- a/src/sim/SimTilburgHand.h +++ b/src/sim/SimTilburgHand.h @@ -1,5 +1,5 @@ -#ifndef RCS_FRANKA_HAND_SIM_H -#define RCS_FRANKA_HAND_SIM_H +#ifndef RCS_TILBURG_HAND_SIM_H +#define RCS_TILBURG_HAND_SIM_H #include #include @@ -62,7 +62,7 @@ struct SimTilburgHandConfig : common::HandConfig { struct SimTilburgHandState : common::HandState { rcs::common::VectorXd last_commanded_qpos = rcs::common::VectorXd::Zero(16); bool is_moving = false; - rcs::common::VectorXd last_width = rcs::common::VectorXd::Zero(16); + rcs::common::VectorXd last_qpos = rcs::common::VectorXd::Zero(16); bool collision = false; }; @@ -117,4 +117,4 @@ class SimTilburgHand : public common::Hand { }; } // namespace sim } // namespace rcs -#endif // RCS_FRANKA_HAND_SIM_H \ No newline at end of file +#endif // RCS_TILBURG_HAND_SIM_H \ No newline at end of file From 515b10ce343a3a65148f15b8c3fcfaf88b360b57 Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Wed, 6 Aug 2025 10:57:17 +0200 Subject: [PATCH 05/74] feat: pybind for SimTilburgHand --- src/pybind/rcs.cpp | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 816c2c25..c0913454 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -491,6 +492,46 @@ 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("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) From 00ca9b3f6fddebed833740a2757a679be095d945 Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Wed, 6 Aug 2025 10:58:19 +0200 Subject: [PATCH 06/74] feat: stubgen for SimTilburgHand --- python/rcs/_core/sim.pyi | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index 6c6e34fe..af459002 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,34 @@ 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 set_parameters(self, cfg: SimTilburgHandConfig) -> bool: ... + +class SimTilburgHandConfig(rcs._core.common.HandConfig): + 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 = From 67fc60f77157a14ee23b033bf68669e06ceb1145 Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Wed, 6 Aug 2025 11:07:42 +0200 Subject: [PATCH 07/74] fix: add SimTIlburgHand to exported stuff --- python/rcs/sim/__init__.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/python/rcs/sim/__init__.py b/python/rcs/sim/__init__.py index c9476131..7499018d 100644 --- a/python/rcs/sim/__init__.py +++ b/python/rcs/sim/__init__.py @@ -3,6 +3,9 @@ SimGripper, SimGripperConfig, SimGripperState, + SimTilburgHand, + SimTilburgHandConfig, + SimTilburgHandState, SimRobot, SimRobotConfig, SimRobotState, @@ -17,6 +20,9 @@ "SimGripper", "SimGripperConfig", "SimGripperState", + "SimTilburgHand", + "SimTilburgHandConfig", + "SimTilburgHandState", "gui_loop", "SimCameraConfig", ] From b07c6f0fca6a144d368f25f08172eab626084306 Mon Sep 17 00:00:00 2001 From: kennkhr Date: Wed, 6 Aug 2025 10:58:48 +0200 Subject: [PATCH 08/74] feat: add xArm7 extension --- extensions/rcs_xarm7/pyproject.toml | 28 ++++ .../rcs_xarm7/src/rcs_xarm7/__init__.py | 0 .../rcs_xarm7/src/rcs_xarm7/creators.py | 125 ++++++++++++++++++ .../src/rcs_xarm7/env_joint_control.py | 73 ++++++++++ extensions/rcs_xarm7/src/rcs_xarm7/hw.py | 54 ++++++++ .../src/rcs_xarm7/xArm7, RobotMetaConfig{.cpp | 11 ++ include/rcs/Robot.h | 14 +- python/rcs/_core/common.pyi | 7 +- src/pybind/rcs.cpp | 1 + 9 files changed, 311 insertions(+), 2 deletions(-) create mode 100644 extensions/rcs_xarm7/pyproject.toml create mode 100644 extensions/rcs_xarm7/src/rcs_xarm7/__init__.py create mode 100644 extensions/rcs_xarm7/src/rcs_xarm7/creators.py create mode 100644 extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py create mode 100644 extensions/rcs_xarm7/src/rcs_xarm7/hw.py create mode 100644 extensions/rcs_xarm7/src/rcs_xarm7/xArm7, RobotMetaConfig{.cpp diff --git a/extensions/rcs_xarm7/pyproject.toml b/extensions/rcs_xarm7/pyproject.toml new file mode 100644 index 00000000..266ca579 --- /dev/null +++ b/extensions/rcs_xarm7/pyproject.toml @@ -0,0 +1,28 @@ +[build-system] +requires = ["setuptools"] +build-backend = "setuptools.build_meta" + +[project] +name = "rcs_xarm7" +version = "0.4.0" +description="RCS xArm7 module" +dependencies = [ + "rcs==0.4.0", + "xarm-python-sdk==1.17.0", +] +readme = "README.md" +maintainers = [ + { name = "Tobias Jülg", email = "tobias.juelg@utn.de" }, +] +authors = [ + { name = "Tobias Jülg", email = "tobias.juelg@utn.de" }, + { name = "Pierre Krack", email = "pierre.krack@utn.de" }, + { name = "Ken Nakahara", email = "knakahara@lasr.org" }, +] +requires-python = ">=3.10" +classifiers = [ + "Development Status :: 3 - Alpha", + "License :: OSI Approved :: GNU Affero General Public License v3 (AGPLv3)", + "Programming Language :: Python :: 3", +] + diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/__init__.py b/extensions/rcs_xarm7/src/rcs_xarm7/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py new file mode 100644 index 00000000..0bf4a1b2 --- /dev/null +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -0,0 +1,125 @@ +import logging +from os import PathLike +from pathlib import Path +from typing import Type + +import gymnasium as gym +from gymnasium.envs.registration import EnvCreator + +from xarm.wrapper import XArmAPI + +from rcs.camera.hw import HardwareCameraSet +from rcs.camera.sim import SimCameraSet +from rcs.envs.base import ( + CameraSetWrapper, + ControlMode, + GripperWrapper, + RelativeActionSpace, + RelativeTo, + RobotEnv, +) +from rcs.envs.creators import RCSHardwareEnvCreator +from rcs.envs.sim import CollisionGuard, GripperWrapperSim, RobotSimWrapper, SimWrapper +from rcs.sim import SimCameraConfig, SimGripperConfig, SimRobotConfig +from rcs_xarm7.hw import XArm7 + +import rcs +from rcs import common, sim + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +class RCSXArm7EnvCreator(RCSHardwareEnvCreator): + def __call__( # type: ignore + self, + ip: str, + urdf_path: str, + calibration_dir: PathLike | str | None = None, + camera_set: HardwareCameraSet | None = None, + max_relative_movement: float | tuple[float, float] | None = None, + relative_to: RelativeTo = RelativeTo.LAST_STEP, + ) -> gym.Env: + if isinstance(calibration_dir, str): + calibration_dir = Path(calibration_dir) + robot = XArm7(ip=ip, urdf_path=urdf_path) + env: gym.Env = RobotEnv(robot, ControlMode.JOINTS) + + if camera_set is not None: + camera_set.start() + camera_set.wait_for_frames() + logger.info("CameraSet started") + env = CameraSetWrapper(env, camera_set) + + if max_relative_movement is not None: + env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + + return env + + +class XArm7SimEnvCreator(EnvCreator): + def __call__( # type: ignore + self, + control_mode: ControlMode, + robot_cfg: SimRobotConfig, + urdf_path: str, + mjcf: str, + collision_guard: bool = False, + gripper_cfg: SimGripperConfig | None = None, + cameras: dict[str, SimCameraConfig] | None = None, + max_relative_movement: float | tuple[float, float] | None = None, + relative_to: RelativeTo = RelativeTo.LAST_STEP, + sim_wrapper: Type[SimWrapper] | None = None, + ) -> gym.Env: + """ + Creates a simulation environment for the FR3 robot. + Args: + control_mode (ControlMode): Control mode for the robot. + 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. + 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 + (in radians) movements. If None, no restriction is applied. + relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step. + urdf_path (str | PathLike | None): Path to the URDF file. If None, the URDF file is automatically deduced + which requires a UTN compatible lab scene to be present. + mjcf (str | PathLike): Path to the Mujoco scene XML file. Defaults to "fr3_empty_world". + sim_wrapper (Type[SimWrapper] | None): Wrapper to be applied before the simulation wrapper. This is useful + for reset management e.g. resetting objects in the scene with correct observations. Defaults to None. + Returns: + gym.Env: The configured simulation environment for the FR3 robot. + """ + simulation = sim.Sim(mjcf) + + ik = rcs.common.RL(str(urdf_path)) + robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) + env: gym.Env = RobotEnv(robot, control_mode) + env = RobotSimWrapper(env, simulation, sim_wrapper) + + if cameras is not None: + camera_set = SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True) + env = CameraSetWrapper(env, camera_set, include_depth=True) + + if gripper_cfg is not None and isinstance(gripper_cfg, SimGripperConfig): + gripper = sim.SimGripper(simulation, gripper_cfg) + env = GripperWrapper(env, gripper, binary=False) + env = GripperWrapperSim(env, gripper) + + if collision_guard: + env = CollisionGuard.env_from_xml_paths( + env, + str(rcs.scenes.get(str(mjcf), mjcf)), + str(urdf_path), + gripper=gripper_cfg is not None, + check_home_collision=False, + control_mode=control_mode, + tcp_offset=rcs.common.Pose(common.FrankaHandTCPOffset()), + sim_gui=True, + truncate_on_collision=True, + ) + if max_relative_movement is not None: + env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + + return env diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py new file mode 100644 index 00000000..ebe1aef3 --- /dev/null +++ b/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py @@ -0,0 +1,73 @@ +import logging +from time import sleep + +from rcs._core.common import RobotPlatform +from rcs.envs.base import ControlMode, RelativeTo +from rcs_xarm7.creators import RCSXArm7EnvCreator, XArm7SimEnvCreator + +import rcs +from rcs import sim + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +ROBOT_IP = "192.168.1.245" +# ROBOT_INSTANCE = RobotPlatform.SIMULATION +ROBOT_INSTANCE = RobotPlatform.HARDWARE + + +def main(): + + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + env_rel = RCSXArm7EnvCreator()( + ip=ROBOT_IP, + urdf_path="/home/ken/Downloads/xarm7.urdf", + relative_to=RelativeTo.LAST_STEP, + ) + else: + cfg = sim.SimRobotConfig() + cfg.robot_type = rcs.common.RobotType.XArm7 + cfg.actuators = ["1", "2", "3", "4", "5"] + cfg.joints = ["1", "2", "3", "4", "5"] + cfg.arm_collision_geoms = [] + cfg.attachment_site = "gripper" + + grp_cfg = sim.SimGripperConfig() + grp_cfg.actuator = "6" + grp_cfg.joint = "6" + grp_cfg.collision_geoms = [] + grp_cfg.collision_geoms_fingers = [] + + env_rel = XArm7SimEnvCreator()( + control_mode=ControlMode.JOINTS, + urdf_path="/home/tobi/coding/lerobot/so101_new_calib.urdf", + robot_cfg=cfg, + collision_guard=False, + mjcf="/home/tobi/coding/lerobot/SO-ARM100/Simulation/XArm7/scene.xml", + gripper_cfg=grp_cfg, + # camera_set_cfg=default_mujoco_cameraset_cfg(), + max_relative_movement=None, + # max_relative_movement=10.0, + # max_relative_movement=0.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(100): + # sample random relative action and execute it + act = env_rel.action_space.sample() + print(act) + # act["gripper"] = 1.0 + obs, reward, terminated, truncated, info = env_rel.step(act) + print(obs) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + logger.info(act["gripper"]) + sleep(1.0) + + +if __name__ == "__main__": + main() diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/hw.py b/extensions/rcs_xarm7/src/rcs_xarm7/hw.py new file mode 100644 index 00000000..f0fb4a6a --- /dev/null +++ b/extensions/rcs_xarm7/src/rcs_xarm7/hw.py @@ -0,0 +1,54 @@ +import typing + +import numpy as np +from xarm.wrapper import XArmAPI + +from rcs import common + + +class XArm7(common.Robot): + def __init__(self, ip: str, urdf_path: str): + self.ik = common.RL(urdf_path=urdf_path) + self._xarm = XArmAPI(ip=ip) + + # def get_base_pose_in_world_coordinates(self) -> Pose: ... + def get_cartesian_position(self) -> common.Pose: + return self.ik.forward(self.get_joint_position()) + + def get_ik(self) -> common.IK | None: + return self.ik + + def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]]: # type: ignore + obs = self._hf_robot.get_servo_angle(is_radian=True)[1] + return np.array(obs, dtype=np.float64) + + def get_parameters(self): + a = common.RobotConfig() + a.robot_platform = common.RobotPlatform.HARDWARE + a.robot_type = common.RobotType.XArm7 + return a + + def get_state(self) -> common.RobotState: + return common.RobotState() + + def move_home(self) -> None: + home = typing.cast( + np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]], + common.robots_meta_config(common.RobotType.XArm7).q_home, + ) + self.set_joint_position(home) + + def reset(self) -> None: + pass + + def set_cartesian_position(self, pose: common.Pose) -> None: + joints = self.ik.ik(pose, q0=self.get_joint_position()) + if joints is not None: + self.set_joint_position(joints) + + def set_joint_position(self, q: np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]]) -> None: # type: ignore + self._hf_robot.set_servo_angle(angle=q, is_radian=True, wait=True) + + # def to_pose_in_robot_coordinates(self, pose_in_world_coordinates: Pose) -> Pose: ... + # def to_pose_in_world_coordinates(self, pose_in_robot_coordinates: Pose) -> Pose: ... + diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/xArm7, RobotMetaConfig{.cpp b/extensions/rcs_xarm7/src/rcs_xarm7/xArm7, RobotMetaConfig{.cpp new file mode 100644 index 00000000..2c2be825 --- /dev/null +++ b/extensions/rcs_xarm7/src/rcs_xarm7/xArm7, RobotMetaConfig{.cpp @@ -0,0 +1,11 @@ +xArm7, RobotMetaConfig{ + // q_home (7‐vector): + (VectorXd(7) << 0, 0, 0, 0, 0, 0, 0).finished(), + // dof: + 7, + // joint_limits (2×7): + (Eigen::Matrix(2, 7) << + // low 7‐tuple + -2 * M_PI, -2.094395, -2 * M_PI, -3.92699, -2 * M_PI, -M_PI, -2 * M_PI, + // high 7‐tuple + 2 * M_PI, 2.059488, 2 * M_PI, 0.191986, 2 * M_PI, 1.692969, 2 * M_PI).finished() \ No newline at end of file diff --git a/include/rcs/Robot.h b/include/rcs/Robot.h index 793ca6e5..b8cde72b 100644 --- a/include/rcs/Robot.h +++ b/include/rcs/Robot.h @@ -18,7 +18,7 @@ struct RobotMetaConfig { Eigen::Matrix joint_limits; }; -enum RobotType { FR3 = 0, UR5e, SO101 }; +enum RobotType { FR3 = 0, UR5e, SO101, XArm7 }; enum RobotPlatform { SIMULATION = 0, HARDWARE }; static const std::unordered_map robots_meta_config = @@ -57,6 +57,18 @@ static const std::unordered_map robots_meta_config = // high 6‐tuple 2 * M_PI, 2 * M_PI, 1 * M_PI, 2 * M_PI, 2 * M_PI, 2 * M_PI) .finished()}}, + // -------------- XArm7 -------------- + {XArm7, RobotMetaConfig{ + // q_home (7‐vector): + (VectorXd(7) << 0, 0, 0, 0, 0, 0, 0).finished(), + // dof: + 7, + // joint_limits (2×7): + (Eigen::Matrix(2, 7) << + // low 7‐tuple + -2 * M_PI, -2.094395, -2 * M_PI, -3.92699, -2 * M_PI, -M_PI, -2 * M_PI, + // high 7‐tuple + 2 * M_PI, 2.059488, 2 * M_PI, 0.191986, 2 * M_PI, 1.692969, 2 * M_PI).finished()}}, // -------------- SO101 -------------- {SO101, RobotMetaConfig{ diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index 3238eb55..1160a811 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -34,6 +34,7 @@ __all__ = [ "SIMULATION", "SO101", "UR5e", + "XArm7", "robots_meta_config", ] M = typing.TypeVar("M", bound=int) @@ -220,14 +221,17 @@ class RobotType: UR5e SO101 + + XArm7 """ FR3: typing.ClassVar[RobotType] # value = SO101: typing.ClassVar[RobotType] # value = UR5e: typing.ClassVar[RobotType] # value = + XArm7: typing.ClassVar[RobotType] # value = __members__: typing.ClassVar[ dict[str, RobotType] - ] # value = {'FR3': , 'UR5e': , 'SO101': } + ] # value = {'FR3': , 'UR5e': , 'SO101': , 'XArm7': } def __eq__(self, other: typing.Any) -> bool: ... def __getstate__(self) -> int: ... def __hash__(self) -> int: ... @@ -255,3 +259,4 @@ HARDWARE: RobotPlatform # value = SIMULATION: RobotPlatform # value = SO101: RobotType # value = UR5e: RobotType # value = +XArm7: RobotType # value = diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 2c55ed72..2618a7ce 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -241,6 +241,7 @@ PYBIND11_MODULE(_core, m) { .value("FR3", rcs::common::RobotType::FR3) .value("UR5e", rcs::common::RobotType::UR5e) .value("SO101", rcs::common::RobotType::SO101) + .value("XArm7", rcs::common::RobotType::XArm7) .export_values(); py::enum_(common, "RobotPlatform") From df35d8af9341a32943743ef59d9e1fe291e1c8be Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Wed, 6 Aug 2025 14:23:31 +0200 Subject: [PATCH 09/74] fix: consistent function name --- python/rcs/envs/base.py | 4 ++-- python/rcs/hand/tilburg_hand.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 8855c378..a008b9d1 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -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/hand/tilburg_hand.py b/python/rcs/hand/tilburg_hand.py index 1501d245..e61bc264 100644 --- a/python/rcs/hand/tilburg_hand.py +++ b/python/rcs/hand/tilburg_hand.py @@ -214,8 +214,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) \ No newline at end of file From aeb3f0ea6365037ceb944910330f66fb14f40228 Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Wed, 6 Aug 2025 14:24:03 +0200 Subject: [PATCH 10/74] feat: added a HandWrapperSim --- python/rcs/envs/sim.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 283b246b..0e4fc640 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -113,6 +113,25 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl info["is_grasped"] = self._gripper.get_normalized_width() > 0.01 and self._gripper.get_normalized_width() < 0.99 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, "Hand action must be of length 16" + 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]]): """ From d5a5a7eefda7d0df8c76f36031356e2cad87acaf Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Wed, 6 Aug 2025 14:24:20 +0200 Subject: [PATCH 11/74] feat: added default config for sim tilburg hand --- python/rcs/envs/utils.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 781391a3..2000bfee 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -38,6 +38,9 @@ def default_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: cfg.add_id(idx) return cfg +def default_sim_tilburg_hand_cfg() -> sim.SimTilburgHandConfig: + cfg = sim.SimTilburgHandConfig() + return cfg def default_digit(name2id: dict[str, str] | None, stream_name: str = "QVGA") -> DigitCam | None: if name2id is None: From b3155aa552d2e063e7b47ebb961ed03cf0299460 Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Wed, 6 Aug 2025 14:24:40 +0200 Subject: [PATCH 12/74] feat: added an option for simulating hand --- python/rcs/envs/creators.py | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index be519b43..ff6a64ea 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,12 +21,14 @@ from rcs.envs.sim import ( CollisionGuard, GripperWrapperSim, + HandWrapperSim, PickCubeSuccessWrapper, RandomCubePos, RobotSimWrapper, SimWrapper, ) -from rcs.envs.utils import default_sim_gripper_cfg, default_sim_robot_cfg +from rcs.envs.utils import default_sim_gripper_cfg, default_sim_robot_cfg, default_sim_tilburg_hand_cfg + import rcs from rcs import sim @@ -45,6 +48,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 +64,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 @@ -94,6 +101,14 @@ 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) From 95d78a46ac93e469be2134a9c4beeb301f701976 Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Wed, 6 Aug 2025 14:25:01 +0200 Subject: [PATCH 13/74] fix: change hard-coded value orders to match hardware --- src/sim/SimTilburgHand.h | 36 +++++++++++++++++++++--------------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/src/sim/SimTilburgHand.h b/src/sim/SimTilburgHand.h index 66259952..25db0013 100644 --- a/src/sim/SimTilburgHand.h +++ b/src/sim/SimTilburgHand.h @@ -13,12 +13,16 @@ namespace rcs { namespace sim { struct SimTilburgHandConfig : common::HandConfig { - rcs::common::VectorXd max_joint_position = (rcs::common::VectorXd(16) << - 1.66, 1.57, 1.75, 1.57, 1.66, 1.66, 1.66, 0.44, - 1.66, 1.66, 1.66, 0.44, 1.66, 1.66, 1.66, 0.44).finished(); - rcs::common::VectorXd min_joint_position = (rcs::common::VectorXd(16) << - -0.09, 0.00, 0.00, 0.00, -0.09, -0.09, 0.00, -0.44, - -0.09, -0.09, 0.00, -0.44, -0.09, -0.09, 0.00, -0.44).finished(); + 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", @@ -26,17 +30,19 @@ struct SimTilburgHandConfig : common::HandConfig { std::vector collision_geoms_fingers = {}; //{"finger_0_left", //"finger_0_right"}; + // following the real robot motor order convention std::vector joints = { - "thumb_cmc", "thumb_mcp_rot", "thumb_mcp", "thumb_ip", - "index_mcp_abadd", "index_mcp", "index_pip", "index_dip", - "middle_mcp_abadd", "middle_mcp", "middle_pip", "middle_dip", - "ring_mcp_abadd", "ring_mcp", "ring_pip", "ring_dip"}; + "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 = { - "thumb_cmc", "thumb_mcp_rot", "thumb_mcp", "thumb_ip", - "index_mcp_abadd", "index_mcp", "index_pip", "index_dip", - "middle_mcp_abadd", "middle_mcp", "middle_pip", "middle_dip", - "ring_mcp_abadd", "ring_mcp", "ring_pip", "ring_dip"}; - + // 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 From 212970d3f8456c5a1edb8b088e8738822c8f7703 Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Wed, 6 Aug 2025 14:25:14 +0200 Subject: [PATCH 14/74] feat: add an example for robot with hand --- examples/tilburg_grasp.py | 95 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 95 insertions(+) create mode 100644 examples/tilburg_grasp.py diff --git a/examples/tilburg_grasp.py b/examples/tilburg_grasp.py new file mode 100644 index 00000000..784d09d8 --- /dev/null +++ b/examples/tilburg_grasp.py @@ -0,0 +1,95 @@ +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_mujoco_cameraset_cfg, + default_tilburg_hw_hand_cfg, + default_sim_robot_cfg, + default_sim_tilburg_hand_cfg, +) + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +ROBOT_IP = "192.168.101.1" +ROBOT_INSTANCE = RobotPlatform.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 rcs_fr3 unlock + +or put it into guiding mode using: + +python -m rcs_fr3 guiding-mode + +When you are done you lock it again using: + +python -m rcs_fr3 lock + +or even shut it down using: + +python -m rcs_fr3 shutdown +""" + + +''' + else: + env_rel = SimEnvCreator()( + control_mode=ControlMode.JOINTS, + collision_guard=False, + robot_cfg=default_sim_robot_cfg(), + gripper_cfg=default_sim_gripper_cfg(), + cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=np.deg2rad(5), + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() +''' +def main(): + + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + # env_rel = RCSFR3EnvCreator()( + # ip=ROBOT_IP, + # control_mode=ControlMode.JOINTS, + # robot_cfg=default_fr3_hw_robot_cfg(), + # collision_guard=None, + # gripper_cfg=default_fr3_hw_gripper_cfg(), + # max_relative_movement=np.deg2rad(5), + # relative_to=RelativeTo.LAST_STEP, + # ) + return + else: + 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="/home/sbien/Documents/Development/RCS/models/scenes/fr3_tilburg_empty_world/scene.xml", + 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() From 80b703b15c9cdbd86256850f57d8b84c53947558 Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Wed, 6 Aug 2025 16:02:29 +0200 Subject: [PATCH 15/74] fix: account for binary case --- python/rcs/envs/sim.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 0e4fc640..42ef346c 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -121,7 +121,7 @@ def __init__(self, env, hand: sim.SimTilburgHand): 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, "Hand action must be of length 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]]: From 0e9ac2d3277edaf5e1f76caf502a07078ee7f903 Mon Sep 17 00:00:00 2001 From: Seongjin Bien Date: Wed, 6 Aug 2025 16:03:35 +0200 Subject: [PATCH 16/74] feat: add the simple hw tilburg test script --- examples/simple_tilburg_test.py | 62 +++++++++++++++++++++++++++++++++ 1 file changed, 62 insertions(+) create mode 100644 examples/simple_tilburg_test.py diff --git a/examples/simple_tilburg_test.py b/examples/simple_tilburg_test.py new file mode 100644 index 00000000..b970c51b --- /dev/null +++ b/examples/simple_tilburg_test.py @@ -0,0 +1,62 @@ +from rcs.hand.tilburg_hand import TilburgHand, THConfig +from rcs._core.common import GraspType +from time import sleep +import numpy as np + +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), "]") + config = 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=config, verbose=True) + hand.set_grasp_type(GraspType.POWER_GRASP) + hand.grasp() + sleep(2) + hand.open() + sleep(5) + hand.reset() + hand.close() + hand.disconnect() From e017caba80b14b78a567e2688ed6ee2315935fa6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 6 Aug 2025 09:45:57 +0200 Subject: [PATCH 17/74] fix(ik): pinocchio ik with mjcf - mjcf of pin ik must be the plain robot xml - it has to have the default attribute which needs to be at the beginning --- assets/fr3/mjcf/fr3_0.xml | 192 +++++++++++++++++++++ assets/fr3/mjcf/fr3_common.xml | 42 ----- assets/fr3/mjcf/fr3_unnamed.xml | 54 +++++- assets/scenes/fr3_empty_world/fr3_0.xml | 156 +---------------- assets/scenes/fr3_simple_pick_up/fr3_0.xml | 153 +--------------- python/rcs/envs/creators.py | 3 + python/rcs/envs/utils.py | 5 +- src/rcs/IK.cpp | 10 +- 8 files changed, 253 insertions(+), 362 deletions(-) create mode 100644 assets/fr3/mjcf/fr3_0.xml mode change 100644 => 120000 assets/scenes/fr3_empty_world/fr3_0.xml mode change 100644 => 120000 assets/scenes/fr3_simple_pick_up/fr3_0.xml diff --git a/assets/fr3/mjcf/fr3_0.xml b/assets/fr3/mjcf/fr3_0.xml new file mode 100644 index 00000000..35f35039 --- /dev/null +++ b/assets/fr3/mjcf/fr3_0.xml @@ -0,0 +1,192 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/fr3/mjcf/fr3_common.xml b/assets/fr3/mjcf/fr3_common.xml index 1196d6bc..a29b9dde 100644 --- a/assets/fr3/mjcf/fr3_common.xml +++ b/assets/fr3/mjcf/fr3_common.xml @@ -1,49 +1,7 @@ -