Pinocchio IK

CMakeLists.txt

@@ -46,7 +46,7 @@ include(FetchContent)
 find_package(Eigen3 REQUIRED)
 find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
 find_package(MuJoCo REQUIRED)
-
+find_package(pinocchio REQUIRED)
 FetchContent_Declare(
     libfranka
     GIT_REPOSITORY https://github.com/frankaemika/libfranka.git

Makefile

@@ -27,8 +27,8 @@ stubgen:
 	pybind11-stubgen -o python --numpy-array-use-type-var rcs
 	find ./python -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/'
 	find ./python -not -path "./python/rcs/_core/*" -name '*.pyi' -delete
-	find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/typing\.Literal[\1]/g'
-	find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/\1/g'
+	find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g'
+	find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g'
 	ruff check --fix python/rcs/_core
 	isort python/rcs/_core
 	black python/rcs/_core

cmake/Findpinocchio.cmake

@@ -18,7 +18,7 @@ if (NOT pinocchio_FOUND)
     endif()
 
     # Check if the library file exists
-    cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio.so OUTPUT_VARIABLE pinocchio_library_path)
+    cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio_default.so OUTPUT_VARIABLE pinocchio_library_path)
     if (NOT EXISTS ${pinocchio_library_path})
         set(pinocchio_FOUND FALSE)
         if (pinocchio_FIND_REQUIRED)
@@ -27,6 +27,16 @@ if (NOT pinocchio_FOUND)
         return()
     endif()
 
+    # Check if the library file exists
+    cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio_parsers.so OUTPUT_VARIABLE pinocchio_parsers_path)
+    if (NOT EXISTS ${pinocchio_parsers_path})
+        set(pinocchio_FOUND FALSE)
+        if (pinocchio_FIND_REQUIRED)
+            message(FATAL_ERROR "Could not find pinocchio parsers path. 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)
@@ -38,8 +48,17 @@ if (NOT pinocchio_FOUND)
     target_include_directories(pinocchio::pinocchio INTERFACE ${pinocchio_INCLUDE_DIRS})
     set_target_properties(
         pinocchio::pinocchio
-        PROPERTIES
+	PROPERTIES
         IMPORTED_LOCATION "${pinocchio_library_path}"
     )
+
+    add_library(pinocchio_parsers SHARED IMPORTED)
+    target_include_directories(pinocchio_parsers INTERFACE ${pinocchio_INCLUDE_DIRS})
+    set_target_properties(
+        pinocchio_parsers
+	PROPERTIES
+        IMPORTED_LOCATION "${pinocchio_parsers_path}"
+    )
     set(pinocchio_FOUND TRUE)
+
 endif()

pyproject.toml

@@ -24,7 +24,7 @@ dependencies = ["websockets>=11.0",
     # NOTE: when changing the mujoco version, also change it in requirements_dev.txt
     "mujoco==3.2.6",
     # NOTE: Same for pinocchio (pin)
-    "pin==2.7.0",
+    "pin==3.7.0",
     "tilburg-hand",
     "digit-interface",
 ]

python/examples/fr3.py

@@ -60,7 +60,7 @@ def main():
         if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
             simulation = sim.Sim(rcs.scenes["fr3_empty_world"]["mjb"])
             urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
-            ik = rcs.common.IK(str(urdf_path))
+            ik = rcs.common.RL(str(urdf_path))
             cfg = sim.SimRobotConfig()
             cfg.add_id("0")
             cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
@@ -92,7 +92,7 @@ def main():
 
         else:
             urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
-            ik = rcs.common.IK(str(urdf_path))
+            ik = rcs.common.RL(str(urdf_path))
             robot = rcs.hw.FR3(ROBOT_IP, ik)
             robot_cfg = FR3Config()
             robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())

python/rcs/_core/common.pyi

