Nava/digit gripper envs

python/examples/grasp_demo.py

@@ -24,7 +24,10 @@ def get_object_pose(self, geom_name) -> Pose:
         data = self.env.get_wrapper_attr("sim").data
 
         geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, geom_name)
-        return Pose(translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3))
+        obj_pose_world_coordinates = Pose(translation=data.geom_xpos[geom_id],
+                                          rotation=data.geom_xmat[geom_id].reshape(3, 3))
+        obj_pose_robot_coordinates = self.env.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates)
+        return obj_pose_robot_coordinates
 
     def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]:
         waypoints = []
@@ -36,11 +39,11 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in
     def step(self, action: np.ndarray) -> dict:
         re = self.env.step(action)
         s: FR3State = self.env.unwrapped.robot.get_state()
-        return re[0]
+        return re
 
     def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 20) -> list[Pose]:
         end_eff_pose = self.env.unwrapped.robot.get_cartesian_position()
-
+        # @todo goal_pose is with respective to the robot base which is different in empty world scene and lab scene?
         goal_pose = self.get_object_pose(geom_name=geom_name)
         # goal pose is above the object and gripper coordinate must flip z-axis (end effector base rotation is [1, 0, 0, 0])
         # be careful we define identity quaternion as as [0, 0, 0, 1]
@@ -55,6 +58,14 @@ def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.
             # calculate delta action
             delta_action = waypoints[i] * waypoints[i - 1].inverse()
             obs = self.step(self._action(delta_action, gripper))
+            ik_success = obs[-1]['ik_success']
+            if not obs[-1]['ik_success']:
+                trans_source, rot_source = waypoints[i - 1].translation(), waypoints[i - 1].rotation_rpy().as_vector()
+                trans_dest, rot_des = waypoints[i].translation(), waypoints[i].rotation_rpy().as_vector()
+                print(f"ik success: {ik_success} when attempting to move from trans: {trans_source}, rot: {rot_source}\n"
+                      f" to trans: {trans_dest} rot: {rot_des}!")
+                print(f"aborting motion!")
+                exit(-1)
         return obs
 
     def approach(self, geom_name: str):
@@ -83,7 +94,10 @@ def pickup(self, geom_name: str):
 
 
 def main():
-    env = gym.make("rcs/SimplePickUpSim-v0", render_mode="human", delta_actions=True)
+    """
+    available envs: "rcs/SimplePickUpSim-v0", "rcs/FR3LabPickUpSimDigitHand-v0", "rcs/SimplePickUpSimDigitHand-v0
+    """
+    env = gym.make("rcs/SimplePickUpSimDigitHand-v0", render_mode="human", delta_actions=True)
     env.reset()
     controller = PickUpDemo(env)
     controller.pickup("yellow_box_geom")

python/rcsss/__init__.py

@@ -6,7 +6,7 @@
 from gymnasium import register
 from rcsss import camera, control, envs, sim
 from rcsss._core import __version__, common, hw
-from rcsss.envs.factories import FR3Real, FR3SimplePickUpSim
+from rcsss.envs.factories import FR3Real, FR3SimplePickUpSim, FR3SimplePickUpSimDigitHand, FR3LabPickUpSimDigitHand
 
 # available mujoco scenes
 scenes = {
@@ -21,6 +21,16 @@
     id="rcs/SimplePickUpSim-v0",
     entry_point=FR3SimplePickUpSim(),
 )
+
+register(
+    id="rcs/SimplePickUpSimDigitHand-v0",
+    entry_point=FR3SimplePickUpSimDigitHand(),
+)
+register(
+    id="rcs/FR3LabPickUpSimDigitHand-v0",
+    entry_point=FR3LabPickUpSimDigitHand(),
+)
+
 register(
     id="rcs/FR3Real-v0",
     entry_point=FR3Real(),

python/rcsss/envs/factories.py

@@ -29,9 +29,9 @@
     FR3Sim,
     FR3SimplePickUpSimSuccessWrapper,
     RandomCubePos,
-    SimWrapper,
+    SimWrapper, RandomCubePosLab,
 )
-
+import numpy as np
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
 
@@ -157,6 +157,12 @@ def default_fr3_sim_robot_cfg():
     cfg.realtime = False
     return cfg
 
+def digit_fr3_sim_robot_cfg():
+    cfg = sim.FR3Config()
+    pose_offset = rcsss.common.Pose(translation=np.array([0, 0, 0.0466]))
+    cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) * pose_offset
+    cfg.realtime = False
+    return cfg
 
 def default_fr3_sim_gripper_cfg():
     return sim.FHConfig()
