Merge pull request #219 from RobotControlStack/refactor/kinematics

refactor: inverse and forward kinematics interface

Author: juelg

CMakeLists.txt

@@ -39,20 +39,14 @@ find_package(MuJoCo REQUIRED)
 find_package(pinocchio REQUIRED)
 
 
-FetchContent_Declare(rl
-    GIT_REPOSITORY https://github.com/roboticslibrary/rl.git
-    GIT_TAG 0b3797215345a1d37903634095361233d190b2e6
-    GIT_PROGRESS TRUE
-    EXCLUDE_FROM_ALL
-)
 FetchContent_Declare(pybind11
     GIT_REPOSITORY https://github.com/pybind/pybind11.git
     GIT_TAG v2.13.4
     GIT_PROGRESS TRUE
     EXCLUDE_FROM_ALL
 )
 
-FetchContent_MakeAvailable(rl pybind11)
+FetchContent_MakeAvailable(pybind11)
 include(compile_scenes)
 
 add_subdirectory(src)

Makefile

@@ -35,16 +35,8 @@ stubgen:
 	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
-	pybind11-stubgen -o extensions --numpy-array-use-type-var rcs_fr3
-	find ./extensions/rcs_fr3 -not -path "./extensions/rcs_fr3/_core/*" -name '*.pyi' -delete
-	find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/'
-	find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g'
-	find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g'
-	rm -r extensions/rcs_fr3/src/rcs_fr3/_core
-	mv extensions/rcs_fr3/_core/ extensions/rcs_fr3/src/rcs_fr3/
-	ruff check --fix extensions/rcs_fr3/src/rcs_fr3/_core
-	isort python/rcs/_core extensions/rcs_fr3/src/rcs_fr3/_core
-	black python/rcs/_core extensions/rcs_fr3/src/rcs_fr3/_core
+	isort python/rcs/_core
+	black python/rcs/_core
 
 # Python
 pycheckformat:

examples/fr3/fr3_direct_control.py

@@ -60,8 +60,11 @@ def main():
         gripper: rcs.common.Gripper
         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.RL(str(urdf_path))
+            mjcf_path = rcs.scenes["fr3_empty_world"].mjcf_robot
+            ik = rcs.common.Pin(
+                mjcf_path,
+                "attachment_site_0",
+            )
             cfg = sim.SimRobotConfig()
             cfg.add_id("0")
             cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
@@ -92,8 +95,11 @@ def main():
             simulation.open_gui()
 
         else:
-            urdf_path = rcs.scenes["fr3_empty_world"].urdf
-            ik = rcs.common.RL(str(urdf_path))
+            mjcf_path = rcs.scenes["fr3_empty_world"].mjcf_robot
+            ik = rcs.common.Pin(
+                mjcf_path,
+                "attachment_site_0",
+            )
             robot = hw.FR3(ROBOT_IP, ik)
             robot_cfg = hw.FR3Config()
             robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())

extensions/rcs_fr3/src/hw/FR3.cpp

