feat(franka): option to keep tcp offset in desk (#262)

Author: juelg

extensions/rcs_fr3/src/hw/Franka.cpp

@@ -110,6 +110,9 @@ common::Pose Franka::get_cartesian_position() {
     x = common::Pose(this->curr_state.O_T_EE);
     this->interpolator_mutex.unlock();
   }
+  if (!this->cfg.tcp_offset_configured_in_desk) {
+    return x * cfg.tcp_offset;
+  }
   return x;
 }
 

extensions/rcs_fr3/src/hw/Franka.h

@@ -43,6 +43,7 @@ struct FrankaConfig : common::RobotConfig {
   std::optional<common::Pose> nominal_end_effector_frame = std::nullopt;
   std::optional<common::Pose> world_to_robot = std::nullopt;
   bool async_control = false;
+  bool tcp_offset_configured_in_desk = true;
 };
 
 struct FR3Config : FrankaConfig {};

extensions/rcs_fr3/src/pybind/rcs.cpp

@@ -133,6 +133,8 @@ PYBIND11_MODULE(_core, m) {
       .def_readwrite("nominal_end_effector_frame",
                      &rcs::hw::FrankaConfig::nominal_end_effector_frame)
       .def_readwrite("world_to_robot", &rcs::hw::FrankaConfig::world_to_robot)
+      .def_readwrite("tcp_offset_configured_in_desk",
+                     &rcs::hw::FrankaConfig::tcp_offset_configured_in_desk)
       .def_readwrite("async_control", &rcs::hw::FrankaConfig::async_control);
 
   py::class_<rcs::hw::FR3Config, rcs::hw::FrankaConfig>(hw, "FR3Config")

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

@@ -100,6 +100,7 @@ class FrankaConfig(rcs._core.common.RobotConfig):
     load_parameters: FrankaLoad | None
     nominal_end_effector_frame: rcs._core.common.Pose | None
     speed_factor: float
+    tcp_offset_configured_in_desk: bool
     world_to_robot: rcs._core.common.Pose | None
     def __init__(self) -> None: ...
 

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

@@ -100,6 +100,7 @@ class FrankaConfig(rcs._core.common.RobotConfig):
     load_parameters: FrankaLoad | None
     nominal_end_effector_frame: rcs._core.common.Pose | None
     speed_factor: float
+    tcp_offset_configured_in_desk: bool
     world_to_robot: rcs._core.common.Pose | None
     def __init__(self) -> None: ...