diff --git a/pyproject.toml b/pyproject.toml index 6c526af0..f41a8a55 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -54,13 +54,18 @@ classifiers = [ # "mypy==1.4.1", # ] +[project.optional-dependencies] +so101 = [ + "lerobot @ git+https://github.com/huggingface/lerobot.git", +] + [tool.scikit-build] build.verbose = true build.targets = ["_core", "scenes"] logging.level = "INFO" build-dir = "build" -wheel.packages = ["python/rcs"] +wheel.packages = ["python/rcs", "python/rcs_so101"] install.components = ["python_package"] [tool.ruff] diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index d650d90c..7216bc9f 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -30,6 +30,7 @@ __all__ = [ "RobotState", "RobotType", "SIMULATION", + "SO101", "UR5e", "robots_meta_config", ] @@ -62,6 +63,7 @@ class GripperState: class IK: def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ... + def forward(self, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ... def ik( self, pose: Pose, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ... ) -> numpy.ndarray[M, numpy.dtype[numpy.float64]] | None: ... @@ -158,6 +160,7 @@ class Robot: class RobotConfig: robot_platform: RobotPlatform robot_type: RobotType + def __init__(self) -> None: ... class RobotMetaConfig: @property @@ -206,13 +209,16 @@ class RobotType: FR3 UR5e + + SO101 """ FR3: typing.ClassVar[RobotType] # value = + SO101: typing.ClassVar[RobotType] # value = UR5e: typing.ClassVar[RobotType] # value = __members__: typing.ClassVar[ dict[str, RobotType] - ] # value = {'FR3': , 'UR5e': } + ] # value = {'FR3': , 'UR5e': , 'SO101': } def __eq__(self, other: typing.Any) -> bool: ... def __getstate__(self) -> int: ... def __hash__(self) -> int: ... @@ -238,4 +244,5 @@ def robots_meta_config(robot_type: RobotType) -> RobotMetaConfig: ... FR3: RobotType # value = HARDWARE: RobotPlatform # value = SIMULATION: RobotPlatform # value = +SO101: RobotType # value = UR5e: RobotType # value = diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index 9a526226..6cb64111 100644 --- a/python/rcs/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -116,8 +116,11 @@ class SimGripperConfig(rcs._core.common.GripperConfig): epsilon_inner: float epsilon_outer: float ignored_collision_geoms: list[str] - joint1: str - joint2: str + joint: str + max_actuator_width: float + max_joint_width: float + min_actuator_width: float + min_joint_width: float seconds_between_callbacks: float def __init__(self) -> None: ... def add_id(self, id: str) -> None: ... @@ -132,8 +135,6 @@ class SimGripperState(rcs._core.common.GripperState): def last_commanded_width(self) -> float: ... @property def last_width(self) -> float: ... - @property - def max_unnormalized_width(self) -> float: ... class SimRobot(rcs._core.common.Robot): def __init__( diff --git a/python/rcs/sim/__init__.py b/python/rcs/sim/__init__.py index 496ae2ad..c9476131 100644 --- a/python/rcs/sim/__init__.py +++ b/python/rcs/sim/__init__.py @@ -1,4 +1,5 @@ from rcs._core.sim import ( + SimCameraConfig, SimGripper, SimGripperConfig, SimGripperState, @@ -17,4 +18,5 @@ "SimGripperConfig", "SimGripperState", "gui_loop", + "SimCameraConfig", ] diff --git a/python/rcs_so101/__init__.py b/python/rcs_so101/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/python/rcs_so101/creators.py b/python/rcs_so101/creators.py new file mode 100644 index 00000000..32d1132c --- /dev/null +++ b/python/rcs_so101/creators.py @@ -0,0 +1,150 @@ +import logging +from os import PathLike +from pathlib import Path +from typing import Type + +import gymnasium as gym +import rcs +from gymnasium.envs.registration import EnvCreator +from lerobot.common.robots import make_robot_from_config +from lerobot.common.robots.so101_follower.config_so101_follower import ( + SO101FollowerConfig, +) +from lerobot.common.teleoperators.so101_leader.config_so101_leader import ( + SO101LeaderConfig, +) +from lerobot.common.teleoperators.so101_leader.so101_leader import SO101Leader +from lerobot.common.teleoperators.utils import make_teleoperator_from_config +from rcs import common, sim +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_so101.hw import SO101, S0101Gripper + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +class RCSSO101EnvCreator(RCSHardwareEnvCreator): + def __call__( # type: ignore + self, + id: str, + port: 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) + cfg = SO101FollowerConfig(id=id, calibration_dir=calibration_dir, port=port) + hf_robot = make_robot_from_config(cfg) + robot = SO101(hf_robot, urdf_path=urdf_path) + env: gym.Env = RobotEnv(robot, ControlMode.JOINTS) + # env = FR3HW(env) + + gripper = S0101Gripper(hf_robot) + env = GripperWrapper(env, gripper, binary=False) + + 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 + + @staticmethod + def teleoperator( + id: str, + port: str, + calibration_dir: PathLike | str | None = None, + ) -> SO101Leader: + if isinstance(calibration_dir, str): + calibration_dir = Path(calibration_dir) + cfg = SO101LeaderConfig(id=id, calibration_dir=calibration_dir, port=port) + teleop = make_teleoperator_from_config(cfg) + teleop.connect() + return teleop + + +class SO101SimEnvCreator(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.IK(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/python/rcs_so101/env_joint_control.py b/python/rcs_so101/env_joint_control.py new file mode 100644 index 00000000..0862e7b9 --- /dev/null +++ b/python/rcs_so101/env_joint_control.py @@ -0,0 +1,73 @@ +import logging +from time import sleep + +import rcs +from rcs import sim +from rcs._core.common import RobotPlatform +from rcs.envs.base import ControlMode, RelativeTo +from rcs_so101.creators import RCSSO101EnvCreator, SO101SimEnvCreator + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +ROBOT_INSTANCE = RobotPlatform.SIMULATION + + +def main(): + + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + env_rel = RCSSO101EnvCreator()( + id="so101_follower", + urdf_path="/home/tobi/coding/lerobot/so101_new_calib.urdf", + port="/dev/ttyACM0", + calibration_dir="/home/tobi/coding/lerobot-container/calibration/robots/so101_follower/ninja_follower.json", + # max_relative_movement=(0.5, np.deg2rad(90)), + relative_to=RelativeTo.LAST_STEP, + ) + else: + cfg = sim.SimRobotConfig() + cfg.robot_type = rcs.common.RobotType.SO101 + 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 = SO101SimEnvCreator()( + 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/SO101/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/python/rcs_so101/hw.py b/python/rcs_so101/hw.py new file mode 100644 index 00000000..ee4e9ec7 --- /dev/null +++ b/python/rcs_so101/hw.py @@ -0,0 +1,118 @@ +import typing + +import numpy as np +from lerobot.common.robots.so101_follower.so101_follower import SO101Follower +from rcs import common + + +class SO101(common.Robot): + def __init__(self, hf_robot: SO101Follower, urdf_path: str): + self.ik = common.IK(urdf_path=urdf_path) + self._hf_robot = hf_robot + self._hf_robot.connect() + + # 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[typing.Literal[5], np.dtype[np.float64]]: # type: ignore + obs = self._hf_robot.get_observation() + return np.array( + [ + obs["shoulder_pan.pos"], + obs["shoulder_lift.pos"], + obs["elbow_flex.pos"], + obs["wrist_flex.pos"], + obs["wrist_roll.pos"], + obs["gripper.pos"], + ], + dtype=np.float64, + ) + + def get_parameters(self): + a = common.RobotConfig() + a.robot_platform = common.RobotPlatform.HARDWARE + a.robot_type = common.RobotType.SO101 + return a + + def get_state(self) -> common.RobotState: + return common.RobotState() + + def move_home(self) -> None: + home = typing.cast( + np.ndarray[typing.Literal[5], np.dtype[np.float64]], + common.robots_meta_config(common.RobotType.SO101).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[typing.Literal[5], np.dtype[np.float64]]) -> None: # type: ignore + self._hf_robot.send_action( + { + "shoulder_pan.pos": q[0], + "shoulder_lift.pos": q[1], + "elbow_flex.pos": q[2], + "wrist_flex.pos": q[3], + "wrist_roll.pos": q[4], + } + ) + + # 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: ... + + +# TODO: problem when we inherit from gripper then we also need to call init which doesnt exist +class S0101Gripper(common.Gripper): + def __init__(self, hf_robot: SO101Follower): + self._hf_robot = hf_robot + + def get_normalized_width(self) -> float: + obs = self._hf_robot.get_observation() + return obs["gripper.pos"] / 100.0 + + # def get_parameters(self) -> GripperConfig: ... + # def get_state(self) -> GripperState: ... + + def grasp(self) -> None: + """ + Close the gripper to grasp an object. + """ + self.shut() + + # def is_grasped(self) -> bool: ... + + def open(self) -> None: + """ + Open the gripper to its maximum width. + """ + self.set_normalized_width(1.0) + + def reset(self) -> None: + pass + + def set_normalized_width(self, width: float, _: float = 0) -> None: + """ + Set the gripper width to a normalized value between 0 and 1. + """ + if not (0 <= width <= 1): + msg = f"Width must be between 0 and 1, got {width}." + raise ValueError(msg) + # Convert normalized width to absolute position + abs_width = width * 100.0 + self._hf_robot.send_action({"gripper.pos": abs_width}) + + def shut(self) -> None: + """ + Close the gripper. + """ + self.set_normalized_width(0.0) diff --git a/requirements_dev.txt b/requirements_dev.txt index 67752008..b1ff0539 100644 --- a/requirements_dev.txt +++ b/requirements_dev.txt @@ -8,6 +8,7 @@ build pytest==8.1.1 commitizen~=3.28.0 scikit-build-core>=0.3.3 +poetry-core>=2.1.3 pybind11 mujoco==3.2.6 pin==2.7.0 diff --git a/src/common/IK.cpp b/src/common/IK.cpp index 04b120df..e100024d 100644 --- a/src/common/IK.cpp +++ b/src/common/IK.cpp @@ -46,5 +46,14 @@ std::optional IK::ik(const Pose& pose, const VectorXd& q0, } } +Pose IK::forward(const VectorXd& q0, const Pose& tcp_offset) { + // pose is assumed to be in the robots coordinate frame + this->rl.kin->setPosition(q0); + this->rl.kin->forwardPosition(); + rcs::common::Pose pose = this->rl.kin->getOperationalPosition(0); + // apply the tcp offset + return pose * tcp_offset.inverse(); +} + } // namespace common } // namespace rcs \ No newline at end of file diff --git a/src/common/IK.h b/src/common/IK.h index 3356f33e..4912c2da 100644 --- a/src/common/IK.h +++ b/src/common/IK.h @@ -29,6 +29,7 @@ class IK { IK(const std::string& urdf_path, size_t max_duration_ms = 300); std::optional ik(const Pose& pose, const VectorXd& q0, const Pose& tcp_offset = Pose::Identity()); + Pose forward(const VectorXd& q0, const Pose& tcp_offset = Pose::Identity()); // TODO: set max time }; diff --git a/src/common/Robot.h b/src/common/Robot.h index fce3b6a5..793ca6e5 100644 --- a/src/common/Robot.h +++ b/src/common/Robot.h @@ -18,7 +18,7 @@ struct RobotMetaConfig { Eigen::Matrix joint_limits; }; -enum RobotType { FR3 = 0, UR5e }; +enum RobotType { FR3 = 0, UR5e, SO101 }; enum RobotPlatform { SIMULATION = 0, HARDWARE }; static const std::unordered_map robots_meta_config = @@ -26,7 +26,8 @@ static const std::unordered_map robots_meta_config = {FR3, RobotMetaConfig{ // q_home: - (Vector7d() << 0.0, -M_PI_4, 0.0, -3.0 * M_PI_4, 0.0, M_PI_2, M_PI_4) + (VectorXd(7) << 0.0, -M_PI_4, 0.0, -3.0 * M_PI_4, 0.0, M_PI_2, + M_PI_4) .finished(), // dof: 7, @@ -43,7 +44,7 @@ static const std::unordered_map robots_meta_config = {UR5e, RobotMetaConfig{ // q_home (6‐vector): - (Vector6d() << -0.4488354, -2.02711196, 1.64630026, -1.18999615, + (VectorXd(6) << -0.4488354, -2.02711196, 1.64630026, -1.18999615, -1.57079762, -2.01963249) .finished(), // dof: @@ -55,6 +56,23 @@ static const std::unordered_map robots_meta_config = -2 * M_PI, -1 * M_PI, -2 * M_PI, -2 * M_PI, -2 * M_PI, // high 6‐tuple 2 * M_PI, 2 * M_PI, 1 * M_PI, 2 * M_PI, 2 * M_PI, 2 * M_PI) + .finished()}}, + // -------------- SO101 -------------- + {SO101, + RobotMetaConfig{ + // q_home (5‐vector): + (VectorXd(5) << -9.40612320177057, -99.66130397967824, + 99.9124726477024, 69.96996996996998, -9.095744680851055) + .finished(), + // dof: + 5, + // joint_limits (2×5): + (Eigen::Matrix(2, 5) << + // low 5‐tuple + -100.0, + -100.0, -100.0, -100.0, -100.0, + // high 5‐tuple + 100.0, 100.0, 100.0, 100.0, 100.0) .finished()}}}}; struct RobotConfig { diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index a6f710ba..1aa6387f 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -227,11 +227,14 @@ PYBIND11_MODULE(_core, m) { .def(py::init(), py::arg("urdf_path"), py::arg("max_duration_ms") = 300) .def("ik", &rcs::common::IK::ik, py::arg("pose"), py::arg("q0"), + py::arg("tcp_offset") = rcs::common::Pose::Identity()) + .def("forward", &rcs::common::IK::forward, py::arg("q0"), py::arg("tcp_offset") = rcs::common::Pose::Identity()); py::enum_(common, "RobotType") .value("FR3", rcs::common::RobotType::FR3) .value("UR5e", rcs::common::RobotType::UR5e) + .value("SO101", rcs::common::RobotType::SO101) .export_values(); py::enum_(common, "RobotPlatform") @@ -253,6 +256,7 @@ PYBIND11_MODULE(_core, m) { py::arg("robot_type")); py::class_(common, "RobotConfig") + .def(py::init<>()) .def_readwrite("robot_type", &rcs::common::RobotConfig::robot_type) .def_readwrite("robot_platform", &rcs::common::RobotConfig::robot_platform); @@ -463,17 +467,22 @@ PYBIND11_MODULE(_core, m) { &rcs::sim::SimGripperConfig::collision_geoms) .def_readwrite("collision_geoms_fingers", &rcs::sim::SimGripperConfig::collision_geoms_fingers) - .def_readwrite("joint1", &rcs::sim::SimGripperConfig::joint1) - .def_readwrite("joint2", &rcs::sim::SimGripperConfig::joint2) + .def_readwrite("joint", &rcs::sim::SimGripperConfig::joint) + .def_readwrite("max_joint_width", + &rcs::sim::SimGripperConfig::max_joint_width) + .def_readwrite("min_joint_width", + &rcs::sim::SimGripperConfig::min_joint_width) .def_readwrite("actuator", &rcs::sim::SimGripperConfig::actuator) + .def_readwrite("max_actuator_width", + &rcs::sim::SimGripperConfig::max_actuator_width) + .def_readwrite("min_actuator_width", + &rcs::sim::SimGripperConfig::min_actuator_width) .def("add_id", &rcs::sim::SimGripperConfig::add_id, py::arg("id")); py::class_( sim, "SimGripperState") .def(py::init<>()) .def_readonly("last_commanded_width", &rcs::sim::SimGripperState::last_commanded_width) - .def_readonly("max_unnormalized_width", - &rcs::sim::SimGripperState::max_unnormalized_width) .def_readonly("is_moving", &rcs::sim::SimGripperState::is_moving) .def_readonly("last_width", &rcs::sim::SimGripperState::last_width) .def_readonly("collision", &rcs::sim::SimGripperState::collision); diff --git a/src/sim/SimGripper.cpp b/src/sim/SimGripper.cpp index 0985cac4..ae331cd9 100644 --- a/src/sim/SimGripper.cpp +++ b/src/sim/SimGripper.cpp @@ -21,15 +21,10 @@ SimGripper::SimGripper(std::shared_ptr sim, const SimGripperConfig &cfg) } // this->tendon_id = // mj_name2id(this->sim->m, mjOBJ_TENDON, ("split_" + id).c_str()); - this->joint_id_1 = this->sim->m->jnt_qposadr[mj_name2id( - this->sim->m, mjOBJ_JOINT, this->cfg.joint1.c_str())]; - if (this->joint_id_1 == -1) { - throw std::runtime_error(std::string("No joint named " + this->cfg.joint1)); - } - this->joint_id_2 = this->sim->m->jnt_qposadr[mj_name2id( - this->sim->m, mjOBJ_JOINT, this->cfg.joint2.c_str())]; - if (this->joint_id_2 == -1) { - throw std::runtime_error(std::string("No joint named " + this->cfg.joint2)); + this->joint_id = this->sim->m->jnt_qposadr[mj_name2id( + this->sim->m, mjOBJ_JOINT, this->cfg.joint.c_str())]; + if (this->joint_id == -1) { + throw std::runtime_error(std::string("No joint named " + this->cfg.joint)); } // Collision geoms this->add_collision_geoms(this->cfg.collision_geoms, this->cgeom, false); @@ -87,7 +82,10 @@ void SimGripper::set_normalized_width(double width, double force) { "width must be between 0 and 1, force must be positive"); } this->state.last_commanded_width = width; - this->sim->d->ctrl[this->actuator_id] = (mjtNum)width * this->MAX_WIDTH; + this->sim->d->ctrl[this->actuator_id] = + (mjtNum)width * + (this->cfg.max_actuator_width - this->cfg.min_actuator_width) + + this->cfg.min_actuator_width; // we ignore force for now // this->sim->d->actuator_force[this->gripper_id] = 0; @@ -95,7 +93,9 @@ void SimGripper::set_normalized_width(double width, double force) { double SimGripper::get_normalized_width() { // TODO: maybe we should use the mujoco sensors? Not sure what the difference // is between reading out from qpos and reading from the sensors. - double width = this->sim->d->qpos[this->joint_id_1] / this->MAX_JOINT_WIDTH; + double width = + (this->sim->d->qpos[this->joint_id] - this->cfg.min_actuator_width) / + (this->cfg.max_joint_width - this->cfg.min_actuator_width); // sometimes the joint is slightly outside of the bounds if (width < 0) { width = 0; @@ -142,8 +142,10 @@ bool SimGripper::is_grasped() { bool SimGripper::convergence_callback() { double w = get_normalized_width(); - this->state.is_moving = std::abs(this->state.last_width - w) > - 0.001 / this->MAX_WIDTH; // 1mm tolerance + this->state.is_moving = + std::abs(this->state.last_width - w) > + 0.001 * (this->cfg.max_actuator_width - + this->cfg.min_actuator_width); // 0.1% tolerance this->state.last_width = w; return not this->state.is_moving; } @@ -155,11 +157,9 @@ void SimGripper::shut() { this->set_normalized_width(0); } void SimGripper::m_reset() { this->state = SimGripperState(); - this->state.max_unnormalized_width = this->MAX_WIDTH; // reset state hard - this->sim->d->qpos[this->joint_id_1] = this->MAX_JOINT_WIDTH; - this->sim->d->qpos[this->joint_id_2] = this->MAX_JOINT_WIDTH; - this->sim->d->ctrl[this->actuator_id] = this->MAX_WIDTH; + this->sim->d->qpos[this->joint_id] = this->cfg.max_joint_width; + this->sim->d->ctrl[this->actuator_id] = this->cfg.max_joint_width; } void SimGripper::reset() { this->m_reset(); } diff --git a/src/sim/SimGripper.h b/src/sim/SimGripper.h index c01ba86c..9d4ceb54 100644 --- a/src/sim/SimGripper.h +++ b/src/sim/SimGripper.h @@ -17,14 +17,17 @@ struct SimGripperConfig : common::GripperConfig { double epsilon_inner = 0.005; double epsilon_outer = 0.005; double seconds_between_callbacks = 0.05; // 20 Hz + double max_actuator_width = 255; + double min_actuator_width = 0; + double max_joint_width = 0.04; + double min_joint_width = 0.0; 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::string joint1 = "finger_joint1"; - std::string joint2 = "finger_joint2"; + std::string joint = "finger_joint1"; std::string actuator = "actuator8"; void add_id(const std::string &id) { @@ -37,15 +40,13 @@ struct SimGripperConfig : common::GripperConfig { for (auto &s : this->ignored_collision_geoms) { s = s + "_" + id; } - this->joint1 = this->joint1 + "_" + id; - this->joint2 = this->joint2 + "_" + id; + this->joint = this->joint + "_" + id; this->actuator = this->actuator + "_" + id; } }; struct SimGripperState : common::GripperState { double last_commanded_width = 0; - double max_unnormalized_width = 0; bool is_moving = false; double last_width = 0; bool collision = false; @@ -53,13 +54,10 @@ struct SimGripperState : common::GripperState { class SimGripper : public common::Gripper { private: - const double MAX_WIDTH = 255; - const double MAX_JOINT_WIDTH = 0.04; SimGripperConfig cfg; std::shared_ptr sim; int actuator_id; - int joint_id_1; - int joint_id_2; + int joint_id; SimGripperState state; bool convergence_callback(); bool collision_callback();