chore: unifed mjcf robot name

Author: juelg

assets/scenes/fr3_empty_world/fr3_0.xml assets/scenes/fr3_empty_world/robot.xmlassets/scenes/fr3_empty_world/fr3_0.xml renamed to assets/scenes/fr3_empty_world/robot.xml

@@ -1,6 +1,6 @@
 <mujoco model="fr3 scene">
   <include file="fr3_common.xml"/>
-  <include file="fr3_0.xml"/>
+  <include file="robot.xml"/>
 
   <statistic center="0.3 0 0.4" extent="1"/>
 

assets/scenes/fr3_empty_world/scene.xml

@@ -1,6 +1,6 @@
 <mujoco model="simple pick up task">
   <include file="fr3_common.xml" />
-  <include file="fr3_0.xml" />
+  <include file="robot.xml" />
 
   <statistic center="0.3 0 0.4" extent="1" />
 

…sets/scenes/fr3_simple_pick_up/fr3_0.xml …sets/scenes/fr3_simple_pick_up/robot.xmlassets/scenes/fr3_simple_pick_up/fr3_0.xml renamed to assets/scenes/fr3_simple_pick_up/robot.xml assets/scenes/fr3_simple_pick_up/fr3_0.xml renamed to assets/scenes/fr3_simple_pick_up/robot.xml

@@ -30,11 +30,6 @@ foreach(scene_dir ${scene_dirs})
     # Install scene files
     install(FILES ${scene_mjb} DESTINATION rcs/scenes/${scene_name} COMPONENT python_package)
 
