refactor: removed get tcp offset from mjcf utility

Author: juelg

README.md

@@ -65,8 +65,8 @@ from rcs import sim
 from rcs._core.sim import CameraType
 from rcs.camera.sim import SimCameraConfig, SimCameraSet
 from time import sleep
-simulation = sim.Sim(rcs.scenes["fr3_empty_world"]["mjb"])
-urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
+simulation = sim.Sim(rcs.scenes["fr3_empty_world"].mjb)
+urdf_path = rcs.scenes["fr3_empty_world"].urdf
 ik = rcs.common.RL(str(urdf_path))
 cfg = sim.SimRobotConfig()
 cfg.add_id("0")

examples/xarm7_env_cartesian_control.py

@@ -2,7 +2,6 @@
 
 from rcs.envs.base import ControlMode, RelativeTo
 from rcs.envs.creators import SimEnvCreator
-from rcs.envs.utils import get_tcp_offset
 
 import rcs
 from rcs import sim
@@ -36,7 +35,6 @@ def main():
     robot_cfg.robot_type = rcs.common.RobotType.XArm7
     robot_cfg.attachment_site = "attachment_site"
     robot_cfg.arm_collision_geoms = []
-    robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes["xarm7_empty_world"].mjcf_scene)
     robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb
     robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot
     env_rel = SimEnvCreator()(

examples/xarm7_env_joint_control.py

@@ -3,7 +3,6 @@
 import numpy as np
 from rcs.envs.base import ControlMode, RelativeTo
 from rcs.envs.creators import SimEnvCreator
-from rcs.envs.utils import get_tcp_offset
 
 import rcs
 from rcs import sim
@@ -36,7 +35,6 @@ def main():
     robot_cfg.robot_type = rcs.common.RobotType.XArm7
     robot_cfg.attachment_site = "attachment_site"
     robot_cfg.arm_collision_geoms = []
-    robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes["xarm7_empty_world"].mjcf_scene)
     robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb
     robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot
     env_rel = SimEnvCreator()(

extensions/rcs_fr3/README.md

@@ -19,7 +19,7 @@ from rcs_fr3._core import hw
 from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk
 user, pw = load_creds_fr3_desk()
 with FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False):
-    urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
+    urdf_path = rcs.scenes["fr3_empty_world"].urdf
     ik = rcs.common.RL(str(urdf_path))
     robot = hw.FR3(ROBOT_IP, ik)
     robot_cfg = FR3Config()

extensions/rcs_xarm7/src/rcs_xarm7/creators.py

