Merge pull request #165 from utn-mi/nava/digit_grippers

feat(sim): support for camera robot lab scene

Author: juelg

.gitignore

@@ -8,7 +8,7 @@ __pycache__
 .ruff_cache
 .mypy_cache
 dist
-.venv
+*venv*
 config.yaml
 MUJOCO_LOG.TXT
 src/pybind/mujoco

CMakeLists.txt

@@ -98,7 +98,7 @@ FetchContent_MakeAvailable(libfranka rl pybind11)
 if (${INCLUDE_UTN_MODELS})
     if (DEFINED GITLAB_MODELS_TOKEN)
         include(download_scenes)
-        set(ref b8234983a5e35e222c955f9145ac4cd129827a8e)
+        set(ref bfdd70ae189ed3a925bf1c16d29162e847a21d56)
         FetchContent_Declare(
             scenes
             URL         "https://gitos.rrze.fau.de/api/v4/projects/1100/repository/archive?path=scenes&sha=${ref}"

python/examples/env_cartesian_control.py

@@ -3,14 +3,13 @@
 from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager
 from rcsss.control.utils import load_creds_fr3_desk
 from rcsss.envs.base import ControlMode, RelativeTo, RobotInstance
-from rcsss.envs.factories import (
+from rcsss.envs.factories import fr3_hw_env, fr3_sim_env
+from rcsss.envs.utils import (
     default_fr3_hw_gripper_cfg,
     default_fr3_hw_robot_cfg,
     default_fr3_sim_gripper_cfg,
     default_fr3_sim_robot_cfg,
     default_mujoco_cameraset_cfg,
-    fr3_hw_env,
-    fr3_sim_env,
 )
 
 logger = logging.getLogger(__name__)

python/examples/env_joint_control.py

@@ -4,14 +4,13 @@
 from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager
 from rcsss.control.utils import load_creds_fr3_desk
 from rcsss.envs.base import ControlMode, RelativeTo, RobotInstance
-from rcsss.envs.factories import (
+from rcsss.envs.factories import fr3_hw_env, fr3_sim_env
+from rcsss.envs.utils import (
     default_fr3_hw_gripper_cfg,
     default_fr3_hw_robot_cfg,
     default_fr3_sim_gripper_cfg,
     default_fr3_sim_robot_cfg,
     default_mujoco_cameraset_cfg,
-    fr3_hw_env,
-    fr3_sim_env,
 )
 
 logger = logging.getLogger(__name__)

python/examples/grasp_demo.py

@@ -32,6 +32,7 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in
         for i in range(num_waypoints + 1):
             t = i / (num_waypoints + 1)
             waypoints.append(start_pose.interpolate(end_pose, t))
+        waypoints.append(end_pose)
         return waypoints
 
     def step(self, action: dict) -> dict:
@@ -81,8 +82,15 @@ def pickup(self, geom_name: str):
 
 
 def main():
-    env = gym.make("rcs/SimplePickUpSim-v0", render_mode="human", delta_actions=True)
+    # compatilbe with rcs/SimplePickUpSimDigitHand-v0 and rcs/SimplePickUpSim-v0
+    env = gym.make(
+        "rcs/SimplePickUpSimDigitHand-v0",
+        render_mode="human",
+        delta_actions=True,
+    )
     env.reset()
+    print(env.unwrapped.robot.get_cartesian_position().translation())  # type: ignore
+    # assert False
     controller = PickUpDemo(env)
     controller.pickup("yellow_box_geom")
 

python/examples/grasp_demo_lab.py

@@ -0,0 +1,96 @@
+import logging
+from typing import Any, cast
+
+import gymnasium as gym
+import mujoco
+import numpy as np
+from rcsss._core.common import Pose
+from rcsss.envs.base import FR3Env, GripperWrapper
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+
+class PickUpDemo:
+    def __init__(self, env: gym.Env):
+        self.env = env
+        self.unwrapped: FR3Env = cast(FR3Env, self.env.unwrapped)
+        self.home_pose = self.unwrapped.robot.get_cartesian_position()
+
+    def _action(self, pose: Pose, gripper: float) -> dict[str, Any]:
+        return {"xyzrpy": pose.xyzrpy(), "gripper": gripper}
+
+    def get_object_pose(self, geom_name) -> Pose:
+        model = self.env.get_wrapper_attr("sim").model
+        data = self.env.get_wrapper_attr("sim").data
+
+        geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, geom_name)
+        obj_pose_world_coordinates = Pose(
+            translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3)
+        )
+        return self.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates)
+
+    def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]:
+        waypoints = []
+        for i in range(num_waypoints + 1):
+            t = i / (num_waypoints + 1)
+            waypoints.append(start_pose.interpolate(end_pose, t))
+        return waypoints
+
+    def step(self, action: dict) -> dict:
+        return self.env.step(action)[0]
+
+    def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 200) -> list[Pose]:
+        end_eff_pose = self.unwrapped.robot.get_cartesian_position()
+        goal_pose = self.get_object_pose(geom_name=geom_name)
+        goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0]))
+        return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints)
+
+    def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict:
+        for i in range(1, len(waypoints)):
+            # calculate delta action
+            delta_action = waypoints[i] * waypoints[i - 1].inverse()
+            obs = self.step(self._action(delta_action, gripper))
+        return obs
+
+    def approach(self, geom_name: str):
+        waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=50)
+        self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN)
+
+    def grasp(self, geom_name: str):
+
+        waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0, num_waypoints=50)
+        self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN)
+
+        self.step(self._action(Pose(), GripperWrapper.BINARY_GRIPPER_CLOSED))
+
+        waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=50)
+        self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED)
+
+    def move_home(self):
+        end_eff_pose = self.unwrapped.robot.get_cartesian_position()
+        waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=50)
+        self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED)
+
+    def pickup(self, geom_name: str):
+        self.approach(geom_name)
+        self.grasp(geom_name)
+        self.move_home()
+
+
+def main():
+    env = gym.make(
+        "rcs/LabPickUpSimDigitHand-v0",
+        render_mode="human",
+        delta_actions=True,
+        cam_robot_joints=np.array(
+            [-0.78452318, -1.18096017, 1.75158399, -1.0718541, -0.11207275, 1.01050546, 2.47343638]
+        ),
+    )
+    env.reset()
+    controller = PickUpDemo(env)
+    controller.pickup("yellow_box_geom")
+
+
+if __name__ == "__main__":
+    main()

