refactor(ik): abstract ik class with rl subclass

Author: juelg

python/examples/fr3.py

@@ -69,7 +69,7 @@ def main():
             simulation = sim.Sim(rcsss.scenes["fr3_empty_world"])
             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)
+            ik = rcsss.common.RL(urdf_path)
             robot = rcsss.sim.FR3(simulation, "0", ik)
             cfg = sim.FR3Config()
             cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset())
@@ -91,7 +91,7 @@ def main():
         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)
+            ik = rcsss.common.RL(urdf_path)
             robot = rcsss.hw.FR3(ROBOT_IP, ik)
             robot_cfg = FR3Config()
             robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset())

python/rcsss/_core/common.pyi

@@ -21,6 +21,7 @@ __all__ = [
     "NRobotsWithGripper",
     "Pose",
     "RConfig",
+    "RL",
     "RPY",
     "RState",
     "Robot",
@@ -45,7 +46,6 @@ class Gripper:
     def shut(self) -> None: ...
 
 class IK:
-    def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ...
     def ik(
         self, pose: Pose, q0: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...
     ) -> numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]] | None: ...
@@ -128,6 +128,9 @@ class Pose:
 class RConfig:
     pass
 
+class RL(IK):
+    def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ...
+
 class RPY:
     pitch: float
     roll: float

python/rcsss/control/record.py

@@ -194,7 +194,7 @@ def __init__(
         poses: list[Pose] | None = None,
         urdf_path: str | None = None,
     ):
-        self.ik = common.IK(urdf_path) if urdf_path else None
+        self.ik = common.RL(urdf_path) if urdf_path else None
         self.r: dict[str, hw.FR3] = {key: hw.FR3(ip, self.ik) for key, ip in name2ip.items()}
         # TODO: this config should be given to the constructor
         cfg = hw.FHConfig()

python/rcsss/envs/factories.py

