From 31f869e9bae918e8b577d18af5640237499f3c8c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20J=C3=BClg?= <tobias.juelg@utn.de>
Date: Tue, 10 Mar 2026 12:23:11 +0100
Subject: [PATCH] feat(franka): option for tcp in desk

---
 extensions/rcs_fr3/src/hw/Franka.cpp                     | 3 +++
 extensions/rcs_fr3/src/hw/Franka.h                       | 1 +
 extensions/rcs_fr3/src/pybind/rcs.cpp                    | 2 ++
 extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi     | 1 +
 extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi | 1 +
 5 files changed, 8 insertions(+)

diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp
index 3283685b..d2b0b9e3 100644
--- a/extensions/rcs_fr3/src/hw/Franka.cpp
+++ b/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;
 }
 
diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h
index 70f85ec7..789d18a4 100644
--- a/extensions/rcs_fr3/src/hw/Franka.h
+++ b/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 {};
diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp
index 8c8de868..54b8171a 100644
--- a/extensions/rcs_fr3/src/pybind/rcs.cpp
+++ b/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")
diff --git a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi
index f1d22f37..3c1c9082 100644
--- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi
+++ b/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: ...
 
diff --git a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi
index f1d22f37..3c1c9082 100644
--- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi
+++ b/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: ...