python/rcsss/__init__.py

@@ -6,7 +6,12 @@
 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 (
+    FR3LabPickUpSimDigitHand,
+    FR3Real,
+    FR3SimplePickUpSim,
+    FR3SimplePickUpSimDigitHand,
+)
 
 # available mujoco scenes
 scenes = {
@@ -21,6 +26,16 @@
     id="rcs/SimplePickUpSim-v0",
     entry_point=FR3SimplePickUpSim(),
 )
+
+register(
+    id="rcs/SimplePickUpSimDigitHand-v0",
+    entry_point=FR3SimplePickUpSimDigitHand(),
+)
+register(
+    id="rcs/LabPickUpSimDigitHand-v0",
+    entry_point=FR3LabPickUpSimDigitHand(),
+)
+
 register(
     id="rcs/FR3Real-v0",
     entry_point=FR3Real(),

python/rcsss/_core/sim.pyi

@@ -86,7 +86,9 @@ class FHState(rcsss._core.common.GState):
     def max_unnormalized_width(self) -> float: ...
 
 class FR3(rcsss._core.common.Robot):
-    def __init__(self, sim: Sim, id: str, ik: rcsss._core.common.IK) -> None: ...
+    def __init__(
+        self, sim: Sim, id: str, ik: rcsss._core.common.IK, register_convergence_callback: bool = True
+    ) -> None: ...
     def get_parameters(self) -> FR3Config: ...
     def get_state(self) -> FR3State: ...
     def set_joints_hard(self, q: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]) -> None: ...

python/rcsss/control/keyboard_control.py

@@ -7,11 +7,8 @@
 from rcsss.control.fr3_desk import FCI, Desk
 from rcsss.control.utils import load_creds_fr3_desk
 from rcsss.envs.base import ControlMode, RelativeTo
-from rcsss.envs.factories import (
-    default_fr3_hw_gripper_cfg,
-    default_fr3_hw_robot_cfg,
-    fr3_hw_env,
-)
+from rcsss.envs.factories import fr3_hw_env
+from rcsss.envs.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg
 
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)

python/rcsss/control/vive.py

@@ -15,14 +15,13 @@
     RelativeTo,
     RobotInstance,
 )
-from rcsss.envs.factories import (
+from rcsss.envs.factories import fr3_hw_env, fr3_sim_env
+from rcsss.envs.utils import (
     default_fr3_hw_gripper_cfg,
     default_fr3_hw_robot_cfg,
     default_fr3_sim_gripper_cfg,
     default_fr3_sim_robot_cfg,
     default_mujoco_cameraset_cfg,
-    fr3_hw_env,
-    fr3_sim_env,
 )
 
 # import matplotlib.pyplot as plt