@@ -327,3 +333,92 @@ def __call__(  # type: ignore
             env_rel.get_wrapper_attr("sim").open_gui()
 
         return env_rel
+
+
+class FR3SimplePickUpSimDigitHand(EnvCreator):
+    def __call__(  # type: ignore
+            self,
+            render_mode: str = "human",
+            control_mode: str = "xyzrpy",
+            resolution: tuple[int, int] | None = None,
+            frame_rate: int = 10,
+            delta_actions: bool = True,
+    ) -> gym.Env:
+        if resolution is None:
+            resolution = (256, 256)
+
+        cameras = {"eye-in-hand_0": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed))}
+
+        camera_cfg = SimCameraSetConfig(
+            cameras=cameras,
+            resolution_width=resolution[0],
+            resolution_height=resolution[1],
+            frame_rate=frame_rate,
+            physical_units=True,
+        )
+
+        env_rel = fr3_sim_env(
+            control_mode=(
+                ControlMode.CARTESIAN_TRPY
+                if control_mode == "xyzrpy"
+                else ControlMode.JOINTS if control_mode == "joints" else ControlMode.CARTESIAN_TQuart
+            ),
+            robot_cfg=digit_fr3_sim_robot_cfg(),
+            collision_guard=False,
+            gripper_cfg=default_fr3_sim_gripper_cfg(),
+            camera_set_cfg=camera_cfg,
+            max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None,
+            relative_to=RelativeTo.LAST_STEP,
+            mjcf="fr3_simple_pick_up_digit_hand",
+            sim_wrapper=RandomCubePos,
+        )
+        env_rel = FR3SimplePickUpSimSuccessWrapper(env_rel)
+        if render_mode == "human":
+            env_rel.get_wrapper_attr("sim").open_gui()
+
+        return env_rel
+
+
+class FR3LabPickUpSimDigitHand(EnvCreator):
+    def __call__(  # type: ignore
+            self,
+            render_mode: str = "human",
+            control_mode: str = "xyzrpy",
+            resolution: tuple[int, int] | None = None,
+            frame_rate: int = 10,
+            delta_actions: bool = True,
+    ) -> gym.Env:
+        if resolution is None:
+            resolution = (256, 256)
+
+        cameras = {"eye-in-hand_0": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed)),
+                   "eye-in-hand_1": SimCameraConfig(identifier="eye-in-hand_1", type=int(CameraType.fixed))}
+
+        camera_cfg = SimCameraSetConfig(
+            cameras=cameras,
+            resolution_width=resolution[0],
+            resolution_height=resolution[1],
+            frame_rate=frame_rate,
+            physical_units=True,
+        )
+
+        env_rel = fr3_sim_env(
+            control_mode=(
+                ControlMode.CARTESIAN_TRPY
+                if control_mode == "xyzrpy"
+                else ControlMode.JOINTS if control_mode == "joints" else ControlMode.CARTESIAN_TQuart
+            ),
+            robot_cfg=digit_fr3_sim_robot_cfg(),
+            collision_guard=False,
+            gripper_cfg=default_fr3_sim_gripper_cfg(),
+            camera_set_cfg=camera_cfg,
+            max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None,
+            relative_to=RelativeTo.LAST_STEP,
+            mjcf="lab_simple_pick_up_digit_hand",
+            sim_wrapper=RandomCubePosLab,
+        )
+        env_rel = FR3SimplePickUpSimSuccessWrapper(env_rel)
+        if render_mode == "human":
+            env_rel.get_wrapper_attr("sim").open_gui()
+
+        return env_rel

python/rcsss/envs/sim.py

@@ -188,6 +188,23 @@ def reset(
         return obs, info
 
 
+class RandomCubePosLab(SimWrapper):
+    """Wrapper to randomly place cube in the FR3SimplePickUpSim environment."""
+
+    def reset(
+        self, seed: int | None = None, options: dict[str, Any] | None = None
+    ) -> tuple[dict[str, Any], dict[str, Any]]:
+        obs, info = super().reset(seed=seed, options=options)
+
+        iso_cube = [0.0, 0.0, 0.826]
+        pos_x = iso_cube[0] + np.random.random() * 0.2 + 0.1
+        pos_y = iso_cube[1] + np.random.random() * 0.2 + 0.1
+        pos_z = 0.826
+        self.sim.data.joint("yellow-box-joint").qpos[:3] = [pos_x, pos_y, pos_z]
+
+        return obs, info
+
+
 class FR3SimplePickUpSimSuccessWrapper(gym.Wrapper):
     """Wrapper to check if the cube is successfully picked up in the FR3SimplePickUpSim environment."""