@@ -19,7 +19,8 @@
 
 namespace rcs {
 namespace hw {
-FR3::FR3(const std::string &ip, std::optional<std::shared_ptr<common::IK>> ik,
+FR3::FR3(const std::string &ip,
+         std::optional<std::shared_ptr<common::Kinematics>> ik,
          const std::optional<FR3Config> &cfg)
     : robot(ip), m_ik(ik) {
   // set collision behavior and impedance
@@ -650,7 +651,9 @@ double quintic_polynomial_speed_profile(double time, double start_time,
   // return (1 - std::cos(M_PI * progress)) / 2.0;
 }
 
-std::optional<std::shared_ptr<common::IK>> FR3::get_ik() { return this->m_ik; }
+std::optional<std::shared_ptr<common::Kinematics>> FR3::get_ik() {
+  return this->m_ik;
+}
 
 void FR3::set_cartesian_position(const common::Pose &x) {
   // pose is assumed to be in the robots coordinate frame
@@ -686,8 +689,8 @@ void FR3::set_cartesian_position_ik(const common::Pose &pose) {
         "No inverse kinematics was provided. Cannot use IK to set cartesian "
         "position.");
   }
-  auto joints = this->m_ik.value()->ik(pose, this->get_joint_position(),
-                                       this->cfg.tcp_offset);
+  auto joints = this->m_ik.value()->inverse(pose, this->get_joint_position(),
+                                            this->cfg.tcp_offset);
 
   if (joints.has_value()) {
     this->set_joint_position(joints.value());

extensions/rcs_fr3/src/hw/FR3.h

@@ -10,7 +10,7 @@
 #include <string>
 #include <thread>
 
-#include "rcs/IK.h"
+#include "rcs/Kinematics.h"
 #include "rcs/LinearPoseTrajInterpolator.h"
 #include "rcs/Pose.h"
 #include "rcs/Robot.h"
@@ -34,6 +34,8 @@ struct FR3Config : common::RobotConfig {
   // TODO: max force and elbow?
   // TODO: we can either write specific bindings for each, or we use python
   // dictionaries with these objects
+  common::RobotType robot_type = common::RobotType::FR3;
+  common::RobotPlatform robot_platform = common::RobotPlatform::HARDWARE;
   IKSolver ik_solver = IKSolver::franka_ik;
   double speed_factor = DEFAULT_SPEED_FACTOR;
   std::optional<FR3Load> load_parameters = std::nullopt;
@@ -48,7 +50,7 @@ class FR3 : public common::Robot {
  private:
   franka::Robot robot;
   FR3Config cfg;
-  std::optional<std::shared_ptr<common::IK>> m_ik;
+  std::optional<std::shared_ptr<common::Kinematics>> m_ik;
   std::optional<std::thread> control_thread = std::nullopt;
   common::LinearPoseTrajInterpolator traj_interpolator;
   double controller_time = 0.0;
@@ -62,7 +64,7 @@ class FR3 : public common::Robot {
 
  public:
   FR3(const std::string &ip,
-      std::optional<std::shared_ptr<common::IK>> ik = std::nullopt,
+      std::optional<std::shared_ptr<common::Kinematics>> ik = std::nullopt,
       const std::optional<FR3Config> &cfg = std::nullopt);
   ~FR3() override;
 
@@ -100,7 +102,7 @@ class FR3 : public common::Robot {
 
   void set_cartesian_position(const common::Pose &pose) override;
 
-  std::optional<std::shared_ptr<common::IK>> get_ik() override;
+  std::optional<std::shared_ptr<common::Kinematics>> get_ik() override;
 
   void set_cartesian_position_internal(const common::Pose &pose,
                                        double max_time,

extensions/rcs_fr3/src/hw/main.cpp

@@ -1,7 +1,7 @@
 #include <franka/exception.h>
 #include <franka/gripper.h>
 #include <franka/robot.h>
-#include <rcs/IK.h>
+#include <rcs/Kinematics.h>
 
 #include <iostream>
 #include <memory>

extensions/rcs_fr3/src/pybind/rcs.cpp

@@ -9,7 +9,7 @@
 
 #include <memory>
 
-#include "rcs/IK.h"
+#include "rcs/Kinematics.h"
 #include "rcs/Pose.h"
 #include "rcs/Robot.h"
 #include "rcs/utils.h"
@@ -99,7 +99,7 @@ PYBIND11_MODULE(_core, m) {
       (py::object)py::module_::import("rcs").attr("common").attr("Robot");
   py::class_<rcs::hw::FR3, std::shared_ptr<rcs::hw::FR3>>(hw, "FR3", robot)
       .def(py::init<const std::string &,
-                    std::optional<std::shared_ptr<rcs::common::IK>>>(),
+                    std::optional<std::shared_ptr<rcs::common::Kinematics>>>(),
            py::arg("ip"), py::arg("ik") = std::nullopt)
       .def("set_config", &rcs::hw::FR3::set_config, py::arg("cfg"))
       .def("get_config", &rcs::hw::FR3::get_config)

extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi

@@ -52,7 +52,7 @@ class FHState(rcs._core.common.GripperState):
     def width(self) -> float: ...
 
 class FR3(rcs._core.common.Robot):
-    def __init__(self, ip: str, ik: rcs._core.common.IK | None = None) -> None: ...
+    def __init__(self, ip: str, ik: rcs._core.common.Kinematics | None = None) -> None: ...
     def automatic_error_recovery(self) -> None: ...
     def controller_set_joint_position(
         self, desired_q: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]]

extensions/rcs_fr3/src/rcs_fr3/creators.py

@@ -38,7 +38,6 @@ def __call__(  # type: ignore
         camera_set: HardwareCameraSet | None = None,
         max_relative_movement: float | tuple[float, float] | None = None,
         relative_to: RelativeTo = RelativeTo.LAST_STEP,
-        urdf_path: str | PathLike | None = None,
     ) -> gym.Env:
         """
         Creates a hardware environment for the FR3 robot.
@@ -55,14 +54,16 @@ def __call__(  # type: ignore
                 translational movement in meters. If tuple, it restricts both translational (in meters) and rotational
                 (in radians) movements. If None, no restriction is applied.
             relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step.
-            urdf_path (str | PathLike | None): Path to the URDF file. If None the included one is used. A URDF file is needed for collision guarding.
 
         Returns:
             gym.Env: The configured hardware environment for the FR3 robot.
         """
-        if urdf_path is None:
-            urdf_path = rcs.scenes["fr3_empty_world"].urdf
-        ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None
+        ik = rcs.common.Pin(
+            robot_cfg.kinematic_model_path,
+            robot_cfg.attachment_site,
+            urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
+        )
+        # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path)
         robot = hw.FR3(ip, ik)
         robot.set_config(robot_cfg)
 
@@ -111,11 +112,15 @@ def __call__(  # type: ignore
         camera_set: HardwareCameraSet | None = None,
         max_relative_movement: float | tuple[float, float] | None = None,
         relative_to: RelativeTo = RelativeTo.LAST_STEP,
-        urdf_path: str | PathLike | None = None,
     ) -> gym.Env:
 
-        urdf_path = rcs.scenes["fr3_empty_world"].urdf
-        ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None
+        ik = rcs.common.Pin(
+            robot_cfg.kinematic_model_path,
+            robot_cfg.attachment_site,
+            urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
+        )
+        # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path)
+
         robots: dict[str, hw.FR3] = {}
         for ip in ips:
             robots[ip] = hw.FR3(ip, ik)

extensions/rcs_fr3/src/rcs_fr3/utils.py

@@ -1,11 +1,15 @@
 from rcs_fr3._core import hw
 
+import rcs
 from rcs import common
 
 
 def default_fr3_hw_robot_cfg(async_control: bool = False) -> hw.FR3Config:
     robot_cfg = hw.FR3Config()
+    robot_cfg.robot_type = rcs.scenes["fr3_empty_world"].robot_type
+    robot_cfg.kinematic_model_path = rcs.scenes["fr3_empty_world"].mjcf_robot
     robot_cfg.tcp_offset = common.Pose(common.FrankaHandTCPOffset())
+    robot_cfg.attachment_site = "attachment_site_0"
     robot_cfg.speed_factor = 0.1
     robot_cfg.ik_solver = hw.IKSolver.rcs_ik
     robot_cfg.async_control = async_control