@@ -21,7 +21,9 @@ __all__ = [
     "IdentityRotMatrix",
     "IdentityRotQuatVec",
     "IdentityTranslation",
+    "Pin",
     "Pose",
+    "RL",
     "RPY",
     "Robot",
     "RobotConfig",
@@ -62,11 +64,13 @@ class GripperState:
     pass
 
 class IK:
-    def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ...
-    def forward(self, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ...
+    def forward(self, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ...
     def ik(
-        self, pose: Pose, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ...
-    ) -> numpy.ndarray[M, numpy.dtype[numpy.float64]] | None: ...
+        self, pose: Pose, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...
+    ) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]] | None: ...
+
+class Pin(IK):
+    def __init__(self, path: str, frame_id: str = "fr3_link8", urdf: bool = True) -> None: ...
 
 class Pose:
     def __getstate__(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
@@ -80,26 +84,28 @@ class Pose:
     def __init__(
         self,
         rotation: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]],
-        translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]],
+        translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]],
     ) -> None: ...
     @typing.overload
     def __init__(
         self,
-        quaternion: numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]],
-        translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]],
+        quaternion: numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]],
+        translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]],
     ) -> None: ...
     @typing.overload
-    def __init__(self, rpy: RPY, translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]) -> None: ...
+    def __init__(
+        self, rpy: RPY, translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]
+    ) -> None: ...
     @typing.overload
     def __init__(
         self,
-        rpy_vector: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]],
-        translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]],
+        rpy_vector: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]],
+        translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]],
     ) -> None: ...
     @typing.overload
-    def __init__(self, translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]) -> None: ...
+    def __init__(self, translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]) -> None: ...
     @typing.overload
-    def __init__(self, quaternion: numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]) -> None: ...
+    def __init__(self, quaternion: numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]) -> None: ...
     @typing.overload
     def __init__(self, rpy: RPY) -> None: ...
     @typing.overload
@@ -118,11 +124,14 @@ class Pose:
     def limit_translation_length(self, max_length: float) -> Pose: ...
     def pose_matrix(self) -> numpy.ndarray[tuple[typing.Literal[4], typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
     def rotation_m(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ...
-    def rotation_q(self) -> numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]: ...
+    def rotation_q(self) -> numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
     def rotation_rpy(self) -> RPY: ...
     def total_angle(self) -> float: ...
-    def translation(self) -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ...
-    def xyzrpy(self) -> numpy.ndarray[typing.Literal[6], numpy.dtype[numpy.float64]]: ...
+    def translation(self) -> numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]: ...
+    def xyzrpy(self) -> numpy.ndarray[tuple[typing.Literal[6]], numpy.dtype[numpy.float64]]: ...
+
+class RL(IK):
+    def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ...
 
 class RPY:
     pitch: float
@@ -133,11 +142,11 @@ class RPY:
     @typing.overload
     def __init__(self, roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0) -> None: ...
     @typing.overload
-    def __init__(self, rpy: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]) -> None: ...
+    def __init__(self, rpy: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]) -> None: ...
     def __setstate__(self, arg0: tuple) -> None: ...
     def __str__(self) -> str: ...