@@ -113,7 +113,7 @@ def fr3_hw_env(
         gym.Env: The configured hardware environment for the FR3 robot.
     """
     urdf_path = get_urdf_path(urdf_path, allow_none_if_not_found=collision_guard is not None)
-    ik = rcsss.common.IK(str(urdf_path)) if urdf_path is not None else None
+    ik = rcsss.common.RL(str(urdf_path)) if urdf_path is not None else None
     robot = rcsss.hw.FR3(ip, ik)
     robot.set_parameters(robot_cfg)
 
@@ -213,7 +213,7 @@ def fr3_sim_env(
         logger.warning("mjcf not found as key in scenes, interpreting mjcf as path the mujoco scene xml")
 
     simulation = sim.Sim(rcsss.scenes.get(str(mjcf), mjcf))
-    ik = rcsss.common.IK(urdf_path)
+    ik = rcsss.common.RL(urdf_path)
     robot = rcsss.sim.FR3(simulation, "0", ik)
     robot.set_parameters(robot_cfg)
     env: gym.Env = FR3Env(robot, control_mode)

python/rcsss/envs/sim.py

@@ -137,7 +137,7 @@ def env_from_xml_paths(
     ) -> "CollisionGuard":
         assert isinstance(env.unwrapped, FR3Env)
         simulation = sim.Sim(mjmld)
-        ik = rcsss.common.IK(urdf, max_duration_ms=300)
+        ik = rcsss.common.RL(urdf, max_duration_ms=300)
         robot = rcsss.sim.FR3(simulation, id, ik)
         cfg = sim.FR3Config()
         cfg.realtime = False

src/common/IK.cpp

@@ -15,32 +15,31 @@
 namespace rcs {
 namespace common {
 
-IK::IK(const std::string& urdf_path, size_t max_duration_ms) : rl() {
-  this->rl = RL();
-
-  this->rl.mdl = rl::mdl::UrdfFactory().create(urdf_path);
-  this->rl.kin = std::dynamic_pointer_cast<rl::mdl::Kinematic>(this->rl.mdl);
-  this->rl.ik =
-      std::make_shared<rl::mdl::JacobianInverseKinematics>(this->rl.kin.get());
-  this->rl.ik->setRandomRestarts(0);
-  this->rl.ik->setEpsilon(1e-3);
-  this->rl.ik->setDuration(std::chrono::milliseconds(max_duration_ms));
+RL::RL(const std::string& urdf_path, size_t max_duration_ms) : rl_data() {
+  this->rl_data.mdl = rl::mdl::UrdfFactory().create(urdf_path);
+  this->rl_data.kin =
+      std::dynamic_pointer_cast<rl::mdl::Kinematic>(this->rl_data.mdl);
+  this->rl_data.ik = std::make_shared<rl::mdl::JacobianInverseKinematics>(
+      this->rl_data.kin.get());
+  this->rl_data.ik->setRandomRestarts(0);
+  this->rl_data.ik->setEpsilon(1e-3);
+  this->rl_data.ik->setDuration(std::chrono::milliseconds(max_duration_ms));
 }
 
-std::optional<Vector7d> IK::ik(const Pose& pose, const Vector7d& q0,
+std::optional<Vector7d> RL::ik(const Pose& pose, const Vector7d& q0,
                                const Pose& tcp_offset) {
   // pose is assumed to be in the robots coordinate frame
-  this->rl.kin->setPosition(q0);
-  this->rl.kin->forwardPosition();
+  this->rl_data.kin->setPosition(q0);
+  this->rl_data.kin->forwardPosition();
   rcs::common::Pose new_pose = pose * tcp_offset.inverse();
 
-  this->rl.ik->addGoal(new_pose.affine_matrix(), 0);
-  bool success = this->rl.ik->solve();
+  this->rl_data.ik->addGoal(new_pose.affine_matrix(), 0);
+  bool success = this->rl_data.ik->solve();
   if (success) {
     // is this forward needed and is it mabye possible to call
     // this on the model?
-    this->rl.kin->forwardPosition();
-    return this->rl.kin->getPosition();
+    this->rl_data.kin->forwardPosition();
+    return this->rl_data.kin->getPosition();
   } else {
     return std::nullopt;
   }

src/common/IK.h

@@ -15,22 +15,29 @@
 namespace rcs {
 namespace common {
 
-struct RL {
-  std::shared_ptr<rl::mdl::Model> mdl;
-  std::shared_ptr<rl::mdl::Kinematic> kin;
-  std::shared_ptr<rl::mdl::JacobianInverseKinematics> ik;
+class IK {
+ public:
+  virtual ~IK(){};
+  virtual std::optional<Vector7d> ik(
+      const Pose& pose, const Vector7d& q0,
+      const Pose& tcp_offset = Pose::Identity()) = 0;
 };
 
-class IK {
+class RL : public IK {
  private:
-  RL rl;
+  struct RLData {
+    std::shared_ptr<rl::mdl::Model> mdl;
+    std::shared_ptr<rl::mdl::Kinematic> kin;
+    std::shared_ptr<rl::mdl::JacobianInverseKinematics> ik;
+  };
+  RLData rl_data;
 
  public:
-  IK(const std::string& urdf_path, size_t max_duration_ms = 300);
-  std::optional<Vector7d> ik(const Pose& pose, const Vector7d& q0,
-                             const Pose& tcp_offset = Pose::Identity());
-
-  // TODO: set max time
+  RL(const std::string& urdf_path, size_t max_duration_ms = 300);
+  ~RL() override = default;
+  std::optional<Vector7d> ik(
+      const Pose& pose, const Vector7d& q0,
+      const Pose& tcp_offset = Pose::Identity()) override;
 };
 }  // namespace common
 }  // namespace rcs

src/hw/main.cpp

@@ -17,7 +17,7 @@ const string urdf_path = "models/urdf/fr3.urdf";
 
 int main() {
   try {
-    auto ik = make_shared<rcs::common::IK>(urdf_path);
+    auto ik = make_shared<rcs::common::RL>(urdf_path);
     rcs::hw::FR3 robot(ip, ik);
     robot.automatic_error_recovery();
     std::cout << "WARNING: This example will move the robot! "

src/pybind/rcsss.cpp

@@ -212,11 +212,14 @@ PYBIND11_MODULE(_core, m) {
           }));
 
   py::class_<rcs::common::IK, std::shared_ptr<rcs::common::IK>>(common, "IK")
-      .def(py::init<const std::string &, size_t>(), py::arg("urdf_path"),
-           py::arg("max_duration_ms") = 300)
       .def("ik", &rcs::common::IK::ik, py::arg("pose"), py::arg("q0"),
            py::arg("tcp_offset") = rcs::common::Pose::Identity());
 
+  py::class_<rcs::common::RL, rcs::common::IK,
+             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::RConfig>(common, "RConfig");
   py::class_<rcs::common::RState>(common, "RState");
   py::class_<rcs::common::GConfig>(common, "GConfig");

src/sim/test.cpp

@@ -126,7 +126,7 @@ int test_sim() {
   sim->set_config(cfg);
   std::string id = "0";
 
-  auto ik = std::make_shared<rcs::common::IK>(urdf);
+  auto ik = std::make_shared<rcs::common::RL>(urdf);
   auto fr3 = rcs::sim::FR3(sim, id, ik);
   auto tcp_offset = rcs::common::Pose(rcs::common::FrankaHandTCPOffset());
   rcs::sim::FR3Config fr3_config = *fr3.get_parameters();