-    # Install URDF files
-    file(GLOB urdfs ${scene_dir}/*.urdf)
-    foreach(urdf ${urdfs})
-        install(FILES ${urdf} DESTINATION rcs/scenes/${scene_name} COMPONENT python_package)
-    endforeach()
 endforeach()
 
 # Create a custom target that depends on all generated .mjb files

assets/scenes/fr3_simple_pick_up/scene.xml

@@ -12,6 +12,8 @@
 from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk
 from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg
 
+import rcs
+
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
 
@@ -64,19 +66,22 @@ def main():
         else:
             env_rel = SimEnvCreator()(
                 control_mode=ControlMode.CARTESIAN_TQuat,
-                robot_cfg=default_sim_robot_cfg(),
+                robot_cfg=default_sim_robot_cfg(mjcf=rcs.scenes["fr3_empty_world"]["mjcf_scene"]),
                 collision_guard=False,
                 gripper_cfg=default_sim_gripper_cfg(),
                 cameras=default_mujoco_cameraset_cfg(),
                 max_relative_movement=0.5,
                 relative_to=RelativeTo.LAST_STEP,
+                mjcf=rcs.scenes["fr3_empty_world"]["mjb"],
+                robot_kinematics_path=rcs.scenes["fr3_empty_world"]["mjcf_robot"],
+                kinematics_frame_id="attachment_site_0",
             )
             env_rel.get_wrapper_attr("sim").open_gui()
 
         env_rel.reset()
         print(env_rel.unwrapped.robot.get_cartesian_position())  # type: ignore
 
-        for _ in range(10):
+        for _ in range(100):
             for _ in range(10):
                 # move 1cm in x direction (forward) and close gripper
                 act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0}

cmake/compile_scenes.cmake

@@ -13,6 +13,8 @@
 from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk
 from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg
 
+import rcs
+
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
 
@@ -66,15 +68,17 @@ def main():
             env_rel = SimEnvCreator()(
                 control_mode=ControlMode.JOINTS,
                 collision_guard=False,
-                robot_cfg=default_sim_robot_cfg(),
+                robot_cfg=default_sim_robot_cfg(mjcf=rcs.scenes["fr3_empty_world"]["mjcf_scene"]),
                 gripper_cfg=default_sim_gripper_cfg(),
                 cameras=default_mujoco_cameraset_cfg(),
                 max_relative_movement=np.deg2rad(5),
                 relative_to=RelativeTo.LAST_STEP,
+                mjcf=rcs.scenes["fr3_empty_world"]["mjb"],
+                robot_kinematics_path=rcs.scenes["fr3_empty_world"]["mjcf_robot"],
             )
             env_rel.get_wrapper_attr("sim").open_gui()
 
-        for _ in range(10):
+        for _ in range(100):
             obs, info = env_rel.reset()
             for _ in range(10):
                 # sample random relative action and execute it

examples/fr3_env_cartesian_control.py

@@ -13,7 +13,9 @@
 scenes: dict[str, dict[str, pathlib.Path]] = {
     path.stem: {
         "mjb": path / "scene.mjb",
-        "urdf": path / "fr3.urdf",
+        "urdf": f"assets/scenes/{path.stem}/robot.urdf",
+        "mjcf_scene": f"assets/scenes/{path.stem}/scene.xml",
+        "mjcf_robot": f"assets/scenes/{path.stem}/robot.xml",
     }
     for path in (pathlib.Path(site.getsitepackages()[0]) / "rcs" / "scenes").glob("*")
 }

examples/fr3_env_joint_control.py

@@ -43,13 +43,14 @@ def __call__(  # type: ignore
         self,
         control_mode: ControlMode,
         robot_cfg: rcs.sim.SimRobotConfig,
+        robot_kinematics_path: str | PathLike,
+        mjcf: str | PathLike,
+        kinematics_frame_id: str = "attachment_site",
         collision_guard: bool = False,
         gripper_cfg: rcs.sim.SimGripperConfig | None = None,
         cameras: dict[str, SimCameraConfig] | None = None,
         max_relative_movement: float | tuple[float, float] | None = None,
         relative_to: RelativeTo = RelativeTo.LAST_STEP,
-        urdf_path: str | PathLike | None = None,
-        mjcf: str | PathLike = "fr3_empty_world",
         sim_wrapper: Type[SimWrapper] | None = None,
     ) -> gym.Env:
         """
@@ -64,28 +65,28 @@ def __call__(  # type: ignore
             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.
+            kinematics_frame_id (str): Frame name of the flange. Defaults to "attachment_site". Note: if name doesnt exist
+                then pinocchio just segfaults.
             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.
+            robot_kinematics_path (str | PathLike | None): Path to either urdf or mjcf file that describes the robot kinematic.
+                This is used for ik.
             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.
         """
-        if urdf_path is None:
-            urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
         if mjcf not in rcs.scenes:
             logger.info("mjcf not found as key in scenes, interpreting mjcf as path the mujoco scene xml")
         if mjcf in rcs.scenes:
             assert isinstance(mjcf, str)
             mjcf = rcs.scenes[mjcf]["mjb"]
         simulation = sim.Sim(mjcf)
 
-        # ik = rcs.common.Pin("/home/juelg/code/internal_rcs/assets/scenes/fr3_empty_world/fr3_0.xml", "attachment_site_0", urdf=False)
-        # ik = rcs.common.Pin(str(urdf_path))
-        ik = rcs.common.RL(str(urdf_path))
+        ik = rcs.common.Pin(
+            str(robot_kinematics_path), kinematics_frame_id, urdf=robot_kinematics_path.endswith(".urdf")
+        )
 
         robot = rcs.sim.SimRobot(simulation, ik, robot_cfg)
         env: gym.Env = RobotEnv(robot, control_mode)
@@ -106,7 +107,7 @@ def __call__(  # type: ignore
             env = CollisionGuard.env_from_xml_paths(
                 env,
                 str(rcs.scenes.get(str(mjcf), mjcf)),
-                str(urdf_path),
+                str(robot_kinematics_path),
                 gripper=gripper_cfg is not None,
                 check_home_collision=False,
                 control_mode=control_mode,
@@ -132,14 +133,15 @@ def __call__(  # type: ignore
 
         env_rel = SimEnvCreator()(
             control_mode=control_mode,
-            robot_cfg=default_sim_robot_cfg(mjcf),
+            robot_cfg=default_sim_robot_cfg(rcs.scenes[mjcf]["mjcf_scene"]),
             collision_guard=False,
             gripper_cfg=default_sim_gripper_cfg(),
             cameras=cameras,
             max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None,
             relative_to=RelativeTo.LAST_STEP,
-            mjcf=mjcf,
+            mjcf=rcs.scenes[mjcf]["mjcf_scene"],
             sim_wrapper=RandomCubePos,
+            robot_kinematics_path=rcs.scenes[mjcf]["mjcf_robot"],
         )
         env_rel = PickCubeSuccessWrapper(env_rel)
         if render_mode == "human":

python/rcs/__init__.py

@@ -202,6 +202,7 @@ def env_from_xml_paths(
         sim_gui: bool = True,
         truncate_on_collision: bool = True,
     ) -> "CollisionGuard":
+        # TODO: remove urdf and use mjcf
         # TODO: this needs to support non FR3 robots
         assert isinstance(env.unwrapped, RobotEnv)
         simulation = sim.Sim(mjmld)