-    def as_quaternion_vector(self) -> numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]: ...
-    def as_vector(self) -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ...
+    def as_quaternion_vector(self) -> numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
+    def as_vector(self) -> numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]: ...
     def is_close(self, other: RPY, eps: float = 1e-08) -> bool: ...
     def rotation_matrix(
         self,
@@ -147,13 +156,13 @@ class Robot:
     def get_base_pose_in_world_coordinates(self) -> Pose: ...
     def get_cartesian_position(self) -> Pose: ...
     def get_ik(self) -> IK | None: ...
-    def get_joint_position(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ...
+    def get_joint_position(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
     def get_parameters(self) -> RobotConfig: ...
     def get_state(self) -> RobotState: ...
     def move_home(self) -> None: ...
     def reset(self) -> None: ...
     def set_cartesian_position(self, pose: Pose) -> None: ...
-    def set_joint_position(self, q: numpy.ndarray[M, numpy.dtype[numpy.float64]]) -> None: ...
+    def set_joint_position(self, q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]) -> None: ...
     def to_pose_in_robot_coordinates(self, pose_in_world_coordinates: Pose) -> Pose: ...
     def to_pose_in_world_coordinates(self, pose_in_robot_coordinates: Pose) -> Pose: ...
 
@@ -168,7 +177,7 @@ class RobotMetaConfig:
     @property
     def joint_limits(self) -> numpy.ndarray[tuple[typing.Literal[2], N], numpy.dtype[numpy.float64]]: ...
     @property
-    def q_home(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ...
+    def q_home(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
 
 class RobotPlatform:
     """
@@ -236,8 +245,8 @@ class RobotType:
 
 def FrankaHandTCPOffset() -> numpy.ndarray[tuple[typing.Literal[4], typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
 def IdentityRotMatrix() -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ...
-def IdentityRotQuatVec() -> numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]: ...
-def IdentityTranslation() -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ...
+def IdentityRotQuatVec() -> numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
+def IdentityTranslation() -> numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]: ...
 def _bootstrap_egl(fn_addr: int, display: int, context: int) -> None: ...
 def robots_meta_config(robot_type: RobotType) -> RobotMetaConfig: ...
 

python/rcs/_core/hw/__init__.pyi

@@ -55,7 +55,7 @@ class FR3(rcs._core.common.Robot):
     def __init__(self, ip: str, ik: rcs._core.common.IK | None = None) -> None: ...
     def automatic_error_recovery(self) -> None: ...
     def controller_set_joint_position(
-        self, desired_q: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]
+        self, desired_q: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]]
     ) -> None: ...
     def double_tap_robot_to_continue(self) -> None: ...
     def get_parameters(self) -> FR3Config: ...
@@ -91,7 +91,7 @@ class FR3Config(rcs._core.common.RobotConfig):
     def __init__(self) -> None: ...
 
 class FR3Load:
-    f_x_cload: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]] | None
+    f_x_cload: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] | None
     load_inertia: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]] | None
     load_mass: float
     def __init__(self) -> None: ...

python/rcs/_core/sim.pyi

@@ -67,9 +67,9 @@ class CameraType:
 class FrameSet:
     def __init__(self) -> None: ...
     @property
-    def color_frames(self) -> dict[str, numpy.ndarray[M, numpy.dtype[numpy.uint8]]]: ...
+    def color_frames(self) -> dict[str, numpy.ndarray[tuple[M], numpy.dtype[numpy.uint8]]]: ...
     @property
-    def depth_frames(self) -> dict[str, numpy.ndarray[M, numpy.dtype[numpy.float32]]]: ...
+    def depth_frames(self) -> dict[str, numpy.ndarray[tuple[M], numpy.dtype[numpy.float32]]]: ...
     @property
     def timestamp(self) -> float: ...
 
@@ -142,7 +142,7 @@ class SimRobot(rcs._core.common.Robot):
     ) -> None: ...
     def get_parameters(self) -> SimRobotConfig: ...
     def get_state(self) -> SimRobotState: ...
-    def set_joints_hard(self, q: numpy.ndarray[M, numpy.dtype[numpy.float64]]) -> None: ...
+    def set_joints_hard(self, q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]) -> None: ...
     def set_parameters(self, cfg: SimRobotConfig) -> bool: ...
 
 class SimRobotConfig(rcs._core.common.RobotConfig):
@@ -173,9 +173,9 @@ class SimRobotState(rcs._core.common.RobotState):
     @property
     def is_moving(self) -> bool: ...
     @property
-    def previous_angles(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ...
+    def previous_angles(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
     @property
-    def target_angles(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ...
+    def target_angles(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
 
 default_free: CameraType  # value = <CameraType.default_free: 3>
 fixed: CameraType  # value = <CameraType.fixed: 2>

python/rcs/envs/creators.py

@@ -84,7 +84,7 @@ def __call__(  # type: ignore
         """
         if urdf_path is None:
             urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
-        ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None
+        ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None
         robot = rcs.hw.FR3(ip, ik)
         robot.set_parameters(robot_cfg)
 
@@ -136,7 +136,7 @@ def __call__(  # type: ignore
     ) -> gym.Env:
 
         urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
-        ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None
+        ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None
         robots: dict[str, rcs.hw.FR3] = {}
         for ip in ips:
             robots[ip] = rcs.hw.FR3(ip, ik)
@@ -232,7 +232,7 @@ def __call__(  # type: ignore
             mjcf = rcs.scenes[mjcf]["mjb"]
         simulation = sim.Sim(mjcf)
 
-        ik = rcs.common.IK(str(urdf_path))
+        ik = rcs.common.RL(str(urdf_path))
         robot = rcs.sim.SimRobot(simulation, ik, robot_cfg)
         env: gym.Env = RobotEnv(robot, control_mode)
         env = RobotSimWrapper(env, simulation, sim_wrapper)

python/rcs/envs/sim.py

@@ -204,7 +204,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.IK(urdf, max_duration_ms=300)
+        ik = rcs.common.RL(urdf, max_duration_ms=300)
         cfg = default_fr3_sim_robot_cfg(mjmld, id)
         cfg.realtime = False
         if tcp_offset is not None:

python/rcs/envs/space_utils.py

@@ -17,10 +17,10 @@
 
 M = TypeVar("M", bound=int)
 VecType: TypeAlias = np.ndarray[M, np.dtype[np.float64]]
-Vec7Type: TypeAlias = np.ndarray[Literal[7], np.dtype[np.float64]]
-Vec3Type: TypeAlias = np.ndarray[Literal[3], np.dtype[np.float64]]
-Vec6Type: TypeAlias = np.ndarray[Literal[6], np.dtype[np.float64]]
-Vec18Type: TypeAlias = np.ndarray[Literal[18], np.dtype[np.float64]]
+Vec7Type: TypeAlias = np.ndarray[tuple[Literal[7]], np.dtype[np.float64]]
+Vec3Type: TypeAlias = np.ndarray[tuple[Literal[3]], np.dtype[np.float64]]
+Vec6Type: TypeAlias = np.ndarray[tuple[Literal[6]], np.dtype[np.float64]]
+Vec18Type: TypeAlias = np.ndarray[tuple[Literal[18]], np.dtype[np.float64]]
 
 
 class RCSpaceType(TypedDict): ...

python/rcs_so101/creators.py

@@ -118,7 +118,7 @@ def __call__(  # type: ignore
         """
         simulation = sim.Sim(mjcf)
 
-        ik = rcs.common.IK(str(urdf_path))
+        ik = rcs.common.RL(str(urdf_path))
         robot = rcs.sim.SimRobot(simulation, ik, robot_cfg)
         env: gym.Env = RobotEnv(robot, control_mode)
         env = RobotSimWrapper(env, simulation, sim_wrapper)

python/rcs_so101/hw.py

@@ -7,7 +7,7 @@
 
 class SO101(common.Robot):
     def __init__(self, hf_robot: SO101Follower, urdf_path: str):
-        self.ik = common.IK(urdf_path=urdf_path)
+        self.ik = common.RL(urdf_path=urdf_path)
         self._hf_robot = hf_robot
         self._hf_robot.connect()
 
@@ -18,7 +18,7 @@ def get_cartesian_position(self) -> common.Pose:
     def get_ik(self) -> common.IK | None:
         return self.ik
 
-    def get_joint_position(self) -> np.ndarray[typing.Literal[5], np.dtype[np.float64]]:  # type: ignore
+    def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]:  # type: ignore
         obs = self._hf_robot.get_observation()
         return np.array(
             [
@@ -43,7 +43,7 @@ def get_state(self) -> common.RobotState:
 
     def move_home(self) -> None:
         home = typing.cast(
-            np.ndarray[typing.Literal[5], np.dtype[np.float64]],
+            np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]],
             common.robots_meta_config(common.RobotType.SO101).q_home,
         )
         self.set_joint_position(home)
@@ -56,7 +56,7 @@ def set_cartesian_position(self, pose: common.Pose) -> None:
         if joints is not None:
             self.set_joint_position(joints)
 
-    def set_joint_position(self, q: np.ndarray[typing.Literal[5], np.dtype[np.float64]]) -> None:  # type: ignore
+    def set_joint_position(self, q: np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]) -> None:  # type: ignore
         self._hf_robot.send_action(
             {
                 "shoulder_pan.pos": q[0],

requirements_dev.txt

@@ -11,5 +11,5 @@ scikit-build-core>=0.3.3
 poetry-core>=2.1.3
 pybind11
 mujoco==3.2.6
-pin==2.7.0
+pin==3.7.0
 wheel

src/common/CMakeLists.txt

@@ -1,3 +1,3 @@
 add_library(common)
 target_sources(common PRIVATE Pose.cpp Robot.cpp IK.cpp utils.cpp)
-target_link_libraries(common PUBLIC Eigen3::Eigen mdl)
+target_link_libraries(common PUBLIC Eigen3::Eigen mdl pinocchio::pinocchio pinocchio_parsers)