Merge branch 'master' into scene_mgmt

Author: pkrack

CMakeLists.txt

@@ -38,6 +38,8 @@ set(RL_BUILD_DEMOS OFF)
 set(RL_BUILD_RL_SG OFF)
 set(RL_BUILD_TESTS OFF)
 set(RL_BUILD_EXTRAS OFF)
+set(BUILD_PYTHON_INTERFACE OFF)
+set(BUILD_DOCUMENTATION OFF)
 
 option(INCLUDE_UTN_MODELS "Whether to include the private UTN models. Requires GITLAB_MODELS_TOKEN to be set to a valid token wit read_api permissions" OFF)
 
@@ -51,7 +53,7 @@ find_package(MuJoCo REQUIRED)
 FetchContent_Declare(
     libfranka
     GIT_REPOSITORY https://github.com/frankaemika/libfranka.git
-    GIT_TAG 0.13.3
+    GIT_TAG 0.14.2
     GIT_PROGRESS TRUE
     EXCLUDE_FROM_ALL
 )
@@ -96,7 +98,7 @@ FetchContent_MakeAvailable(libfranka rl pybind11)
 if (${INCLUDE_UTN_MODELS})
     if (DEFINED GITLAB_MODELS_TOKEN)
         include(download_scenes)
-        set(ref b13a8d55bacc6470ece0788c654739e5cdef9265)
+        set(ref b8234983a5e35e222c955f9145ac4cd129827a8e)
         FetchContent_Declare(
             scenes
             URL         "https://gitos.rrze.fau.de/api/v4/projects/1100/repository/archive?path=scenes&sha=${ref}"

Makefile

@@ -1,4 +1,4 @@
-PYSRC = python/rcsss
+PYSRC = python
 CPPSRC = src
 COMPILE_MODE = Release
 

cmake/Findpinocchio.cmake

@@ -0,0 +1,45 @@
+if (NOT pinocchio_FOUND)
+    if (NOT Python3_FOUND)
+        set(pinocchio_FOUND FALSE)
+        if (pinocchio_FIND_REQUIRED)
+            message(FATAL_ERROR "Could not find pinocchio. Please install pinocchio using pip.")
+        endif()
+        return()
+    endif()
+
+    # Check if the include directory exists
+    cmake_path(APPEND Python3_SITELIB cmeel.prefix include OUTPUT_VARIABLE pinocchio_INCLUDE_DIRS)
+    if (NOT EXISTS ${pinocchio_INCLUDE_DIRS})
+        set(pinocchio_FOUND FALSE)
+        if (pinocchio_FIND_REQUIRED)
+            message(FATAL_ERROR "Could not find pinocchio. Please install pinocchio using pip.")
+        endif()
+        return()
+    endif()
+
+    # Check if the library file exists
+    cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio.so OUTPUT_VARIABLE pinocchio_library_path)
+    if (NOT EXISTS ${pinocchio_library_path})
+        set(pinocchio_FOUND FALSE)
+        if (pinocchio_FIND_REQUIRED)
+            message(FATAL_ERROR "Could not find pinocchio. Please install pinocchio using pip.")
+        endif()
+        return()
+    endif()
+
+    # Extract version from the library filename
+    file(GLOB pinocchio_dist_info "${Python3_SITELIB}/pin-*.dist-info")
+    cmake_path(GET pinocchio_dist_info FILENAME pinocchio_library_filename)
+    string(REPLACE "pin-" "" pinocchio_VERSION "${pinocchio_library_filename}")
+    string(REPLACE ".dist-info" "" pinocchio_VERSION "${pinocchio_VERSION}")
+
+    # Create the imported target
+    add_library(pinocchio::pinocchio SHARED IMPORTED)
+    target_include_directories(pinocchio::pinocchio INTERFACE ${pinocchio_INCLUDE_DIRS})
+    set_target_properties(
+        pinocchio::pinocchio
+        PROPERTIES
+        IMPORTED_LOCATION "${pinocchio_library_path}"
+    )
+    set(pinocchio_FOUND TRUE)
+endif()

pyproject.toml