@@ -37,8 +37,6 @@ def __call__(  # type: ignore
         control_mode: ControlMode,
         ip: str,
         calibration_dir: PathLike | str | None = None,
-        collision_guard: str | PathLike | None = None,
-        cg_kinematics_path: str | PathLike | None = None,
         camera_set: HardwareCameraSet | None = None,
         hand_cfg: THConfig | None = None,
         max_relative_movement: float | tuple[float, float] | None = None,
@@ -47,7 +45,6 @@ def __call__(  # type: ignore
         if isinstance(calibration_dir, str):
             calibration_dir = Path(calibration_dir)
         robot = XArm7(ip=ip)
-        # env: gym.Env = RobotEnv(robot, ControlMode.JOINTS, home_on_reset=True)
         env: gym.Env = RobotEnv(robot, control_mode, home_on_reset=True)
 
         if camera_set is not None:
@@ -59,20 +56,6 @@ def __call__(  # type: ignore
             hand = TilburgHand(cfg=hand_cfg, verbose=True)
             env = HandWrapper(env, hand, True)
 
-        # if collision_guard:
-        #     env = CollisionGuard.env_from_xml_paths(
-        #         env=env,
-        #         cg_kinematics_path=cg_kinematics_path,
-        #         hand=True,
-        #         gripper=False,
-        #         check_home_collision=False,
-        #         control_mode=control_mode,
-        #         tcp_offset=rcs.common.Pose(),
-        #         sim_gui=True,
-        #         truncate_on_collision=True,
-        #         id="",
-        #     )
-
         if max_relative_movement is not None:
             env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to)
 

extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py

@@ -4,7 +4,7 @@
 from rcs._core.common import RobotPlatform
 from rcs.envs.base import ControlMode, RelativeTo
 from rcs.envs.creators import SimEnvCreator
-from rcs.envs.utils import default_sim_tilburg_hand_cfg, get_tcp_offset
+from rcs.envs.utils import default_sim_tilburg_hand_cfg
 from rcs.hand.tilburg_hand import THConfig
 from rcs_xarm7.creators import RCSXArm7EnvCreator
 
@@ -43,7 +43,6 @@ def sim_env():
     robot_cfg.robot_type = rcs.common.RobotType.XArm7
     robot_cfg.attachment_site = "attachment_site"
     robot_cfg.arm_collision_geoms = []
-    robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes["xarm7_empty_world"].mjcf_scene)
     robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb
     robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot
     env_rel = SimEnvCreator()(
@@ -72,8 +71,6 @@ def main():
             hand_cfg=hand_cfg,
             relative_to=RelativeTo.LAST_STEP,
             max_relative_movement=np.deg2rad(3),
-            # collision_guard=rcs.scenes["xarm7_empty_world"]["mjcf_scene"],
-            # cg_kinematics_path=rcs.scenes["xarm7_empty_world"]["mjcf_robot"],
         )
     else:
         env_rel = sim_env()

extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py

@@ -5,7 +5,7 @@
 from rcs._core.common import RobotPlatform
 from rcs.envs.base import ControlMode, RelativeTo
 from rcs.envs.creators import SimEnvCreator
-from rcs.envs.utils import default_sim_tilburg_hand_cfg, get_tcp_offset
+from rcs.envs.utils import default_sim_tilburg_hand_cfg
 from rcs.hand.tilburg_hand import THConfig
 from rcs_xarm7.creators import RCSXArm7EnvCreator
 
@@ -44,7 +44,6 @@ def sim_env():
     robot_cfg.robot_type = rcs.common.RobotType.XArm7
     robot_cfg.attachment_site = "attachment_site"
     robot_cfg.arm_collision_geoms = []
-    robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes["xarm7_empty_world"].mjcf_scene)
     env_rel = SimEnvCreator()(
         robot_cfg=robot_cfg,
         control_mode=ControlMode.JOINTS,

extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py

@@ -5,7 +5,6 @@
 from rcs._core.common import RobotPlatform
 from rcs.envs.base import ControlMode, RelativeTo
 from rcs.envs.creators import SimEnvCreator
-from rcs.envs.utils import get_tcp_offset
 from rcs_xarm7.creators import RCSXArm7EnvCreator
 
 import rcs
@@ -52,7 +51,6 @@ def main():
         robot_cfg.robot_type = rcs.common.RobotType.XArm7
         robot_cfg.attachment_site = "attachment_site"
         robot_cfg.arm_collision_geoms = []
-        robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes["xarm7_empty_world"].mjcf_scene)
         robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb
         robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot
         env_rel = SimEnvCreator()(

python/rcs/envs/utils.py

@@ -1,9 +1,6 @@
 import logging
 from os import PathLike
-from pathlib import Path
 
-import mujoco as mj
-import numpy as np
 from digit_interface import Digit
 from rcs._core.common import BaseCameraConfig
 from rcs._core.sim import CameraType, SimCameraConfig
@@ -19,7 +16,6 @@
 
 def default_sim_robot_cfg(scene: str = "fr3_empty_world", idx: str = "0") -> sim.SimRobotConfig:
     robot_cfg = rcs.sim.SimRobotConfig()
-    robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes[scene].mjcf_scene)
     robot_cfg.realtime = False
     robot_cfg.robot_type = rcs.scenes[scene].robot_type
     robot_cfg.add_id(idx)
@@ -73,34 +69,3 @@ def default_mujoco_cameraset_cfg() -> dict[str, SimCameraConfig]:
         ),
         # "bird_eye": SimCameraConfig(identifier="bird_eye_cam", type=int(CameraType.fixed), frame_rate=10, resolution_width=256, resolution_height=256),
     }
-
-
-def get_tcp_offset(mjcf: str | Path) -> rcs.common.Pose:
-    """Reads out tcp offset set in mjcf file.
-
-    Convention: The tcp offset is stored in the model as a numeric attribute named "tcp_offset".
-
-    Args:
-        mjcf (str | PathLike): Path to the mjcf file.
-
-    Returns:
-        rcs.common.Pose: The tcp offset.
-    """
-    mjcf = Path(mjcf)
-    if mjcf.suffix in (".xml", ".mjcf"):
-        model = mj.MjModel.from_xml_path(str(mjcf))
-    elif mjcf.suffix == ".mjb":
-        model = mj.MjModel.from_binary_path(str(mjcf))
-    else:
-        msg = f"Expected .mjb, .mjcf or.xml, got {mjcf.suffix} and {mjcf}"
-        raise AssertionError(msg)
-    try:
-        tcp_offset_translation = np.array(model.numeric("tcp_offset_translation").data)
-        tcp_offset_rotation_matrix = np.array(model.numeric("tcp_offset_rotation_matrix").data)
-        return rcs.common.Pose(
-            translation=tcp_offset_translation, rotation=tcp_offset_rotation_matrix.reshape((3, 3))  # type: ignore
-        )
-    except KeyError:
-        msg = "No tcp offset found in the model. Using an identity transform as tcp offset."
-        logging.warning(msg)
-    return rcs.common.Pose()