refactor: rename ik into kinematics and inverse

juelg · juelg · commit 6c88b88207ba · 2025-09-25T23:32:51.000+02:00
diff --git a/extensions/rcs_fr3/src/hw/FR3.cpp b/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());
diff --git a/extensions/rcs_fr3/src/hw/FR3.h b/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"
@@ -48,7 +48,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 +62,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 +100,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,
diff --git a/extensions/rcs_fr3/src/hw/main.cpp b/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>
diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/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)
diff --git a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi b/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]]
diff --git a/extensions/rcs_so101/src/rcs_so101/hw.py b/extensions/rcs_so101/src/rcs_so101/hw.py
@@ -55,7 +55,7 @@ def reset(self) -> None:
         pass
 
     def set_cartesian_position(self, pose: common.Pose) -> None:
-        joints = self.ik.ik(pose, q0=self.get_joint_position())
+        joints = self.ik.inverse(pose, q0=self.get_joint_position())
         if joints is not None:
             self.set_joint_position(joints)
 
diff --git a/include/rcs/Kinematics.h b/include/rcs/Kinematics.h
@@ -20,16 +20,16 @@
 namespace rcs {
 namespace common {
 
-class IK {
+class Kinematics {
  public:
-  virtual ~IK(){};
-  virtual std::optional<VectorXd> ik(
+  virtual ~Kinematics(){};
+  virtual std::optional<VectorXd> inverse(
       const Pose& pose, const VectorXd& q0,
       const Pose& tcp_offset = Pose::Identity()) = 0;
   virtual Pose forward(const VectorXd& q0, const Pose& tcp_offset) = 0;
 };
 
-class RL : public IK {
+class RL : public Kinematics {
  private:
   const int random_restarts = 0;
   const double eps = 1e-3;
@@ -41,13 +41,13 @@ class RL : public IK {
 
  public:
   RL(const std::string& path, size_t max_duration_ms = 300);
-  std::optional<VectorXd> ik(
+  std::optional<VectorXd> inverse(
       const Pose& pose, const VectorXd& q0,
       const Pose& tcp_offset = Pose::Identity()) override;
   Pose forward(const VectorXd& q0, const Pose& tcp_offset) override;
 };
 
-class Pin : public IK {
+class Pin : public Kinematics {
  private:
   const double eps = 1e-4;
   const int IT_MAX = 1000;
@@ -60,7 +60,7 @@ class Pin : public IK {
 
  public:
   Pin(const std::string& path, const std::string& frame_id, bool urdf);
-  std::optional<VectorXd> ik(
+  std::optional<VectorXd> inverse(
       const Pose& pose, const VectorXd& q0,
       const Pose& tcp_offset = Pose::Identity()) override;
   Pose forward(const VectorXd& q0, const Pose& tcp_offset) override;
diff --git a/include/rcs/Robot.h b/include/rcs/Robot.h
@@ -5,7 +5,7 @@
 #include <optional>
 #include <string>
 
-#include "IK.h"
+#include "Kinematics.h"
 #include "Pose.h"
 #include "utils.h"
 
@@ -152,7 +152,7 @@ class Robot {
 
   virtual void set_cartesian_position(const Pose& pose) = 0;
 
-  virtual std::optional<std::shared_ptr<IK>> get_ik() = 0;
+  virtual std::optional<std::shared_ptr<Kinematics>> get_ik() = 0;
 
   common::Pose to_pose_in_world_coordinates(
       const Pose& pose_in_robot_coordinates);
diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi
@@ -21,10 +21,10 @@ __all__ = [
     "Hand",
     "HandConfig",
     "HandState",
-    "IK",
     "IdentityRotMatrix",
     "IdentityRotQuatVec",
     "IdentityTranslation",
+    "Kinematics",
     "LATERAL_GRASP",
     "POWER_GRASP",
     "PRECISION_GRASP",
@@ -128,13 +128,13 @@ class HandConfig:
 class HandState:
     def __init__(self) -> None: ...
 
-class IK:
+class Kinematics:
     def forward(self, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ...
-    def ik(
+    def inverse(
         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):
+class Pin(Kinematics):
     def __init__(self, path: str, frame_id: str = "fr3_link8", urdf: bool = True) -> None: ...
 
 class Pose:
@@ -195,7 +195,7 @@ class Pose:
     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):
+class RL(Kinematics):
     def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ...
 
 class RPY:
@@ -223,7 +223,7 @@ class Robot:
     def get_base_pose_in_world_coordinates(self) -> Pose: ...
     def get_cartesian_position(self) -> Pose: ...
     def get_config(self) -> RobotConfig: ...
-    def get_ik(self) -> IK | None: ...
+    def get_ik(self) -> Kinematics | None: ...
     def get_joint_position(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
     def get_state(self) -> RobotState: ...
     def move_home(self) -> None: ...
diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi
@@ -151,7 +151,7 @@ class SimGripperState(rcs._core.common.GripperState):
 
 class SimRobot(rcs._core.common.Robot):
     def __init__(
-        self, sim: Sim, ik: rcs._core.common.IK, cfg: SimRobotConfig, register_convergence_callback: bool = True
+        self, sim: Sim, ik: rcs._core.common.Kinematics, cfg: SimRobotConfig, register_convergence_callback: bool = True
     ) -> None: ...
     def get_config(self) -> SimRobotConfig: ...
     def get_state(self) -> SimRobotState: ...
diff --git a/python/rcs/ompl/mj_ompl.py b/python/rcs/ompl/mj_ompl.py
@@ -281,7 +281,7 @@ def ik(self, pose, q0=None, tcp_offset=None):
             numpy.ndarray: Joint positions that achieve the desired pose, or None if no solution is found.
         """
         tcp_offset = tcp_offset if tcp_offset is not None else self.franka_hand_tcp
-        return self.env.unwrapped.robot.get_ik().ik(pose, q0, tcp_offset)  # type: ignore[attr-defined]
+        return self.env.unwrapped.robot.get_ik().inverse(pose, q0, tcp_offset)  # type: ignore[attr-defined]
 
 
 class MjOStateSpace(ob.RealVectorStateSpace):
diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp
@@ -11,7 +11,7 @@
 
 #include <memory>
 
-#include "rcs/IK.h"
+#include "rcs/Kinematics.h"
 #include "rcs/Pose.h"
 #include "rcs/Robot.h"
 #include "rcs/utils.h"
@@ -69,9 +69,10 @@ class PyRobot : public rcs::common::Robot {
   void close() override {
     PYBIND11_OVERRIDE_PURE(void, rcs::common::Robot, close, );
   }
-  std::optional<std::shared_ptr<rcs::common::IK>> get_ik() override {
-    PYBIND11_OVERRIDE_PURE(std::optional<std::shared_ptr<rcs::common::IK>>,
-                           rcs::common::Robot, get_ik, );
+  std::optional<std::shared_ptr<rcs::common::Kinematics>> get_ik() override {
+    PYBIND11_OVERRIDE_PURE(
+        std::optional<std::shared_ptr<rcs::common::Kinematics>>,
+        rcs::common::Robot, get_ik, );
   }
   rcs::common::Pose get_base_pose_in_world_coordinates() override {
     PYBIND11_OVERRIDE_PURE(rcs::common::Pose, rcs::common::Robot,
@@ -286,18 +287,19 @@ PYBIND11_MODULE(_core, m) {
             return rcs::common::Pose(t);
           }));
 
-  py::class_<rcs::common::IK, std::shared_ptr<rcs::common::IK>>(common, "IK")
-      .def("ik", &rcs::common::IK::ik, py::arg("pose"), py::arg("q0"),
-           py::arg("tcp_offset") = rcs::common::Pose::Identity())
-      .def("forward", &rcs::common::IK::forward, py::arg("q0"),
+  py::class_<rcs::common::Kinematics, std::shared_ptr<rcs::common::Kinematics>>(
+      common, "Kinematics")
+      .def("inverse", &rcs::common::Kinematics::inverse, py::arg("pose"),
+           py::arg("q0"), py::arg("tcp_offset") = rcs::common::Pose::Identity())
+      .def("forward", &rcs::common::Kinematics::forward, py::arg("q0"),
            py::arg("tcp_offset") = rcs::common::Pose::Identity());
 
-  py::class_<rcs::common::RL, rcs::common::IK,
+  py::class_<rcs::common::RL, rcs::common::Kinematics,
              std::shared_ptr<rcs::common::RL>>(common, "RL")
       .def(py::init<const std::string &, size_t>(), py::arg("urdf_path"),
            py::arg("max_duration_ms") = 300);
 
-  py::class_<rcs::common::Pin, rcs::common::IK,
+  py::class_<rcs::common::Pin, rcs::common::Kinematics,
              std::shared_ptr<rcs::common::Pin>>(common, "Pin")
       .def(py::init<const std::string &, const std::string &, bool>(),
            py::arg("path"), py::arg("frame_id") = "fr3_link8",
@@ -521,8 +523,8 @@ PYBIND11_MODULE(_core, m) {
   py::class_<rcs::sim::SimRobot, rcs::common::Robot,
              std::shared_ptr<rcs::sim::SimRobot>>(sim, "SimRobot")
       .def(py::init<std::shared_ptr<rcs::sim::Sim>,
-                    std::shared_ptr<rcs::common::IK>, rcs::sim::SimRobotConfig,
-                    bool>(),
+                    std::shared_ptr<rcs::common::Kinematics>,
+                    rcs::sim::SimRobotConfig, bool>(),
            py::arg("sim"), py::arg("ik"), py::arg("cfg"),
            py::arg("register_convergence_callback") = true)
       .def("get_config", &rcs::sim::SimRobot::get_config)
diff --git a/src/rcs/CMakeLists.txt b/src/rcs/CMakeLists.txt
@@ -1,6 +1,6 @@
 add_library(rcs SHARED)
 target_include_directories(rcs PUBLIC ${CMAKE_SOURCE_DIR}/include)
-target_sources(rcs PRIVATE Pose.cpp Robot.cpp IK.cpp utils.cpp)
+target_sources(rcs PRIVATE Pose.cpp Robot.cpp Kinematics.cpp utils.cpp)
 target_link_libraries(rcs PUBLIC Eigen3::Eigen mdl pinocchio::all)
 set_target_properties(rcs PROPERTIES
     INSTALL_RPATH "$ORIGIN;$ORIGIN/../cmeel.prefix/lib"
diff --git a/src/rcs/Kinematics.cpp b/src/rcs/Kinematics.cpp
@@ -1,4 +1,4 @@
-#include "rcs/IK.h"
+#include "rcs/Kinematics.h"
 
 #include <rl/hal/CartesianPositionActuator.h>
 #include <rl/hal/CartesianPositionSensor.h>
@@ -33,8 +33,8 @@ RL::RL(const std::string& urdf_path, size_t max_duration_ms) : rl_data() {
   this->rl_data.ik->setDuration(std::chrono::milliseconds(max_duration_ms));
 }
 
-std::optional<VectorXd> RL::ik(const Pose& pose, const VectorXd& q0,
-                               const Pose& tcp_offset) {
+std::optional<VectorXd> RL::inverse(const Pose& pose, const VectorXd& q0,
+                                    const Pose& tcp_offset) {
   // pose is assumed to be in the robots coordinate frame
   this->rl_data.kin->setPosition(q0);
   this->rl_data.kin->forwardPosition();
@@ -75,8 +75,8 @@ Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf = true)
   }
 }
 
-std::optional<VectorXd> Pin::ik(const Pose& pose, const VectorXd& q0,
-                                const Pose& tcp_offset) {
+std::optional<VectorXd> Pin::inverse(const Pose& pose, const VectorXd& q0,
+                                     const Pose& tcp_offset) {
   rcs::common::Pose new_pose = pose * tcp_offset.inverse();
   VectorXd q(model.nq);
   q.setZero();
diff --git a/src/sim/SimRobot.cpp b/src/sim/SimRobot.cpp
@@ -24,8 +24,9 @@ namespace sim {
 
 // TODO: check dof contraints
 // TODO: use C++11 feature to call one constructor from another
-SimRobot::SimRobot(std::shared_ptr<Sim> sim, std::shared_ptr<common::IK> ik,
-                   SimRobotConfig cfg, bool register_convergence_callback)
+SimRobot::SimRobot(std::shared_ptr<Sim> sim,
+                   std::shared_ptr<common::Kinematics> ik, SimRobotConfig cfg,
+                   bool register_convergence_callback)
     : sim{sim}, cfg{cfg}, state{}, m_ik(ik) {
   this->init_ids();
   if (register_convergence_callback) {
@@ -137,14 +138,14 @@ common::VectorXd SimRobot::get_joint_position() {
   return q;
 }
 
-std::optional<std::shared_ptr<common::IK>> SimRobot::get_ik() {
+std::optional<std::shared_ptr<common::Kinematics>> SimRobot::get_ik() {
   return this->m_ik;
 }
 
 void SimRobot::set_cartesian_position(const common::Pose& pose) {
   // pose is assumed to be in the robots coordinate frame
-  auto joint_vals =
-      this->m_ik->ik(pose, this->get_joint_position(), this->cfg.tcp_offset);
+  auto joint_vals = this->m_ik->inverse(pose, this->get_joint_position(),
+                                        this->cfg.tcp_offset);
   if (joint_vals.has_value()) {
     this->state.ik_success = true;
     this->set_joint_position(joint_vals.value());
diff --git a/src/sim/SimRobot.h b/src/sim/SimRobot.h
