chore: collision guard update

Author: juelg

extensions/rcs_xarm7/src/rcs_xarm7/creators.py

@@ -35,6 +35,8 @@ 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,
@@ -55,6 +57,21 @@ 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,
+                collision_guard=collision_guard,
+                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)
 

python/rcs/envs/sim.py

@@ -3,9 +3,9 @@
 
 import gymnasium as gym
 import numpy as np
-from rcs.envs.base import ControlMode, GripperWrapper, MultiRobotWrapper, RobotEnv
+from rcs.envs.base import ControlMode, GripperWrapper, HandWrapper, MultiRobotWrapper, RobotEnv
 from rcs.envs.space_utils import ActObsInfoWrapper
-from rcs.envs.utils import default_sim_robot_cfg
+from rcs.envs.utils import default_sim_robot_cfg, default_sim_tilburg_hand_cfg
 
 import rcs
 from rcs import sim
@@ -213,9 +213,10 @@ def env_from_xml_paths(
         cls,
         env: gym.Env,
         mjmld: str,
-        urdf: str,
+        cg_kinematics_path: str,
         id: str = "0",
         gripper: bool = True,
+        hand: bool = False,
         check_home_collision: bool = True,
         tcp_offset: rcs.common.Pose | None = None,
         control_mode: ControlMode | None = None,
@@ -226,7 +227,7 @@ def env_from_xml_paths(
         # TODO: this needs to support non FR3 robots
         assert isinstance(env.unwrapped, RobotEnv)
         simulation = sim.Sim(mjmld)
-        ik = rcs.common.RL(urdf, max_duration_ms=300)
+        ik = rcs.common.Pin(cg_kinematics_path, "attachment_site", False)
         cfg = default_sim_robot_cfg(mjmld, id)
         cfg.realtime = False
         if tcp_offset is not None:
@@ -250,6 +251,13 @@ def env_from_xml_paths(
             fh = sim.SimGripper(simulation, gripper_cfg)
             c_env = GripperWrapper(c_env, fh)
             c_env = GripperWrapperSim(c_env, fh)
+        if hand:
+            hand_cfg = default_sim_tilburg_hand_cfg()
+            # hand_cfg.add_id(id)
+            th = sim.SimTilburgHand(simulation, hand_cfg)
+            c_env = HandWrapper(c_env, th)
+            c_env = HandWrapperSim(c_env, th)
+
         return cls(
             env=env,
             simulation=simulation,