feat(franka): add get_cartesian_flange_position method

Author: juelg

extensions/rcs_fr3/src/hw/Franka.cpp

@@ -19,13 +19,17 @@
 
 namespace rcs {
 namespace hw {
+common::Pose GetFlangeInBaseFrame(const franka::RobotState& robot_state) {
+  return common::Pose(robot_state.O_T_EE) *
+         common::Pose(robot_state.F_T_EE).inverse();
+}
+
 common::Pose GetTCPInBaseFrame(const franka::RobotState& robot_state,
                                const std::optional<common::Pose>& tcp_offset) {
   if (!tcp_offset.has_value()) {
     return common::Pose(robot_state.O_T_EE);
   }
-  return common::Pose(robot_state.O_T_EE) *
-         common::Pose(robot_state.F_T_EE).inverse() * tcp_offset.value();
+  return GetFlangeInBaseFrame(robot_state) * tcp_offset.value();
 }
 
 Franka::Franka(const FrankaConfig& cfg,
@@ -120,6 +124,19 @@ common::Pose Franka::get_cartesian_position() {
   return GetTCPInBaseFrame(robot_state, this->m_cfg.tcp_offset);
 }
 
+common::Pose Franka::get_cartesian_flange_position() {
+  this->check_for_background_errors();
+  franka::RobotState robot_state;
+  if (this->running_controller.load() == Controller::none) {
+    this->curr_state = this->robot.readOnce();
+    robot_state = this->curr_state;
+  } else {
+    std::lock_guard<std::mutex> lock(this->interpolator_mutex);
+    robot_state = this->curr_state;
+  }
+  return GetFlangeInBaseFrame(robot_state);
+}
+
 void Franka::set_joint_position(const common::VectorXd& q) {
   if (this->m_cfg.async_control) {
     this->controller_set_joint_position(q);

extensions/rcs_fr3/src/hw/Franka.h

@@ -119,6 +119,8 @@ class Franka : public common::Robot {
 
   common::Pose get_cartesian_position() override;
 
+  common::Pose get_cartesian_flange_position();
+
   void set_joint_position(const common::VectorXd& q) override;
 
   common::VectorXd get_joint_position() override;

extensions/rcs_fr3/src/pybind/rcs.cpp

@@ -301,6 +301,8 @@ PYBIND11_MODULE(_core, m) {
       .def("set_config", &rcs::hw::Franka::set_config, py::arg("cfg"))
       .def("get_config", &rcs::hw::Franka::get_config)
       .def("get_state", &rcs::hw::Franka::get_state)
+      .def("get_cartesian_flange_position",
+           &rcs::hw::Franka::get_cartesian_flange_position)
       .def("set_default_robot_behavior",
            &rcs::hw::Franka::set_default_robot_behavior)
       .def("set_guiding_mode", &rcs::hw::Franka::set_guiding_mode,

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

@@ -80,6 +80,7 @@ class Franka(rcs._core.common.Robot):
         self, desired_q: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]]
     ) -> None: ...
     def double_tap_robot_to_continue(self) -> None: ...
+    def get_cartesian_flange_position(self) -> rcs._core.common.Pose: ...
     def get_config(self) -> FrankaConfig: ...
     def get_state(self) -> FrankaState: ...
     def osc_set_cartesian_position(self, desired_pos_EE_in_base_frame: rcs._core.common.Pose) -> None: ...

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

@@ -80,6 +80,7 @@ class Franka(rcs._core.common.Robot):
         self, desired_q: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]]
     ) -> None: ...
     def double_tap_robot_to_continue(self) -> None: ...
+    def get_cartesian_flange_position(self) -> rcs._core.common.Pose: ...
     def get_config(self) -> FrankaConfig: ...
     def get_state(self) -> FrankaState: ...
     def osc_set_cartesian_position(self, desired_pos_EE_in_base_frame: rcs._core.common.Pose) -> None: ...