@@ -8,7 +8,7 @@ version = "0.3.1"
 description="Python Interface for libfranka to control the Franka Research 3 Robot"
 dependencies = ["websockets>=11.0",
     "requests~=2.31",
-    "numpy==2.0",
+    "numpy",
     "typer~=0.9",
     "pydantic~=2.6",
     "gymnasium~=0.29.1",
@@ -23,7 +23,9 @@ dependencies = ["websockets>=11.0",
     "python-dotenv==1.0.1",
     "opencv-python~=4.10.0.84",
     # NOTE: when changing the mujoco version, also change it in requirements_dev.txt
-    "mujoco==3.1.5"
+    "mujoco==3.2.6",
+    # NOTE: Same for pinocchio (pin)
+    "pin==2.7.0"
 ]
 readme = "README.md"
 maintainers = [
@@ -98,6 +100,8 @@ ignore = [
   "T201",    # print() used
   "PLR5501", # elif to reduce indentation
   "PT018",   # assertion should be broken down into multiple parts
+  "NPY002",
+  "G004",   # Logging format string does not contain any placeholders
 ]
 
 [tool.pylint.format]

python/examples/env_cartesian_control.py

@@ -1,6 +1,5 @@
 import logging
 
-from dotenv import dotenv_values
 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
@@ -45,6 +44,7 @@
 
 
 def main():
+    resource_manger: FCI | DummyResourceManager
     if ROBOT_INSTANCE == RobotInstance.HARDWARE:
         user, pw = load_creds_fr3_desk()
         resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False)
@@ -60,7 +60,7 @@ def main():
                 collision_guard="lab",
                 gripper_cfg=default_fr3_hw_gripper_cfg(),
                 max_relative_movement=0.5,
-                relative_to=RelativeTo.LAST_STEP
+                relative_to=RelativeTo.LAST_STEP,
             )
         else:
             env_rel = fr3_sim_env(
@@ -70,12 +70,12 @@ def main():
                 gripper_cfg=default_fr3_sim_gripper_cfg(),
                 camera_set_cfg=default_mujoco_cameraset_cfg(),
                 max_relative_movement=0.5,
-                relative_to=RelativeTo.LAST_STEP
+                relative_to=RelativeTo.LAST_STEP,
             )
             env_rel.get_wrapper_attr("sim").open_gui()
 
         env_rel.reset()
-        print(env_rel.unwrapped.robot.get_cartesian_position())
+        print(env_rel.unwrapped.robot.get_cartesian_position())  # type: ignore
 
         for _ in range(10):
             for _ in range(10):

python/examples/env_joint_control.py

@@ -1,9 +1,6 @@
 import logging
 
-import mujoco
 import numpy as np
-import rcsss
-from dotenv import dotenv_values
 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
@@ -48,6 +45,7 @@
 
 
 def main():
+    resource_manger: FCI | DummyResourceManager
     if ROBOT_INSTANCE == RobotInstance.HARDWARE:
         user, pw = load_creds_fr3_desk()
         resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False)

python/examples/fr3.py

@@ -1,10 +1,8 @@
 import logging
 import sys
-from time import sleep
 
 import numpy as np
 import rcsss
-from dotenv import dotenv_values
 from rcsss import sim
 from rcsss._core.hw import FR3Config, IKController
 from rcsss._core.sim import CameraType
@@ -57,54 +55,54 @@ def main():
     if "lab" not in rcsss.scenes:
         logger.error("This pip package was not built with the UTN lab models, aborting.")
         sys.exit()
+    resource_manger: FCI | DummyResourceManager
     if ROBOT_INSTANCE == RobotInstance.HARDWARE:
         user, pw = load_creds_fr3_desk()
         resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False)
     else:
         resource_manger = DummyResourceManager()
 
     with resource_manger:
+        robot: rcsss.common.Robot
+        gripper: rcsss.common.Gripper
         if ROBOT_INSTANCE == RobotInstance.SIMULATION:
             simulation = sim.Sim(rcsss.scenes["fr3_empty_world"]["mjb"])
             urdf_path = get_urdf_path(URDF_PATH, allow_none_if_not_found=False)
+            assert urdf_path is not None
             ik = rcsss.common.IK(urdf_path)
             robot = rcsss.sim.FR3(simulation, "0", ik)
             cfg = sim.FR3Config()
             cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset())
             cfg.realtime = False
             robot.set_parameters(cfg)
 
-            gripper_cfg = sim.FHConfig()
-            gripper = sim.FrankaHand(simulation, "0", gripper_cfg)
+            gripper_cfg_sim = sim.FHConfig()
+            gripper = sim.FrankaHand(simulation, "0", gripper_cfg_sim)
 
             # add camera to have a rendering gui
             cameras = {
-                "default_free": SimCameraConfig(
-                    identifier="", type=int(CameraType.default_free)
-                ),
-                "wrist": SimCameraConfig(
-                    identifier="eye-in-hand_0", type=int(CameraType.fixed)
-                ),
-                # TODO: odd behavior when not both cameras are used: only last image is shown
+                "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free)),
+                "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed)),
             }
             cam_cfg = SimCameraSetConfig(cameras=cameras, resolution_width=1280, resolution_height=720, frame_rate=20)
-            camera_set = SimCameraSet(simulation, cam_cfg)
+            camera_set = SimCameraSet(simulation, cam_cfg)  # noqa: F841
             simulation.open_gui()
 
         else:
             urdf_path = get_urdf_path(URDF_PATH, allow_none_if_not_found=False)
+            assert urdf_path is not None
             ik = rcsss.common.IK(urdf_path)
-            robot = rcsss.hw.FR3(ROBOT_IP, str(rcsss.scenes["lab"].parent / "fr3.urdf"), ik)
+            robot = rcsss.hw.FR3(ROBOT_IP, ik)
             robot_cfg = FR3Config()
             robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset())
             robot_cfg.controller = IKController.robotics_library
             robot.set_parameters(robot_cfg)
 
-            gripper_cfg = rcsss.hw.FHConfig()
-            gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1
-            gripper_cfg.speed = 0.1
-            gripper_cfg.force = 30
-            gripper = rcsss.hw.FrankaHand(ROBOT_IP, gripper_cfg)
+            gripper_cfg_hw = rcsss.hw.FHConfig()
+            gripper_cfg_hw.epsilon_inner = gripper_cfg_hw.epsilon_outer = 0.1
+            gripper_cfg_hw.speed = 0.1
+            gripper_cfg_hw.force = 30
+            gripper = rcsss.hw.FrankaHand(ROBOT_IP, gripper_cfg_hw)
             input("the robot is going to move, press enter whenever you are ready")
 
         # move to home position and open gripper
@@ -120,7 +118,7 @@ def main():
         )
         if ROBOT_INSTANCE == RobotInstance.SIMULATION:
             simulation.step_until_convergence()
-            logger.debug(f"IK success: {robot.get_state().ik_success}")
+            logger.debug(f"IK success: {robot.get_state().ik_success}")  # type: ignore
             logger.debug(f"sim converged: {simulation.is_converged()}")
 
         # 5cm in y direction
@@ -129,7 +127,7 @@ def main():
         )
         if ROBOT_INSTANCE == RobotInstance.SIMULATION:
             simulation.step_until_convergence()
-            logger.debug(f"IK success: {robot.get_state().ik_success}")
+            logger.debug(f"IK success: {robot.get_state().ik_success}")  # type: ignore
             logger.debug(f"sim converged: {simulation.is_converged()}")
 
         # 5cm in z direction
@@ -138,7 +136,7 @@ def main():
         )
         if ROBOT_INSTANCE == RobotInstance.SIMULATION:
             simulation.step_until_convergence()
-            logger.debug(f"IK success: {robot.get_state().ik_success}")
+            logger.debug(f"IK success: {robot.get_state().ik_success}")  # type: ignore
             logger.debug(f"sim converged: {simulation.is_converged()}")
 
         # rotate the arm 90 degrees around the inverted y and z axis
@@ -148,7 +146,7 @@ def main():
         robot.set_cartesian_position(new_pose)
         if ROBOT_INSTANCE == RobotInstance.SIMULATION:
             simulation.step_until_convergence()
-            logger.debug(f"IK success: {robot.get_state().ik_success}")
+            logger.debug(f"IK success: {robot.get_state().ik_success}")  # type: ignore
             logger.debug(f"sim converged: {simulation.is_converged()}")
 
         if ROBOT_INSTANCE == RobotInstance.HARDWARE:
@@ -162,7 +160,7 @@ def main():
         )
         if ROBOT_INSTANCE == RobotInstance.SIMULATION:
             simulation.step_until_convergence()
-            logger.debug(f"IK success: {robot.get_state().ik_success}")
+            logger.debug(f"IK success: {robot.get_state().ik_success}")  # type: ignore
             logger.debug(f"sim converged: {simulation.is_converged()}")
 
         # grasp the object
@@ -177,7 +175,7 @@ def main():
         )
         if ROBOT_INSTANCE == RobotInstance.SIMULATION:
             simulation.step_until_convergence()
-            logger.debug(f"IK success: {robot.get_state().ik_success}")
+            logger.debug(f"IK success: {robot.get_state().ik_success}")  # type: ignore
             logger.debug(f"sim converged: {simulation.is_converged()}")
 
         if ROBOT_INSTANCE == RobotInstance.HARDWARE:

python/examples/grasp_demo.py

@@ -0,0 +1,91 @@
+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)
+        return Pose(translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3))
+
+    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 = 20) -> list[Pose]:
+        end_eff_pose = self.unwrapped.robot.get_cartesian_position()
+
+        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]
+        # this does not work if the object is flipped
+        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=5)
+        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=15)
+        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=20)
+        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=10)
+        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/SimplePickUpSim-v0", render_mode="human", delta_actions=True)
+    env.reset()
+    controller = PickUpDemo(env)
+    controller.pickup("yellow_box_geom")
+
+
+if __name__ == "__main__":
+    main()