From a095071df79be40643ab70c941c27e81f52f1fe1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20J=C3=BClg?= <tobias.juelg@utn.de>
Date: Fri, 28 Nov 2025 01:15:45 +0100
Subject: [PATCH 01/10] fix: controller

---
 extensions/rcs_fr3/src/hw/Franka.cpp     | 48 ++++++++++++----
 include/rcs/LinearPoseTrajInterpolator.h | 70 +++++++++++-------------
 2 files changed, 70 insertions(+), 48 deletions(-)

diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp
index 1b6972a0..0197121e 100644
--- a/extensions/rcs_fr3/src/hw/Franka.cpp
+++ b/extensions/rcs_fr3/src/hw/Franka.cpp
@@ -426,20 +426,32 @@ void Franka::osc() {
     dist2joint_min = q - joint_min_.matrix();
 
     for (int i = 0; i < 7; i++) {
-      if (dist2joint_max[i] < 0.25 && dist2joint_max[i] > 0.1)
-        avoidance_force[i] += -avoidance_weights_[i] * dist2joint_max[i];
-      if (dist2joint_min[i] < 0.25 && dist2joint_min[i] > 0.1)
-        avoidance_force[i] += avoidance_weights_[i] * dist2joint_min[i];
+      double limit_buffer = 0.25;  // rad
+      if (dist2joint_max[i] < limit_buffer) {
+        // Repulsive force increases as we get closer to the limit
+        avoidance_force[i] +=
+            -avoidance_weights_[i] * (1.0 - dist2joint_max[i] / limit_buffer);
+      }
+      if (dist2joint_min[i] < limit_buffer) {
+        avoidance_force[i] +=
+            avoidance_weights_[i] * (1.0 - dist2joint_min[i] / limit_buffer);
+      }
     }
     tau_d << tau_d + Nullspace * avoidance_force;
-    for (int i = 0; i < 7; i++) {
-      if (dist2joint_max[i] < 0.1 && tau_d[i] > 0.) tau_d[i] = 0.;
-      if (dist2joint_min[i] < 0.1 && tau_d[i] < 0.) tau_d[i] = 0.;
-    }
 
     std::array<double, 7> tau_d_array{};
     Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
 
+    // Safety check for NaN/inf values
+    for (size_t i = 0; i < 7; ++i) {
+      if (!std::isfinite(tau_d_array[i])) {
+        std::cerr << "Franka::osc: Commanded torque for joint " << i
+                  << " is not finite. Commanding zero torques." << std::endl;
+        std::fill(tau_d_array.begin(), tau_d_array.end(), 0.0);
+        break;
+      }
+    }
+
     // end of controller
     std::chrono::high_resolution_clock::time_point t2 =
         std::chrono::high_resolution_clock::now();
@@ -520,6 +532,7 @@ void Franka::joint_controller() {
 
     tau_d << Kp.cwiseProduct(joint_pos_error) - Kd.cwiseProduct(dq);
 
+    // Joint limit avoidance
     Eigen::Matrix<double, 7, 1> dist2joint_max;
     Eigen::Matrix<double, 7, 1> dist2joint_min;
 
@@ -527,13 +540,28 @@ void Franka::joint_controller() {
     dist2joint_min = q - joint_min_.matrix();
 
     for (int i = 0; i < 7; i++) {
-      if (dist2joint_max[i] < 0.1 && tau_d[i] > 0.) tau_d[i] = 0.;
-      if (dist2joint_min[i] < 0.1 && tau_d[i] < 0.) tau_d[i] = 0.;
+      double limit_buffer = 0.15;  // rad
+      if (dist2joint_max[i] < limit_buffer && tau_d[i] > 0.) {
+        tau_d[i] *= dist2joint_max[i] / limit_buffer;
+      }
+      if (dist2joint_min[i] < limit_buffer && tau_d[i] < 0.) {
+        tau_d[i] *= dist2joint_min[i] / limit_buffer;
+      }
     }
 
     std::array<double, 7> tau_d_array{};
     Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
 
+    // Safety check for NaN/inf values
+    for (size_t i = 0; i < 7; ++i) {
+      if (!std::isfinite(tau_d_array[i])) {
+        std::cerr << "Franka::joint_controller: Commanded torque for joint " << i
+                  << " is not finite. Commanding zero torques." << std::endl;
+        std::fill(tau_d_array.begin(), tau_d_array.end(), 0.0);
+        break;
+      }
+    }
+
     // end of controller
     std::chrono::high_resolution_clock::time_point t2 =
         std::chrono::high_resolution_clock::now();
diff --git a/include/rcs/LinearPoseTrajInterpolator.h b/include/rcs/LinearPoseTrajInterpolator.h
index cecaa754..9c855b73 100644
--- a/include/rcs/LinearPoseTrajInterpolator.h
+++ b/include/rcs/LinearPoseTrajInterpolator.h
@@ -10,28 +10,32 @@ class LinearPoseTrajInterpolator {
   Eigen::Vector3d p_start_;
   Eigen::Vector3d p_goal_;
   Eigen::Vector3d last_p_t_;
-  Eigen::Vector3d prev_p_goal_;
 
   Eigen::Quaterniond q_start_;
   Eigen::Quaterniond q_goal_;
   Eigen::Quaterniond last_q_t_;
-  Eigen::Quaterniond prev_q_goal_;
 
   double dt_;
   double last_time_;
   double max_time_;
   double start_time_;
   bool start_;
-  bool first_goal_;
+  bool initialized_;
 
  public:
   inline LinearPoseTrajInterpolator()
-      : dt_(0.),
+      : p_start_(Eigen::Vector3d::Zero()),
+        p_goal_(Eigen::Vector3d::Zero()),
+        last_p_t_(Eigen::Vector3d::Zero()),
+        q_start_(Eigen::Quaterniond::Identity()),
+        q_goal_(Eigen::Quaterniond::Identity()),
+        last_q_t_(Eigen::Quaterniond::Identity()),
+        dt_(0.),
         last_time_(0.),
         max_time_(1.),
         start_time_(0.),
         start_(false),
-        first_goal_(true){};
+        initialized_(false){};
 
   inline ~LinearPoseTrajInterpolator(){};
 
@@ -41,37 +45,31 @@ class LinearPoseTrajInterpolator {
                     const Eigen::Quaterniond &q_goal, const int &policy_rate,
                     const int &rate,
                     const double &traj_interpolator_time_fraction) {
+    if (policy_rate > 0) {
+      max_time_ = 1. / static_cast<double>(policy_rate) *
+                  traj_interpolator_time_fraction;
+    }
     dt_ = 1. / static_cast<double>(rate);
     last_time_ = time_sec;
-    max_time_ =
-        1. / static_cast<double>(policy_rate) * traj_interpolator_time_fraction;
     start_time_ = time_sec;
 
     start_ = false;
 
-    if (first_goal_) {
+    if (!initialized_) {
       p_start_ = p_start;
       q_start_ = q_start;
-
-      prev_p_goal_ = p_start;
-      prev_q_goal_ = q_start;
-      first_goal_ = false;
+      initialized_ = true;
     } else {
-      // If the goal is already set, use prev goal as the starting point of
-      // interpolation.
-      prev_p_goal_ = p_goal_;
-      prev_q_goal_ = q_goal_;
-
-      p_start_ = prev_p_goal_;
-      q_start_ = prev_q_goal_;
+      p_start_ = last_p_t_;
+      q_start_ = last_q_t_;
     }
 
     p_goal_ = p_goal;
     q_goal_ = q_goal;
 
     // Flip the sign if the dot product of quaternions is negative
-    if (q_goal_.coeffs().dot(q_start_.coeffs()) < 0.0) {
-      q_start_.coeffs() << -q_start_.coeffs();
+    if (q_start_.coeffs().dot(q_goal_.coeffs()) < 0.0) {
+      q_goal_.coeffs() << -q_goal_.coeffs();
     }
   };
 
@@ -98,32 +96,29 @@ class LinearPoseTrajInterpolator {
 class LinearJointPositionTrajInterpolator {
  private:
   using Vector7d = Eigen::Matrix<double, 7, 1>;
-  using Vector7i = Eigen::Matrix<int, 7, 1>;
 
   Vector7d q_start_;
   Vector7d q_goal_;
-
   Vector7d last_q_t_;
-  Vector7d prev_q_goal_;
 
   double dt_;
   double last_time_;
   double max_time_;
   double start_time_;
   bool start_;
-  bool first_goal_;
-
-  double interpolation_fraction_;  // fraction of actual interpolation within an
-                                   // interval
+  bool initialized_;
 
  public:
   inline LinearJointPositionTrajInterpolator()
-      : dt_(0.),
+      : q_start_(Vector7d::Zero()),
+        q_goal_(Vector7d::Zero()),
+        last_q_t_(Vector7d::Zero()),
+        dt_(0.),
         last_time_(0.),
         max_time_(1.),
         start_time_(0.),
         start_(false),
-        first_goal_(true){};
+        initialized_(false){};
 
   inline ~LinearJointPositionTrajInterpolator(){};
 
@@ -132,23 +127,22 @@ class LinearJointPositionTrajInterpolator {
                     const Eigen::Matrix<double, 7, 1> &q_goal,
                     const int &policy_rate, const int &rate,
                     const double &traj_interpolator_time_fraction) {
+    if (policy_rate > 0) {
+      max_time_ = 1. / static_cast<double>(policy_rate) *
+                  traj_interpolator_time_fraction;
+    }
     dt_ = 1. / static_cast<double>(rate);
     last_time_ = time_sec;
 
-    max_time_ =
-        1. / static_cast<double>(policy_rate) * traj_interpolator_time_fraction;
     start_time_ = time_sec;
 
     start_ = false;
 
-    if (first_goal_) {
+    if (!initialized_) {
       q_start_ = q_start;
-      prev_q_goal_ = q_start;
-      first_goal_ = false;
-      // std::cout << "First goal of the interpolation" << std::endl;
+      initialized_ = true;
     } else {
-      prev_q_goal_ = q_goal_;
-      q_start_ = prev_q_goal_;
+      q_start_ = last_q_t_;
     }
     q_goal_ = q_goal;
   };

From 3346e7c3ef0c36b2c79ec8b1e431228c55ac32ee Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20J=C3=BClg?= <tobias.juelg@utn.de>
Date: Fri, 28 Nov 2025 01:15:57 +0100
Subject: [PATCH 02/10] feat: controller torque limit per joint

---
 extensions/rcs_fr3/src/hw/Franka.cpp | 27 ++++++++++++---------------
 1 file changed, 12 insertions(+), 15 deletions(-)

diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp
index 0197121e..0b0230e8 100644
--- a/extensions/rcs_fr3/src/hw/Franka.cpp
+++ b/extensions/rcs_fr3/src/hw/Franka.cpp
@@ -153,14 +153,11 @@ void PInverse(const Eigen::MatrixXd& M, Eigen::MatrixXd& M_inv,
   M_inv = Eigen::MatrixXd(svd.matrixV() * S_inv * svd.matrixU().transpose());
 }
 
-void TorqueSafetyGuardFn(std::array<double, 7>& tau_d_array, double min_torque,
-                         double max_torque) {
+void TorqueSafetyGuardFn(std::array<double, 7> &tau_d_array,
+                         const std::array<double, 7> &max_torques) {
   for (size_t i = 0; i < tau_d_array.size(); i++) {
-    if (tau_d_array[i] < min_torque) {
-      tau_d_array[i] = min_torque;
-    } else if (tau_d_array[i] > max_torque) {
-      tau_d_array[i] = max_torque;
-    }
+    tau_d_array[i] =
+        std::max(std::min(tau_d_array[i], max_torques[i]), -max_torques[i]);
   }
 }
 
@@ -460,10 +457,10 @@ void Franka::osc() {
     std::array<double, 7> tau_d_rate_limited = franka::limitRate(
         franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d);
 
-    // deoxys/config/control_config.yml
-    double min_torque = -5;
-    double max_torque = 5;
-    TorqueSafetyGuardFn(tau_d_rate_limited, min_torque, max_torque);
+    // Set per-joint torque limits
+    std::array<double, 7> max_torques = {
+        {20.0, 20.0, 20.0, 20.0, 10.0, 10.0, 10.0}};
+    TorqueSafetyGuardFn(tau_d_rate_limited, max_torques);
 
     return tau_d_rate_limited;
   });
@@ -570,10 +567,10 @@ void Franka::joint_controller() {
     std::array<double, 7> tau_d_rate_limited = franka::limitRate(
         franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d);
 
-    // deoxys/config/control_config.yml
-    double min_torque = -5;
-    double max_torque = 5;
-    TorqueSafetyGuardFn(tau_d_rate_limited, min_torque, max_torque);
+    // Set per-joint torque limits
+    std::array<double, 7> max_torques = {
+        {20.0, 20.0, 20.0, 20.0, 10.0, 10.0, 10.0}};
+    TorqueSafetyGuardFn(tau_d_rate_limited, max_torques);
 
     return tau_d_rate_limited;
   });

From b3bf2ed4baacb8884ed2ea0e0763ee07eced1184 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20J=C3=BClg?= <tobias.juelg@utn.de>
Date: Fri, 28 Nov 2025 01:23:54 +0100
Subject: [PATCH 03/10] feat: auto recover franka

---
 extensions/rcs_fr3/src/hw/Franka.cpp | 523 ++++++++++++++-------------
 1 file changed, 275 insertions(+), 248 deletions(-)

diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp
index 0b0230e8..2d7ee0b5 100644
--- a/extensions/rcs_fr3/src/hw/Franka.cpp
+++ b/extensions/rcs_fr3/src/hw/Franka.cpp
@@ -290,180 +290,189 @@ void Franka::osc() {
   joint_min_ << -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973;
   avoidance_weights_ << 1., 1., 1., 1., 1., 10., 10.;
 
-  this->robot.control([&](const franka::RobotState& robot_state,
-                          franka::Duration period) -> franka::Torques {
-    std::chrono::high_resolution_clock::time_point t1 =
-        std::chrono::high_resolution_clock::now();
-
-    // torques handler
-    if (this->running_controller == Controller::none) {
-      franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
-      return franka::MotionFinished(zero_torques);
-    }
-    // TO BE replaced
-    // if (!this->control_thread_running && dq.maxCoeff() < 0.0001) {
-    //   return franka::MotionFinished(franka::Torques(tau_d_array));
-    // }
-
-    Eigen::Vector3d desired_pos_EE_in_base_frame;
-    Eigen::Quaterniond desired_quat_EE_in_base_frame;
-
-    common::Pose pose(robot_state.O_T_EE);
-    // form deoxys/config/charmander.yml
-    int policy_rate = 20;
-    int traj_rate = 500;
-
-    this->interpolator_mutex.lock();
-    this->curr_state = robot_state;
-    this->controller_time += period.toSec();
-    this->traj_interpolator.next_step(this->controller_time,
-                                      desired_pos_EE_in_base_frame,
-                                      desired_quat_EE_in_base_frame);
-    this->interpolator_mutex.unlock();
-
-    // end torques handler
-
-    Eigen::Matrix<double, 7, 1> tau_d;
-
-    std::array<double, 49> mass_array = model.mass(robot_state);
-    Eigen::Map<Eigen::Matrix<double, 7, 7>> M(mass_array.data());
-
-    M = M + Eigen::Matrix<double, 7, 7>(residual_mass_vec_.asDiagonal());
-
-    // coriolis and gravity
-    std::array<double, 7> coriolis_array = model.coriolis(robot_state);
-    Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(
-        coriolis_array.data());
-
-    std::array<double, 7> gravity_array = model.gravity(robot_state);
-    Eigen::Map<const Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
-
-    std::array<double, 42> jacobian_array =
-        model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
-    Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(
-        jacobian_array.data());
-
-    Eigen::MatrixXd jacobian_pos(3, 7);
-    Eigen::MatrixXd jacobian_ori(3, 7);
-    jacobian_pos << jacobian.block(0, 0, 3, 7);
-    jacobian_ori << jacobian.block(3, 0, 3, 7);
-
-    // End effector pose in base frame
-    Eigen::Affine3d T_EE_in_base_frame(
-        Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
-    Eigen::Vector3d pos_EE_in_base_frame(T_EE_in_base_frame.translation());
-    Eigen::Quaterniond quat_EE_in_base_frame(T_EE_in_base_frame.linear());
-
-    // Nullspace goal
-    Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
-
-    // Joint velocity
-    Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
-
-    if (desired_quat_EE_in_base_frame.coeffs().dot(
-            quat_EE_in_base_frame.coeffs()) < 0.0) {
-      quat_EE_in_base_frame.coeffs() << -quat_EE_in_base_frame.coeffs();
-    }
-
-    Eigen::Vector3d pos_error;
-
-    pos_error << desired_pos_EE_in_base_frame - pos_EE_in_base_frame;
-    Eigen::Quaterniond quat_error(desired_quat_EE_in_base_frame.inverse() *
-                                  quat_EE_in_base_frame);
-    Eigen::Vector3d ori_error;
-    ori_error << quat_error.x(), quat_error.y(), quat_error.z();
-    ori_error << -T_EE_in_base_frame.linear() * ori_error;
-
-    // Compute matrices
-    Eigen::Matrix<double, 7, 7> M_inv(M.inverse());
-    Eigen::MatrixXd Lambda_inv(6, 6);
-    Lambda_inv << jacobian * M_inv * jacobian.transpose();
-    Eigen::MatrixXd Lambda(6, 6);
-    PInverse(Lambda_inv, Lambda);
-
-    Eigen::Matrix<double, 7, 6> J_inv;
-    J_inv << M_inv * jacobian.transpose() * Lambda;
-    Eigen::Matrix<double, 7, 7> Nullspace;
-    Nullspace << Eigen::MatrixXd::Identity(7, 7) -
-                     jacobian.transpose() * J_inv.transpose();
-
-    // Decoupled mass matrices
-    Eigen::MatrixXd Lambda_pos_inv(3, 3);
-    Lambda_pos_inv << jacobian_pos * M_inv * jacobian_pos.transpose();
-    Eigen::MatrixXd Lambda_ori_inv(3, 3);
-    Lambda_ori_inv << jacobian_ori * M_inv * jacobian_ori.transpose();
-
-    Eigen::MatrixXd Lambda_pos(3, 3);
-    Eigen::MatrixXd Lambda_ori(3, 3);
-    PInverse(Lambda_pos_inv, Lambda_pos);
-    PInverse(Lambda_ori_inv, Lambda_ori);
-
-    pos_error =
-        pos_error.unaryExpr([](double x) { return (abs(x) < 1e-4) ? 0. : x; });
-    ori_error =
-        ori_error.unaryExpr([](double x) { return (abs(x) < 5e-3) ? 0. : x; });
-
-    tau_d << jacobian_pos.transpose() *
-                     (Lambda_pos *
-                      (Kp_p * pos_error - Kd_p * (jacobian_pos * dq))) +
-                 jacobian_ori.transpose() *
-                     (Lambda_ori *
-                      (Kp_r * ori_error - Kd_r * (jacobian_ori * dq)));
-
-    // nullspace control
-    tau_d << tau_d + Nullspace * (static_q_task_ - q);
-
-    // Add joint avoidance potential
-    Eigen::Matrix<double, 7, 1> avoidance_force;
-    avoidance_force.setZero();
-    Eigen::Matrix<double, 7, 1> dist2joint_max;
-    Eigen::Matrix<double, 7, 1> dist2joint_min;
-
-    dist2joint_max = joint_max_.matrix() - q;
-    dist2joint_min = q - joint_min_.matrix();
-
-    for (int i = 0; i < 7; i++) {
-      double limit_buffer = 0.25;  // rad
-      if (dist2joint_max[i] < limit_buffer) {
-        // Repulsive force increases as we get closer to the limit
-        avoidance_force[i] +=
-            -avoidance_weights_[i] * (1.0 - dist2joint_max[i] / limit_buffer);
-      }
-      if (dist2joint_min[i] < limit_buffer) {
-        avoidance_force[i] +=
-            avoidance_weights_[i] * (1.0 - dist2joint_min[i] / limit_buffer);
+  try {
+    this->robot.control([&](const franka::RobotState &robot_state,
+                            franka::Duration period) -> franka::Torques {
+      std::chrono::high_resolution_clock::time_point t1 =
+          std::chrono::high_resolution_clock::now();
+
+      // torques handler
+      if (this->running_controller == Controller::none) {
+        franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+        return franka::MotionFinished(zero_torques);
       }
-    }
-    tau_d << tau_d + Nullspace * avoidance_force;
-
-    std::array<double, 7> tau_d_array{};
-    Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
-
-    // Safety check for NaN/inf values
-    for (size_t i = 0; i < 7; ++i) {
-      if (!std::isfinite(tau_d_array[i])) {
-        std::cerr << "Franka::osc: Commanded torque for joint " << i
-                  << " is not finite. Commanding zero torques." << std::endl;
-        std::fill(tau_d_array.begin(), tau_d_array.end(), 0.0);
-        break;
+      // TO BE replaced
+      // if (!this->control_thread_running && dq.maxCoeff() < 0.0001) {
+      //   return franka::MotionFinished(franka::Torques(tau_d_array));
+      // }
+
+      Eigen::Vector3d desired_pos_EE_in_base_frame;
+      Eigen::Quaterniond desired_quat_EE_in_base_frame;
+
+      common::Pose pose(robot_state.O_T_EE);
+      // form deoxys/config/charmander.yml
+      int policy_rate = 20;
+      int traj_rate = 500;
+
+      this->interpolator_mutex.lock();
+      this->curr_state = robot_state;
+      this->controller_time += period.toSec();
+      this->traj_interpolator.next_step(this->controller_time,
+                                        desired_pos_EE_in_base_frame,
+                                        desired_quat_EE_in_base_frame);
+      this->interpolator_mutex.unlock();
+
+      // end torques handler
+
+      Eigen::Matrix<double, 7, 1> tau_d;
+
+      std::array<double, 49> mass_array = model.mass(robot_state);
+      Eigen::Map<Eigen::Matrix<double, 7, 7>> M(mass_array.data());
+
+      M = M + Eigen::Matrix<double, 7, 7>(residual_mass_vec_.asDiagonal());
+
+      // coriolis and gravity
+      std::array<double, 7> coriolis_array = model.coriolis(robot_state);
+      Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(
+          coriolis_array.data());
+
+      std::array<double, 7> gravity_array = model.gravity(robot_state);
+      Eigen::Map<const Eigen::Matrix<double, 7, 1>> gravity(
+          gravity_array.data());
+
+      std::array<double, 42> jacobian_array =
+          model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
+      Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(
+          jacobian_array.data());
+
+      Eigen::MatrixXd jacobian_pos(3, 7);
+      Eigen::MatrixXd jacobian_ori(3, 7);
+      jacobian_pos << jacobian.block(0, 0, 3, 7);
+      jacobian_ori << jacobian.block(3, 0, 3, 7);
+
+      // End effector pose in base frame
+      Eigen::Affine3d T_EE_in_base_frame(
+          Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
+      Eigen::Vector3d pos_EE_in_base_frame(T_EE_in_base_frame.translation());
+      Eigen::Quaterniond quat_EE_in_base_frame(T_EE_in_base_frame.linear());
+
+      // Nullspace goal
+      Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
+
+      // Joint velocity
+      Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
+
+      if (desired_quat_EE_in_base_frame.coeffs().dot(
+              quat_EE_in_base_frame.coeffs()) < 0.0) {
+        quat_EE_in_base_frame.coeffs() << -quat_EE_in_base_frame.coeffs();
       }
-    }
 
-    // end of controller
-    std::chrono::high_resolution_clock::time_point t2 =
-        std::chrono::high_resolution_clock::now();
-    auto time = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
-
-    std::array<double, 7> tau_d_rate_limited = franka::limitRate(
-        franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d);
-
-    // Set per-joint torque limits
-    std::array<double, 7> max_torques = {
-        {20.0, 20.0, 20.0, 20.0, 10.0, 10.0, 10.0}};
-    TorqueSafetyGuardFn(tau_d_rate_limited, max_torques);
+      Eigen::Vector3d pos_error;
+
+      pos_error << desired_pos_EE_in_base_frame - pos_EE_in_base_frame;
+      Eigen::Quaterniond quat_error(desired_quat_EE_in_base_frame.inverse() *
+                                    quat_EE_in_base_frame);
+      Eigen::Vector3d ori_error;
+      ori_error << quat_error.x(), quat_error.y(), quat_error.z();
+      ori_error << -T_EE_in_base_frame.linear() * ori_error;
+
+      // Compute matrices
+      Eigen::Matrix<double, 7, 7> M_inv(M.inverse());
+      Eigen::MatrixXd Lambda_inv(6, 6);
+      Lambda_inv << jacobian * M_inv * jacobian.transpose();
+      Eigen::MatrixXd Lambda(6, 6);
+      PInverse(Lambda_inv, Lambda);
+
+      Eigen::Matrix<double, 7, 6> J_inv;
+      J_inv << M_inv * jacobian.transpose() * Lambda;
+      Eigen::Matrix<double, 7, 7> Nullspace;
+      Nullspace << Eigen::MatrixXd::Identity(7, 7) -
+                       jacobian.transpose() * J_inv.transpose();
+
+      // Decoupled mass matrices
+      Eigen::MatrixXd Lambda_pos_inv(3, 3);
+      Lambda_pos_inv << jacobian_pos * M_inv * jacobian_pos.transpose();
+      Eigen::MatrixXd Lambda_ori_inv(3, 3);
+      Lambda_ori_inv << jacobian_ori * M_inv * jacobian_ori.transpose();
+
+      Eigen::MatrixXd Lambda_pos(3, 3);
+      Eigen::MatrixXd Lambda_ori(3, 3);
+      PInverse(Lambda_pos_inv, Lambda_pos);
+      PInverse(Lambda_ori_inv, Lambda_ori);
+
+      pos_error = pos_error.unaryExpr(
+          [](double x) { return (abs(x) < 1e-4) ? 0. : x; });
+      ori_error = ori_error.unaryExpr(
+          [](double x) { return (abs(x) < 5e-3) ? 0. : x; });
+
+      tau_d << jacobian_pos.transpose() *
+                       (Lambda_pos *
+                        (Kp_p * pos_error - Kd_p * (jacobian_pos * dq))) +
+                   jacobian_ori.transpose() *
+                       (Lambda_ori *
+                        (Kp_r * ori_error - Kd_r * (jacobian_ori * dq)));
+
+      // nullspace control
+      tau_d << tau_d + Nullspace * (static_q_task_ - q);
+
+      // Add joint avoidance potential
+      Eigen::Matrix<double, 7, 1> avoidance_force;
+      avoidance_force.setZero();
+      Eigen::Matrix<double, 7, 1> dist2joint_max;
+      Eigen::Matrix<double, 7, 1> dist2joint_min;
+
+      dist2joint_max = joint_max_.matrix() - q;
+      dist2joint_min = q - joint_min_.matrix();
+
+      for (int i = 0; i < 7; i++) {
+        double limit_buffer = 0.25;  // rad
+        if (dist2joint_max[i] < limit_buffer) {
+          // Repulsive force increases as we get closer to the limit
+          avoidance_force[i] +=
+              -avoidance_weights_[i] * (1.0 - dist2joint_max[i] / limit_buffer);
+        }
+        if (dist2joint_min[i] < limit_buffer) {
+          avoidance_force[i] +=
+              avoidance_weights_[i] * (1.0 - dist2joint_min[i] / limit_buffer);
+        }
+      }
+      tau_d << tau_d + Nullspace * avoidance_force;
+
+      std::array<double, 7> tau_d_array{};
+      Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
+
+      // Safety check for NaN/inf values
+      for (size_t i = 0; i < 7; ++i) {
+        if (!std::isfinite(tau_d_array[i])) {
+          std::cerr << "Franka::osc: Commanded torque for joint " << i
+                    << " is not finite. Commanding zero torques." << std::endl;
+          std::fill(tau_d_array.begin(), tau_d_array.end(), 0.0);
+          break;
+        }
+      }
 
-    return tau_d_rate_limited;
-  });
+      // end of controller
+      std::chrono::high_resolution_clock::time_point t2 =
+          std::chrono::high_resolution_clock::now();
+      auto time =
+          std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
+
+      std::array<double, 7> tau_d_rate_limited = franka::limitRate(
+          franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d);
+
+      // Set per-joint torque limits
+      std::array<double, 7> max_torques = {
+          {20.0, 20.0, 20.0, 20.0, 10.0, 10.0, 10.0}};
+      TorqueSafetyGuardFn(tau_d_rate_limited, max_torques);
+
+      return tau_d_rate_limited;
+    });
+  } catch (const franka::Exception &e) {
+    std::cerr << "Franka::osc() caught exception: " << e.what() << std::endl;
+    std::cerr << "Attempting to recover from exception..." << std::endl;
+    this->automatic_error_recovery();
+    this->running_controller = Controller::none;
+  }
 }
 
 void Franka::joint_controller() {
@@ -493,87 +502,97 @@ void Franka::joint_controller() {
   joint_max_ << 2.8978, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973;
   joint_min_ << -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973;
 
-  this->robot.control([&](const franka::RobotState& robot_state,
-                          franka::Duration period) -> franka::Torques {
-    std::chrono::high_resolution_clock::time_point t1 =
-        std::chrono::high_resolution_clock::now();
+  try {
+    this->robot.control([&](const franka::RobotState &robot_state,
+                            franka::Duration period) -> franka::Torques {
+      std::chrono::high_resolution_clock::time_point t1 =
+          std::chrono::high_resolution_clock::now();
+
+      // torques handler
+      if (this->running_controller == Controller::none) {
+        // TODO: test if this also works when the robot is moving
+        franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+        return franka::MotionFinished(zero_torques);
+      }
 
-    // torques handler
-    if (this->running_controller == Controller::none) {
-      // TODO: test if this also works when the robot is moving
-      franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
-      return franka::MotionFinished(zero_torques);
-    }
+      common::Vector7d desired_q;
+      common::Pose pose(robot_state.O_T_EE);
 
-    common::Vector7d desired_q;
-    common::Pose pose(robot_state.O_T_EE);
+      this->interpolator_mutex.lock();
+      this->curr_state = robot_state;
+      this->controller_time += period.toSec();
+      this->joint_interpolator.next_step(this->controller_time, desired_q);
+      this->interpolator_mutex.unlock();
+      // end torques handler
 
-    this->interpolator_mutex.lock();
-    this->curr_state = robot_state;
-    this->controller_time += period.toSec();
-    this->joint_interpolator.next_step(this->controller_time, desired_q);
-    this->interpolator_mutex.unlock();
-    // end torques handler
+      Eigen::Matrix<double, 7, 1> tau_d;
 
-    Eigen::Matrix<double, 7, 1> tau_d;
+      // Current joint velocity
+      Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
 
-    // Current joint velocity
-    Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
+      // Current joint position
+      Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
 
-    // Current joint position
-    Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
+      Eigen::MatrixXd joint_pos_error(7, 1);
 
-    Eigen::MatrixXd joint_pos_error(7, 1);
+      joint_pos_error << desired_q - q;
 
-    joint_pos_error << desired_q - q;
+      tau_d << Kp.cwiseProduct(joint_pos_error) - Kd.cwiseProduct(dq);
 
-    tau_d << Kp.cwiseProduct(joint_pos_error) - Kd.cwiseProduct(dq);
+      // Joint limit avoidance
+      Eigen::Matrix<double, 7, 1> dist2joint_max;
+      Eigen::Matrix<double, 7, 1> dist2joint_min;
 
-    // Joint limit avoidance
-    Eigen::Matrix<double, 7, 1> dist2joint_max;
-    Eigen::Matrix<double, 7, 1> dist2joint_min;
+      dist2joint_max = joint_max_.matrix() - q;
+      dist2joint_min = q - joint_min_.matrix();
 
-    dist2joint_max = joint_max_.matrix() - q;
-    dist2joint_min = q - joint_min_.matrix();
-
-    for (int i = 0; i < 7; i++) {
-      double limit_buffer = 0.15;  // rad
-      if (dist2joint_max[i] < limit_buffer && tau_d[i] > 0.) {
-        tau_d[i] *= dist2joint_max[i] / limit_buffer;
-      }
-      if (dist2joint_min[i] < limit_buffer && tau_d[i] < 0.) {
-        tau_d[i] *= dist2joint_min[i] / limit_buffer;
+      for (int i = 0; i < 7; i++) {
+        double limit_buffer = 0.15;  // rad
+        if (dist2joint_max[i] < limit_buffer && tau_d[i] > 0.) {
+          tau_d[i] *= dist2joint_max[i] / limit_buffer;
+        }
+        if (dist2joint_min[i] < limit_buffer && tau_d[i] < 0.) {
+          tau_d[i] *= dist2joint_min[i] / limit_buffer;
+        }
       }
-    }
-
-    std::array<double, 7> tau_d_array{};
-    Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
 
-    // Safety check for NaN/inf values
-    for (size_t i = 0; i < 7; ++i) {
-      if (!std::isfinite(tau_d_array[i])) {
-        std::cerr << "Franka::joint_controller: Commanded torque for joint " << i
-                  << " is not finite. Commanding zero torques." << std::endl;
-        std::fill(tau_d_array.begin(), tau_d_array.end(), 0.0);
-        break;
+      std::array<double, 7> tau_d_array{};
+      Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
+
+      // Safety check for NaN/inf values
+      for (size_t i = 0; i < 7; ++i) {
+        if (!std::isfinite(tau_d_array[i])) {
+          std::cerr << "Franka::joint_controller: Commanded torque for joint "
+                    << i << " is not finite. Commanding zero torques."
+                    << std::endl;
+          std::fill(tau_d_array.begin(), tau_d_array.end(), 0.0);
+          break;
+        }
       }
-    }
-
-    // end of controller
-    std::chrono::high_resolution_clock::time_point t2 =
-        std::chrono::high_resolution_clock::now();
-    auto time = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
-
-    std::array<double, 7> tau_d_rate_limited = franka::limitRate(
-        franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d);
-
-    // Set per-joint torque limits
-    std::array<double, 7> max_torques = {
-        {20.0, 20.0, 20.0, 20.0, 10.0, 10.0, 10.0}};
-    TorqueSafetyGuardFn(tau_d_rate_limited, max_torques);
 
-    return tau_d_rate_limited;
-  });
+      // end of controller
+      std::chrono::high_resolution_clock::time_point t2 =
+          std::chrono::high_resolution_clock::now();
+      auto time =
+          std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
+
+      std::array<double, 7> tau_d_rate_limited = franka::limitRate(
+          franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d);
+
+      // Set per-joint torque limits
+      std::array<double, 7> max_torques = {
+          {20.0, 20.0, 20.0, 20.0, 10.0, 10.0, 10.0}};
+      TorqueSafetyGuardFn(tau_d_rate_limited, max_torques);
+
+      return tau_d_rate_limited;
+    });
+  } catch (const franka::Exception &e) {
+    std::cerr << "Franka::joint_controller() caught exception: " << e.what()
+              << std::endl;
+    std::cerr << "Attempting to recover from exception..." << std::endl;
+    this->automatic_error_recovery();
+    this->running_controller = Controller::none;
+  }
 }
 
 void Franka::zero_torque_guiding() {
@@ -595,18 +614,26 @@ void Franka::zero_torque_controller() {
       {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
 
   this->controller_time = 0.0;
-  this->robot.control([&](const franka::RobotState& robot_state,
-                          franka::Duration period) -> franka::Torques {
-    this->interpolator_mutex.lock();
-    this->curr_state = robot_state;
-    this->controller_time += period.toSec();
-    this->interpolator_mutex.unlock();
-    if (this->running_controller == Controller::none) {
-      // stop
-      return franka::MotionFinished(franka::Torques({0, 0, 0, 0, 0, 0, 0}));
-    }
-    return franka::Torques({0, 0, 0, 0, 0, 0, 0});
-  });
+  try {
+    this->robot.control([&](const franka::RobotState &robot_state,
+                            franka::Duration period) -> franka::Torques {
+      this->interpolator_mutex.lock();
+      this->curr_state = robot_state;
+      this->controller_time += period.toSec();
+      this->interpolator_mutex.unlock();
+      if (this->running_controller == Controller::none) {
+        // stop
+        return franka::MotionFinished(franka::Torques({0, 0, 0, 0, 0, 0, 0}));
+      }
+      return franka::Torques({0, 0, 0, 0, 0, 0, 0});
+    });
+  } catch (const franka::Exception &e) {
+    std::cerr << "Franka::zero_torque_controller() caught exception: "
+              << e.what() << std::endl;
+    std::cerr << "Attempting to recover from exception..." << std::endl;
+    this->automatic_error_recovery();
+    this->running_controller = Controller::none;
+  }
 }
 
 void Franka::move_home() {

From d8f7312de79e8abef3b7a26c050e1defa754d3e6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20J=C3=BClg?= <tobias.juelg@utn.de>
Date: Fri, 28 Nov 2025 20:58:53 +0100
Subject: [PATCH 04/10] feat: improve controller

- add coriolis forces
- make pseudo inverse more stable with dampening
- ramp up with zero torques
---
 extensions/rcs_fr3/src/hw/Franka.cpp | 24 ++++++++++++++++--------
 1 file changed, 16 insertions(+), 8 deletions(-)

diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp
index 2d7ee0b5..f850fee0 100644
--- a/extensions/rcs_fr3/src/hw/Franka.cpp
+++ b/extensions/rcs_fr3/src/hw/Franka.cpp
@@ -134,8 +134,8 @@ void Franka::set_guiding_mode(bool x, bool y, bool z, bool roll, bool pitch,
   this->robot.setGuidingMode(activated, elbow);
 }
 
-void PInverse(const Eigen::MatrixXd& M, Eigen::MatrixXd& M_inv,
-              double epsilon = 0.00025) {
+void PInverse(const Eigen::MatrixXd &M, Eigen::MatrixXd &M_inv,
+              double damping_factor = 0.05) {
   Eigen::JacobiSVD<Eigen::MatrixXd> svd(
       M, Eigen::ComputeFullU | Eigen::ComputeFullV);
   Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType singular_vals =
@@ -143,12 +143,10 @@ void PInverse(const Eigen::MatrixXd& M, Eigen::MatrixXd& M_inv,
 
   Eigen::MatrixXd S_inv = M;
   S_inv.setZero();
+  double damping = damping_factor * damping_factor;
   for (int i = 0; i < singular_vals.size(); i++) {
-    if (singular_vals(i) < epsilon) {
-      S_inv(i, i) = 0.;
-    } else {
-      S_inv(i, i) = 1. / singular_vals(i);
-    }
+    S_inv(i, i) = singular_vals(i) /
+                  (singular_vals(i) * singular_vals(i) + damping);
   }
   M_inv = Eigen::MatrixXd(svd.matrixV() * S_inv * svd.matrixU().transpose());
 }
@@ -306,6 +304,14 @@ void Franka::osc() {
       //   return franka::MotionFinished(franka::Torques(tau_d_array));
       // }
 
+      this->controller_time += period.toSec();
+      // On the first few cycles, the robot state might be unstable.
+      // We'll command zero torques to give it time to stabilize.
+      if (this->controller_time < 0.005) {
+        franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+        return zero_torques;
+      }
+
       Eigen::Vector3d desired_pos_EE_in_base_frame;
       Eigen::Quaterniond desired_quat_EE_in_base_frame;
 
@@ -316,7 +322,6 @@ void Franka::osc() {
 
       this->interpolator_mutex.lock();
       this->curr_state = robot_state;
-      this->controller_time += period.toSec();
       this->traj_interpolator.next_step(this->controller_time,
                                         desired_pos_EE_in_base_frame,
                                         desired_quat_EE_in_base_frame);
@@ -438,6 +443,9 @@ void Franka::osc() {
       }
       tau_d << tau_d + Nullspace * avoidance_force;
 
+      // Add feedforward torques for coriolis forces
+      tau_d << tau_d + coriolis;
+
       std::array<double, 7> tau_d_array{};
       Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
 

From 1595fdf4a9c63e959c3de1ea1eecebace6830220 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20J=C3=BClg?= <tobias.juelg@utn.de>
Date: Fri, 28 Nov 2025 21:08:10 +0100
Subject: [PATCH 05/10] feat: safety reject for joint and osc

---
 extensions/rcs_fr3/src/hw/Franka.cpp          | 66 +++++++++++++++++--
 extensions/rcs_fr3/src/hw/Franka.h            |  5 ++
 extensions/rcs_fr3/src/pybind/rcs.cpp         |  5 +-
 .../rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi |  3 +
 4 files changed, 72 insertions(+), 7 deletions(-)

diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp
index f850fee0..ffddb229 100644
--- a/extensions/rcs_fr3/src/hw/Franka.cpp
+++ b/extensions/rcs_fr3/src/hw/Franka.cpp
@@ -166,9 +166,11 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) {
   int policy_rate = 20;
   int traj_rate = 500;
 
+  common::Vector7d current_q;
+
   if (this->running_controller == Controller::none) {
     this->controller_time = 0.0;
-    this->get_joint_position();
+    current_q = this->get_joint_position();
   } else if (this->running_controller != Controller::jsc) {
     // runtime error
     throw std::runtime_error(
@@ -177,12 +179,28 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) {
         ". To change controller type stop the current controller first.");
   } else {
     this->interpolator_mutex.lock();
+    current_q = Eigen::Map<common::Vector7d>(this->curr_state.q.data());
+  }
+
+  // Safety check for joint distance
+  if (this->cfg.max_joint_dist.has_value()) {
+    double joint_dist = (desired_q - current_q).norm();
+    if (joint_dist > this->cfg.max_joint_dist.value()) {
+      if (this->running_controller == Controller::jsc) {
+        this->interpolator_mutex.unlock();
+      }
+      throw std::runtime_error(
+          "Desired joint configuration is too far from current configuration. "
+          "Distance: " +
+          std::to_string(joint_dist) +
+          " rad, Limit: " + std::to_string(this->cfg.max_joint_dist.value()) +
+          " rad.");
+    }
   }
 
   this->joint_interpolator.reset(
-      this->controller_time,
-      Eigen::Map<common::Vector7d>(this->curr_state.q.data()), desired_q,
-      policy_rate, traj_rate, traj_interpolation_time_fraction);
+      this->controller_time, current_q, desired_q, policy_rate, traj_rate,
+      traj_interpolation_time_fraction);
 
   // if not thread is running, then start
   if (this->running_controller == Controller::none) {
@@ -201,9 +219,11 @@ void Franka::osc_set_cartesian_position(
   int policy_rate = 20;
   int traj_rate = 500;
 
+  common::Pose curr_pose;
+
   if (this->running_controller == Controller::none) {
     this->controller_time = 0.0;
-    this->get_cartesian_position();
+    curr_pose = this->get_cartesian_position();
   } else if (this->running_controller != Controller::osc) {
     throw std::runtime_error(
         "Controller type must but osc but is " +
@@ -211,9 +231,43 @@ void Franka::osc_set_cartesian_position(
         ". To change controller type stop the current controller first.");
   } else {
     this->interpolator_mutex.lock();
+    curr_pose = common::Pose(this->curr_state.O_T_EE);
+  }
+
+  // Safety check for distance
+  if (this->cfg.max_cartesian_pos_dist.has_value()) {
+    double dist = (curr_pose.translation() -
+                   desired_pose_EE_in_base_frame.translation())
+                      .norm();
+    if (dist > this->cfg.max_cartesian_pos_dist.value()) {
+      if (this->running_controller == Controller::osc) {
+        this->interpolator_mutex.unlock();
+      }
+      throw std::runtime_error(
+          "Desired cartesian position is too far from current position (dist: " +
+          std::to_string(dist) +
+          "m, max: " + std::to_string(cfg.max_cartesian_pos_dist.value()) +
+          "m)");
+    }
+  }
+  // Safety check for orientation
+  if (this->cfg.max_cartesian_ori_dist.has_value()) {
+    Eigen::Quaterniond q1 = curr_pose.quaternion();
+    Eigen::Quaterniond q2 = desired_pose_EE_in_base_frame.quaternion();
+    double angle = Eigen::AngleAxisd(q1.inverse() * q2).angle();
+    if (angle > this->cfg.max_cartesian_ori_dist.value()) {
+      if (this->running_controller == Controller::osc) {
+        this->interpolator_mutex.unlock();
+      }
+      throw std::runtime_error(
+          "Desired cartesian orientation is too far from current orientation "
+          "(dist: " +
+          std::to_string(angle) +
+          "rad, max: " + std::to_string(cfg.max_cartesian_ori_dist.value()) +
+          "rad)");
+    }
   }
 
-  common::Pose curr_pose(this->curr_state.O_T_EE);
   this->traj_interpolator.reset(
       this->controller_time, curr_pose.translation(), curr_pose.quaternion(),
       desired_pose_EE_in_base_frame.translation(),
diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h
index fe4a5af4..45d9b9ed 100644
--- a/extensions/rcs_fr3/src/hw/Franka.h
+++ b/extensions/rcs_fr3/src/hw/Franka.h
@@ -42,6 +42,11 @@ 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;
+
+  // Safety limits for rejecting distant targets
+  std::optional<double> max_cartesian_pos_dist = 0.3;   // in meters
+  std::optional<double> max_cartesian_ori_dist = 1.57;  // in radians (90 deg)
+  std::optional<double> max_joint_dist = 1.5;  // norm of joint vector in radians
 };
 
 struct FR3Config : FrankaConfig {};
diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp
index ea56afff..35726e7b 100644
--- a/extensions/rcs_fr3/src/pybind/rcs.cpp
+++ b/extensions/rcs_fr3/src/pybind/rcs.cpp
@@ -66,7 +66,10 @@ 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("async_control", &rcs::hw::FrankaConfig::async_control);
+      .def_readwrite("async_control", &rcs::hw::FrankaConfig::async_control)
+      .def_readwrite("max_cartesian_pos_dist", &rcs::hw::FrankaConfig::max_cartesian_pos_dist)
+      .def_readwrite("max_cartesian_ori_dist", &rcs::hw::FrankaConfig::max_cartesian_ori_dist)
+      .def_readwrite("max_joint_dist", &rcs::hw::FrankaConfig::max_joint_dist);
 
   py::class_<rcs::hw::FR3Config, rcs::hw::FrankaConfig>(hw, "FR3Config")
       .def(py::init<>());
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 006e7e50..4ff1106e 100644
--- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi
+++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi
@@ -86,6 +86,9 @@ class FrankaConfig(rcs._core.common.RobotConfig):
     async_control: bool
     ik_solver: IKSolver
     load_parameters: FrankaLoad | None
+    max_cartesian_ori_dist: float | None
+    max_cartesian_pos_dist: float | None
+    max_joint_dist: float | None
     nominal_end_effector_frame: rcs._core.common.Pose | None
     speed_factor: float
     world_to_robot: rcs._core.common.Pose | None

From 0ea25723f99acdf611fea60a5e35fa44ebf90f7f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20J=C3=BClg?= <tobias.juelg@utn.de>
Date: Fri, 28 Nov 2025 21:08:37 +0100
Subject: [PATCH 06/10] style: format

---
 extensions/rcs_fr3/src/hw/Franka.cpp  | 34 +++++++++++++--------------
 extensions/rcs_fr3/src/hw/Franka.h    |  3 ++-
 extensions/rcs_fr3/src/pybind/rcs.cpp |  6 +++--
 3 files changed, 22 insertions(+), 21 deletions(-)

diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp
index ffddb229..f50a5fae 100644
--- a/extensions/rcs_fr3/src/hw/Franka.cpp
+++ b/extensions/rcs_fr3/src/hw/Franka.cpp
@@ -145,8 +145,8 @@ void PInverse(const Eigen::MatrixXd &M, Eigen::MatrixXd &M_inv,
   S_inv.setZero();
   double damping = damping_factor * damping_factor;
   for (int i = 0; i < singular_vals.size(); i++) {
-    S_inv(i, i) = singular_vals(i) /
-                  (singular_vals(i) * singular_vals(i) + damping);
+    S_inv(i, i) =
+        singular_vals(i) / (singular_vals(i) * singular_vals(i) + damping);
   }
   M_inv = Eigen::MatrixXd(svd.matrixV() * S_inv * svd.matrixU().transpose());
 }
@@ -192,15 +192,14 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) {
       throw std::runtime_error(
           "Desired joint configuration is too far from current configuration. "
           "Distance: " +
-          std::to_string(joint_dist) +
-          " rad, Limit: " + std::to_string(this->cfg.max_joint_dist.value()) +
-          " rad.");
+          std::to_string(joint_dist) + " rad, Limit: " +
+          std::to_string(this->cfg.max_joint_dist.value()) + " rad.");
     }
   }
 
-  this->joint_interpolator.reset(
-      this->controller_time, current_q, desired_q, policy_rate, traj_rate,
-      traj_interpolation_time_fraction);
+  this->joint_interpolator.reset(this->controller_time, current_q, desired_q,
+                                 policy_rate, traj_rate,
+                                 traj_interpolation_time_fraction);
 
   // if not thread is running, then start
   if (this->running_controller == Controller::none) {
@@ -236,18 +235,18 @@ void Franka::osc_set_cartesian_position(
 
   // Safety check for distance
   if (this->cfg.max_cartesian_pos_dist.has_value()) {
-    double dist = (curr_pose.translation() -
-                   desired_pose_EE_in_base_frame.translation())
-                      .norm();
+    double dist =
+        (curr_pose.translation() - desired_pose_EE_in_base_frame.translation())
+            .norm();
     if (dist > this->cfg.max_cartesian_pos_dist.value()) {
       if (this->running_controller == Controller::osc) {
         this->interpolator_mutex.unlock();
       }
       throw std::runtime_error(
-          "Desired cartesian position is too far from current position (dist: " +
-          std::to_string(dist) +
-          "m, max: " + std::to_string(cfg.max_cartesian_pos_dist.value()) +
-          "m)");
+          "Desired cartesian position is too far from current position "
+          "(dist: " +
+          std::to_string(dist) + "m, max: " +
+          std::to_string(cfg.max_cartesian_pos_dist.value()) + "m)");
     }
   }
   // Safety check for orientation
@@ -262,9 +261,8 @@ void Franka::osc_set_cartesian_position(
       throw std::runtime_error(
           "Desired cartesian orientation is too far from current orientation "
           "(dist: " +
-          std::to_string(angle) +
-          "rad, max: " + std::to_string(cfg.max_cartesian_ori_dist.value()) +
-          "rad)");
+          std::to_string(angle) + "rad, max: " +
+          std::to_string(cfg.max_cartesian_ori_dist.value()) + "rad)");
     }
   }
 
diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h
index 45d9b9ed..f3ec6588 100644
--- a/extensions/rcs_fr3/src/hw/Franka.h
+++ b/extensions/rcs_fr3/src/hw/Franka.h
@@ -46,7 +46,8 @@ struct FrankaConfig : common::RobotConfig {
   // Safety limits for rejecting distant targets
   std::optional<double> max_cartesian_pos_dist = 0.3;   // in meters
   std::optional<double> max_cartesian_ori_dist = 1.57;  // in radians (90 deg)
-  std::optional<double> max_joint_dist = 1.5;  // norm of joint vector in radians
+  std::optional<double> max_joint_dist =
+      1.5;  // norm of joint vector in radians
 };
 
 struct FR3Config : FrankaConfig {};
diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp
index 35726e7b..26287a85 100644
--- a/extensions/rcs_fr3/src/pybind/rcs.cpp
+++ b/extensions/rcs_fr3/src/pybind/rcs.cpp
@@ -67,8 +67,10 @@ PYBIND11_MODULE(_core, m) {
                      &rcs::hw::FrankaConfig::nominal_end_effector_frame)
       .def_readwrite("world_to_robot", &rcs::hw::FrankaConfig::world_to_robot)
       .def_readwrite("async_control", &rcs::hw::FrankaConfig::async_control)
-      .def_readwrite("max_cartesian_pos_dist", &rcs::hw::FrankaConfig::max_cartesian_pos_dist)
-      .def_readwrite("max_cartesian_ori_dist", &rcs::hw::FrankaConfig::max_cartesian_ori_dist)
+      .def_readwrite("max_cartesian_pos_dist",
+                     &rcs::hw::FrankaConfig::max_cartesian_pos_dist)
+      .def_readwrite("max_cartesian_ori_dist",
+                     &rcs::hw::FrankaConfig::max_cartesian_ori_dist)
       .def_readwrite("max_joint_dist", &rcs::hw::FrankaConfig::max_joint_dist);
 
   py::class_<rcs::hw::FR3Config, rcs::hw::FrankaConfig>(hw, "FR3Config")

From 0297dee2f3838e2ff0a7f9549b1fb6163e40ed06 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20J=C3=BClg?= <tobias.juelg@utn.de>
Date: Fri, 28 Nov 2025 21:59:31 +0100
Subject: [PATCH 07/10] feat: add franka state

---
 extensions/rcs_fr3/src/hw/Franka.cpp          |  13 +-
 extensions/rcs_fr3/src/hw/Franka.h            |   5 +-
 extensions/rcs_fr3/src/pybind/rcs.cpp         |  69 ++++-
 .../rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi | 244 +++++++++++++++++-
 4 files changed, 323 insertions(+), 8 deletions(-)

diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp
index f50a5fae..d36f1993 100644
--- a/extensions/rcs_fr3/src/hw/Franka.cpp
+++ b/extensions/rcs_fr3/src/hw/Franka.cpp
@@ -73,9 +73,16 @@ FrankaConfig* Franka::get_config() {
   return cfg;
 }
 
-FrankaState* Franka::get_state() {
-  // dummy state until we define a prober state
-  FrankaState* state = new FrankaState();
+FrankaState *Franka::get_state() {
+  franka::RobotState current_robot_state;
+  if (this->running_controller == Controller::none) {
+    current_robot_state = this->robot.readOnce();
+  } else {
+    std::lock_guard<std::mutex> lock(this->interpolator_mutex);
+    current_robot_state = this->curr_state;
+  }
+  auto *state = new FrankaState();
+  state->robot_state = current_robot_state;
   return state;
 }
 
diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h
index f3ec6588..cd75ffc1 100644
--- a/extensions/rcs_fr3/src/hw/Franka.h
+++ b/extensions/rcs_fr3/src/hw/Franka.h
@@ -2,6 +2,7 @@
 #define RCS_FRANKA_H
 
 #include <franka/robot.h>
+#include <franka/robot_state.h>
 
 #include <cmath>
 #include <memory>
@@ -55,7 +56,9 @@ struct PandaConfig : FrankaConfig {
   common::RobotType robot_type = common::RobotType::Panda;
 };
 
-struct FrankaState : common::RobotState {};
+struct FrankaState : common::RobotState {
+  franka::RobotState robot_state;
+};
 
 class Franka : public common::Robot {
  private:
diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp
index 26287a85..ba19cff8 100644
--- a/extensions/rcs_fr3/src/pybind/rcs.cpp
+++ b/extensions/rcs_fr3/src/pybind/rcs.cpp
@@ -1,4 +1,5 @@
 #include <franka/exception.h>
+#include <franka/robot_state.h>
 #include <hw/Franka.h>
 #include <hw/FrankaHand.h>
 #include <pybind11/cast.h>
@@ -41,10 +42,76 @@ PYBIND11_MODULE(_core, m) {
   // HARDWARE MODULE
   auto hw = m.def_submodule("hw", "rcs franka module");
 
+  py::enum_<franka::RobotMode>(hw, "RobotMode")
+      .value("kOther", franka::RobotMode::kOther)
+      .value("kIdle", franka::RobotMode::kIdle)
+      .value("kMove", franka::RobotMode::kMove)
+      .value("kGuiding", franka::RobotMode::kGuiding)
+      .value("kReflex", franka::RobotMode::kReflex)
+      .value("kUserStopped", franka::RobotMode::kUserStopped)
+      .value("kAutomaticErrorRecovery",
+             franka::RobotMode::kAutomaticErrorRecovery)
+      .export_values();
+
+  py::class_<franka::RobotState>(hw, "RobotState")
+      .def(py::init<>())
+      .def_readonly("O_T_EE", &franka::RobotState::O_T_EE)
+      .def_readonly("O_T_EE_d", &franka::RobotState::O_T_EE_d)
+      .def_readonly("F_T_EE", &franka::RobotState::F_T_EE)
+      .def_readonly("F_T_NE", &franka::RobotState::F_T_NE)
+      .def_readonly("NE_T_EE", &franka::RobotState::NE_T_EE)
+      .def_readonly("EE_T_K", &franka::RobotState::EE_T_K)
+      .def_readonly("m_ee", &franka::RobotState::m_ee)
+      .def_readonly("I_ee", &franka::RobotState::I_ee)
+      .def_readonly("F_x_Cee", &franka::RobotState::F_x_Cee)
+      .def_readonly("m_load", &franka::RobotState::m_load)
+      .def_readonly("I_load", &franka::RobotState::I_load)
+      .def_readonly("F_x_Cload", &franka::RobotState::F_x_Cload)
+      .def_readonly("m_total", &franka::RobotState::m_total)
+      .def_readonly("I_total", &franka::RobotState::I_total)
+      .def_readonly("F_x_Ctotal", &franka::RobotState::F_x_Ctotal)
+      .def_readonly("elbow", &franka::RobotState::elbow)
+      .def_readonly("elbow_d", &franka::RobotState::elbow_d)
+      .def_readonly("elbow_c", &franka::RobotState::elbow_c)
+      .def_readonly("delbow_c", &franka::RobotState::delbow_c)
+      .def_readonly("ddelbow_c", &franka::RobotState::ddelbow_c)
+      .def_readonly("tau_J", &franka::RobotState::tau_J)
+      .def_readonly("tau_J_d", &franka::RobotState::tau_J_d)
+      .def_readonly("dtau_J", &franka::RobotState::dtau_J)
+      .def_readonly("q", &franka::RobotState::q)
+      .def_readonly("q_d", &franka::RobotState::q_d)
+      .def_readonly("dq", &franka::RobotState::dq)
+      .def_readonly("dq_d", &franka::RobotState::dq_d)
+      .def_readonly("ddq_d", &franka::RobotState::ddq_d)
+      .def_readonly("joint_contact", &franka::RobotState::joint_contact)
+      .def_readonly("cartesian_contact", &franka::RobotState::cartesian_contact)
+      .def_readonly("joint_collision", &franka::RobotState::joint_collision)
+      .def_readonly("cartesian_collision",
+                    &franka::RobotState::cartesian_collision)
+      .def_readonly("tau_ext_hat_filtered",
+                    &franka::RobotState::tau_ext_hat_filtered)
+      .def_readonly("O_F_ext_hat_K", &franka::RobotState::O_F_ext_hat_K)
+      .def_readonly("K_F_ext_hat_K", &franka::RobotState::K_F_ext_hat_K)
+      .def_readonly("O_dP_EE_d", &franka::RobotState::O_dP_EE_d)
+      .def_readonly("O_ddP_O", &franka::RobotState::O_ddP_O)
+      .def_readonly("O_T_EE_c", &franka::RobotState::O_T_EE_c)
+      .def_readonly("O_dP_EE_c", &franka::RobotState::O_dP_EE_c)
+      .def_readonly("O_ddP_EE_c", &franka::RobotState::O_ddP_EE_c)
+      .def_readonly("theta", &franka::RobotState::theta)
+      .def_readonly("dtheta", &franka::RobotState::dtheta)
+      .def_readonly("current_errors", &franka::RobotState::current_errors)
+      .def_readonly("last_motion_errors",
+                    &franka::RobotState::last_motion_errors)
+      .def_readonly("control_command_success_rate",
+                    &franka::RobotState::control_command_success_rate)
+      .def_readonly("robot_mode", &franka::RobotState::robot_mode)
+      .def_readonly("time", &franka::RobotState::time);
+
   py::object robot_state =
       (py::object)py::module_::import("rcs").attr("common").attr("RobotState");
   py::class_<rcs::hw::FrankaState>(hw, "FrankaState", robot_state)
-      .def(py::init<>());
+      .def(py::init<>())
+      .def_readonly("robot_state", &rcs::hw::FrankaState::robot_state);
   py::class_<rcs::hw::FrankaLoad>(hw, "FrankaLoad")
       .def(py::init<>())
       .def_readwrite("load_mass", &rcs::hw::FrankaLoad::load_mass)
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 4ff1106e..6ef5a678 100644
--- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi
+++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi
@@ -7,6 +7,7 @@ from __future__ import annotations
 import typing
 
 import numpy
+import pybind11_stubgen.typing_ext
 import rcs._core.common
 
 from . import exceptions
@@ -22,8 +23,17 @@ __all__: list[str] = [
     "FrankaState",
     "IKSolver",
     "PandaConfig",
+    "RobotMode",
+    "RobotState",
     "exceptions",
     "franka_ik",
+    "kAutomaticErrorRecovery",
+    "kGuiding",
+    "kIdle",
+    "kMove",
+    "kOther",
+    "kReflex",
+    "kUserStopped",
     "rcs_ik",
 ]
 
@@ -109,9 +119,6 @@ class FrankaLoad:
     load_mass: float
     def __init__(self) -> None: ...
 
-class FrankaState(rcs._core.common.RobotState):
-    def __init__(self) -> None: ...
-
 class IKSolver:
     """
     Members:
@@ -141,11 +148,242 @@ class IKSolver:
     @property
     def value(self) -> int: ...
 
+class RobotMode:
+    """
+    Members:
+
+      kOther
+
+      kIdle
+
+      kMove
+
+      kGuiding
+
+      kReflex
+
+      kUserStopped
+
+      kAutomaticErrorRecovery
+    """
+
+    __members__: typing.ClassVar[
+        dict[str, RobotMode]
+    ]  # value = {'kOther': <RobotMode.kOther: 0>, 'kIdle': <RobotMode.kIdle: 1>, 'kMove': <RobotMode.kMove: 2>, 'kGuiding': <RobotMode.kGuiding: 3>, 'kReflex': <RobotMode.kReflex: 4>, 'kUserStopped': <RobotMode.kUserStopped: 5>, 'kAutomaticErrorRecovery': <RobotMode.kAutomaticErrorRecovery: 6>}
+    kAutomaticErrorRecovery: typing.ClassVar[RobotMode]  # value = <RobotMode.kAutomaticErrorRecovery: 6>
+    kGuiding: typing.ClassVar[RobotMode]  # value = <RobotMode.kGuiding: 3>
+    kIdle: typing.ClassVar[RobotMode]  # value = <RobotMode.kIdle: 1>
+    kMove: typing.ClassVar[RobotMode]  # value = <RobotMode.kMove: 2>
+    kOther: typing.ClassVar[RobotMode]  # value = <RobotMode.kOther: 0>
+    kReflex: typing.ClassVar[RobotMode]  # value = <RobotMode.kReflex: 4>
+    kUserStopped: typing.ClassVar[RobotMode]  # value = <RobotMode.kUserStopped: 5>
+    def __eq__(self, other: typing.Any) -> bool: ...
+    def __getstate__(self) -> int: ...
+    def __hash__(self) -> int: ...
+    def __index__(self) -> int: ...
+    def __init__(self, value: int) -> None: ...
+    def __int__(self) -> int: ...
+    def __ne__(self, other: typing.Any) -> bool: ...
+    def __repr__(self) -> str: ...
+    def __setstate__(self, state: int) -> None: ...
+    def __str__(self) -> str: ...
+    @property
+    def name(self) -> str: ...
+    @property
+    def value(self) -> int: ...
+
+class RobotState:
+    def __init__(self) -> None: ...
+    @property
+    def EE_T_K(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def F_T_EE(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def F_T_NE(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def F_x_Cee(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    @property
+    def F_x_Cload(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    @property
+    def F_x_Ctotal(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    @property
+    def I_ee(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
+    @property
+    def I_load(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
+    @property
+    def I_total(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
+    @property
+    def K_F_ext_hat_K(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def NE_T_EE(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def O_F_ext_hat_K(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def O_T_EE(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def O_T_EE_c(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def O_T_EE_d(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def O_dP_EE_c(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def O_dP_EE_d(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def O_ddP_EE_c(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def O_ddP_O(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    @property
+    def cartesian_collision(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def cartesian_contact(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def control_command_success_rate(self) -> float: ...
+    @property
+    def current_errors(self) -> ...: ...
+    @property
+    def ddelbow_c(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def ddq_d(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def delbow_c(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def dq(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def dq_d(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def dtau_J(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def dtheta(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def elbow(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def elbow_c(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def elbow_d(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def joint_collision(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def joint_contact(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def last_motion_errors(self) -> ...: ...
+    @property
+    def m_ee(self) -> float: ...
+    @property
+    def m_load(self) -> float: ...
+    @property
+    def m_total(self) -> float: ...
+    @property
+    def q(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def q_d(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def robot_mode(self) -> RobotMode: ...
+    @property
+    def tau_J(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def tau_J_d(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def tau_ext_hat_filtered(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def theta(
+        self,
+    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def time(self) -> ...: ...
+
 class FR3Config(FrankaConfig):
     def __init__(self) -> None: ...
 
 class PandaConfig(FrankaConfig):
     def __init__(self) -> None: ...
 
+class FrankaState(rcs._core.common.RobotState):
+    def __init__(self) -> None: ...
+    @property
+    def robot_state(self) -> RobotState: ...
+
 franka_ik: IKSolver  # value = <IKSolver.franka_ik: 0>
+kAutomaticErrorRecovery: RobotMode  # value = <RobotMode.kAutomaticErrorRecovery: 6>
+kGuiding: RobotMode  # value = <RobotMode.kGuiding: 3>
+kIdle: RobotMode  # value = <RobotMode.kIdle: 1>
+kMove: RobotMode  # value = <RobotMode.kMove: 2>
+kOther: RobotMode  # value = <RobotMode.kOther: 0>
+kReflex: RobotMode  # value = <RobotMode.kReflex: 4>
+kUserStopped: RobotMode  # value = <RobotMode.kUserStopped: 5>
 rcs_ik: IKSolver  # value = <IKSolver.rcs_ik: 1>

From 9057c515c445ddd37feb86a38571d3e12e297241 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20J=C3=BClg?= <tobias.juelg@utn.de>
Date: Wed, 3 Dec 2025 09:14:21 +0100
Subject: [PATCH 08/10] style: stubfile generation and linting

---
 .../rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi | 156 ++++------------
 .../src/rcs_panda/_core/hw/__init__.pyi       | 169 +++++++++++++++++-
 2 files changed, 205 insertions(+), 120 deletions(-)

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 6ef5a678..dc5632f3 100644
--- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi
+++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi
@@ -195,141 +195,75 @@ class RobotMode:
 class RobotState:
     def __init__(self) -> None: ...
     @property
-    def EE_T_K(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    def EE_T_K(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
     @property
-    def F_T_EE(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    def F_T_EE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
     @property
-    def F_T_NE(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    def F_T_NE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
     @property
-    def F_x_Cee(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    def F_x_Cee(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
     @property
-    def F_x_Cload(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    def F_x_Cload(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
     @property
-    def F_x_Ctotal(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    def F_x_Ctotal(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
     @property
-    def I_ee(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
+    def I_ee(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
     @property
-    def I_load(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
+    def I_load(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
     @property
-    def I_total(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
+    def I_total(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
     @property
-    def K_F_ext_hat_K(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    def K_F_ext_hat_K(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
     @property
-    def NE_T_EE(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    def NE_T_EE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
     @property
-    def O_F_ext_hat_K(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    def O_F_ext_hat_K(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
     @property
-    def O_T_EE(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    def O_T_EE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
     @property
-    def O_T_EE_c(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    def O_T_EE_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
     @property
-    def O_T_EE_d(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    def O_T_EE_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
     @property
-    def O_dP_EE_c(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    def O_dP_EE_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
     @property
-    def O_dP_EE_d(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    def O_dP_EE_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
     @property
-    def O_ddP_EE_c(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    def O_ddP_EE_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
     @property
-    def O_ddP_O(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    def O_ddP_O(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
     @property
-    def cartesian_collision(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    def cartesian_collision(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
     @property
-    def cartesian_contact(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    def cartesian_contact(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
     @property
     def control_command_success_rate(self) -> float: ...
     @property
     def current_errors(self) -> ...: ...
     @property
-    def ddelbow_c(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    def ddelbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
     @property
-    def ddq_d(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    def ddq_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
-    def delbow_c(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    def delbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
     @property
-    def dq(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    def dq(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
-    def dq_d(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    def dq_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
-    def dtau_J(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    def dtau_J(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
-    def dtheta(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    def dtheta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
-    def elbow(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    def elbow(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
     @property
-    def elbow_c(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    def elbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
     @property
-    def elbow_d(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    def elbow_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
     @property
-    def joint_collision(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    def joint_collision(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
-    def joint_contact(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    def joint_contact(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
     def last_motion_errors(self) -> ...: ...
     @property
@@ -339,31 +273,19 @@ class RobotState:
     @property
     def m_total(self) -> float: ...
     @property
-    def q(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    def q(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
-    def q_d(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    def q_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
     def robot_mode(self) -> RobotMode: ...
     @property
-    def tau_J(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    def tau_J(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
-    def tau_J_d(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    def tau_J_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
-    def tau_ext_hat_filtered(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    def tau_ext_hat_filtered(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
-    def theta(
-        self,
-    ) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    def theta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
     def time(self) -> ...: ...
 
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 006e7e50..dc5632f3 100644
--- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi
+++ b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi
@@ -7,6 +7,7 @@ from __future__ import annotations
 import typing
 
 import numpy
+import pybind11_stubgen.typing_ext
 import rcs._core.common
 
 from . import exceptions
@@ -22,8 +23,17 @@ __all__: list[str] = [
     "FrankaState",
     "IKSolver",
     "PandaConfig",
+    "RobotMode",
+    "RobotState",
     "exceptions",
     "franka_ik",
+    "kAutomaticErrorRecovery",
+    "kGuiding",
+    "kIdle",
+    "kMove",
+    "kOther",
+    "kReflex",
+    "kUserStopped",
     "rcs_ik",
 ]
 
@@ -86,6 +96,9 @@ class FrankaConfig(rcs._core.common.RobotConfig):
     async_control: bool
     ik_solver: IKSolver
     load_parameters: FrankaLoad | None
+    max_cartesian_ori_dist: float | None
+    max_cartesian_pos_dist: float | None
+    max_joint_dist: float | None
     nominal_end_effector_frame: rcs._core.common.Pose | None
     speed_factor: float
     world_to_robot: rcs._core.common.Pose | None
@@ -106,9 +119,6 @@ class FrankaLoad:
     load_mass: float
     def __init__(self) -> None: ...
 
-class FrankaState(rcs._core.common.RobotState):
-    def __init__(self) -> None: ...
-
 class IKSolver:
     """
     Members:
@@ -138,11 +148,164 @@ class IKSolver:
     @property
     def value(self) -> int: ...
 
+class RobotMode:
+    """
+    Members:
+
+      kOther
+
+      kIdle
+
+      kMove
+
+      kGuiding
+
+      kReflex
+
+      kUserStopped
+
+      kAutomaticErrorRecovery
+    """
+
+    __members__: typing.ClassVar[
+        dict[str, RobotMode]
+    ]  # value = {'kOther': <RobotMode.kOther: 0>, 'kIdle': <RobotMode.kIdle: 1>, 'kMove': <RobotMode.kMove: 2>, 'kGuiding': <RobotMode.kGuiding: 3>, 'kReflex': <RobotMode.kReflex: 4>, 'kUserStopped': <RobotMode.kUserStopped: 5>, 'kAutomaticErrorRecovery': <RobotMode.kAutomaticErrorRecovery: 6>}
+    kAutomaticErrorRecovery: typing.ClassVar[RobotMode]  # value = <RobotMode.kAutomaticErrorRecovery: 6>
+    kGuiding: typing.ClassVar[RobotMode]  # value = <RobotMode.kGuiding: 3>
+    kIdle: typing.ClassVar[RobotMode]  # value = <RobotMode.kIdle: 1>
+    kMove: typing.ClassVar[RobotMode]  # value = <RobotMode.kMove: 2>
+    kOther: typing.ClassVar[RobotMode]  # value = <RobotMode.kOther: 0>
+    kReflex: typing.ClassVar[RobotMode]  # value = <RobotMode.kReflex: 4>
+    kUserStopped: typing.ClassVar[RobotMode]  # value = <RobotMode.kUserStopped: 5>
+    def __eq__(self, other: typing.Any) -> bool: ...
+    def __getstate__(self) -> int: ...
+    def __hash__(self) -> int: ...
+    def __index__(self) -> int: ...
+    def __init__(self, value: int) -> None: ...
+    def __int__(self) -> int: ...
+    def __ne__(self, other: typing.Any) -> bool: ...
+    def __repr__(self) -> str: ...
+    def __setstate__(self, state: int) -> None: ...
+    def __str__(self) -> str: ...
+    @property
+    def name(self) -> str: ...
+    @property
+    def value(self) -> int: ...
+
+class RobotState:
+    def __init__(self) -> None: ...
+    @property
+    def EE_T_K(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def F_T_EE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def F_T_NE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def F_x_Cee(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    @property
+    def F_x_Cload(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    @property
+    def F_x_Ctotal(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    @property
+    def I_ee(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
+    @property
+    def I_load(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
+    @property
+    def I_total(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(9)]: ...
+    @property
+    def K_F_ext_hat_K(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def NE_T_EE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def O_F_ext_hat_K(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def O_T_EE(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def O_T_EE_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def O_T_EE_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
+    @property
+    def O_dP_EE_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def O_dP_EE_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def O_ddP_EE_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def O_ddP_O(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(3)]: ...
+    @property
+    def cartesian_collision(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def cartesian_contact(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(6)]: ...
+    @property
+    def control_command_success_rate(self) -> float: ...
+    @property
+    def current_errors(self) -> ...: ...
+    @property
+    def ddelbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def ddq_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def delbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def dq(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def dq_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def dtau_J(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def dtheta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def elbow(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def elbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def elbow_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
+    @property
+    def joint_collision(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def joint_contact(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def last_motion_errors(self) -> ...: ...
+    @property
+    def m_ee(self) -> float: ...
+    @property
+    def m_load(self) -> float: ...
+    @property
+    def m_total(self) -> float: ...
+    @property
+    def q(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def q_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def robot_mode(self) -> RobotMode: ...
+    @property
+    def tau_J(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def tau_J_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def tau_ext_hat_filtered(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def theta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
+    @property
+    def time(self) -> ...: ...
+
 class FR3Config(FrankaConfig):
     def __init__(self) -> None: ...
 
 class PandaConfig(FrankaConfig):
     def __init__(self) -> None: ...
 
+class FrankaState(rcs._core.common.RobotState):
+    def __init__(self) -> None: ...
+    @property
+    def robot_state(self) -> RobotState: ...
+
 franka_ik: IKSolver  # value = <IKSolver.franka_ik: 0>
+kAutomaticErrorRecovery: RobotMode  # value = <RobotMode.kAutomaticErrorRecovery: 6>
+kGuiding: RobotMode  # value = <RobotMode.kGuiding: 3>
+kIdle: RobotMode  # value = <RobotMode.kIdle: 1>
+kMove: RobotMode  # value = <RobotMode.kMove: 2>
+kOther: RobotMode  # value = <RobotMode.kOther: 0>
+kReflex: RobotMode  # value = <RobotMode.kReflex: 4>
+kUserStopped: RobotMode  # value = <RobotMode.kUserStopped: 5>
 rcs_ik: IKSolver  # value = <IKSolver.rcs_ik: 1>

From 6c00126e33157aee9f2b65272e62e0c5a009abb4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20J=C3=BClg?= <tobias.juelg@utn.de>
Date: Wed, 3 Dec 2025 09:31:56 +0100
Subject: [PATCH 09/10] fix: remove libfranka unbound state errors

---
 extensions/rcs_fr3/src/pybind/rcs.cpp                  | 10 +++++-----
 extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi   |  6 ------
 .../rcs_panda/src/rcs_panda/_core/hw/__init__.pyi      |  6 ------
 3 files changed, 5 insertions(+), 17 deletions(-)

diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp
index ba19cff8..af1be351 100644
--- a/extensions/rcs_fr3/src/pybind/rcs.cpp
+++ b/extensions/rcs_fr3/src/pybind/rcs.cpp
@@ -99,13 +99,13 @@ PYBIND11_MODULE(_core, m) {
       .def_readonly("O_ddP_EE_c", &franka::RobotState::O_ddP_EE_c)
       .def_readonly("theta", &franka::RobotState::theta)
       .def_readonly("dtheta", &franka::RobotState::dtheta)
-      .def_readonly("current_errors", &franka::RobotState::current_errors)
-      .def_readonly("last_motion_errors",
-                    &franka::RobotState::last_motion_errors)
+    //   .def_readonly("current_errors", &franka::RobotState::current_errors)
+    //   .def_readonly("last_motion_errors",
+    //                 &franka::RobotState::last_motion_errors)
       .def_readonly("control_command_success_rate",
                     &franka::RobotState::control_command_success_rate)
-      .def_readonly("robot_mode", &franka::RobotState::robot_mode)
-      .def_readonly("time", &franka::RobotState::time);
+    //   .def_readonly("time", &franka::RobotState::time)
+      .def_readonly("robot_mode", &franka::RobotState::robot_mode);
 
   py::object robot_state =
       (py::object)py::module_::import("rcs").attr("common").attr("RobotState");
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 dc5632f3..eb16d931 100644
--- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi
+++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi
@@ -239,8 +239,6 @@ class RobotState:
     @property
     def control_command_success_rate(self) -> float: ...
     @property
-    def current_errors(self) -> ...: ...
-    @property
     def ddelbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
     @property
     def ddq_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
@@ -265,8 +263,6 @@ class RobotState:
     @property
     def joint_contact(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
-    def last_motion_errors(self) -> ...: ...
-    @property
     def m_ee(self) -> float: ...
     @property
     def m_load(self) -> float: ...
@@ -286,8 +282,6 @@ class RobotState:
     def tau_ext_hat_filtered(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
     def theta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
-    @property
-    def time(self) -> ...: ...
 
 class FR3Config(FrankaConfig):
     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 dc5632f3..eb16d931 100644
--- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi
+++ b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi
@@ -239,8 +239,6 @@ class RobotState:
     @property
     def control_command_success_rate(self) -> float: ...
     @property
-    def current_errors(self) -> ...: ...
-    @property
     def ddelbow_c(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(2)]: ...
     @property
     def ddq_d(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
@@ -265,8 +263,6 @@ class RobotState:
     @property
     def joint_contact(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
-    def last_motion_errors(self) -> ...: ...
-    @property
     def m_ee(self) -> float: ...
     @property
     def m_load(self) -> float: ...
@@ -286,8 +282,6 @@ class RobotState:
     def tau_ext_hat_filtered(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
     @property
     def theta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ...
-    @property
-    def time(self) -> ...: ...
 
 class FR3Config(FrankaConfig):
     def __init__(self) -> None: ...

From df600fd498a7c1f65e23b2188874d1d930f96df8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20J=C3=BClg?= <tobias.juelg@utn.de>
Date: Wed, 3 Dec 2025 20:47:15 +0100
Subject: [PATCH 10/10] style: format cpp

---
 extensions/rcs_fr3/src/hw/Franka.cpp  | 22 +++++++++++-----------
 extensions/rcs_fr3/src/pybind/rcs.cpp |  8 ++++----
 2 files changed, 15 insertions(+), 15 deletions(-)

diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp
index d36f1993..b789858b 100644
--- a/extensions/rcs_fr3/src/hw/Franka.cpp
+++ b/extensions/rcs_fr3/src/hw/Franka.cpp
@@ -73,7 +73,7 @@ FrankaConfig* Franka::get_config() {
   return cfg;
 }
 
-FrankaState *Franka::get_state() {
+FrankaState* Franka::get_state() {
   franka::RobotState current_robot_state;
   if (this->running_controller == Controller::none) {
     current_robot_state = this->robot.readOnce();
@@ -81,7 +81,7 @@ FrankaState *Franka::get_state() {
     std::lock_guard<std::mutex> lock(this->interpolator_mutex);
     current_robot_state = this->curr_state;
   }
-  auto *state = new FrankaState();
+  auto* state = new FrankaState();
   state->robot_state = current_robot_state;
   return state;
 }
@@ -141,7 +141,7 @@ void Franka::set_guiding_mode(bool x, bool y, bool z, bool roll, bool pitch,
   this->robot.setGuidingMode(activated, elbow);
 }
 
-void PInverse(const Eigen::MatrixXd &M, Eigen::MatrixXd &M_inv,
+void PInverse(const Eigen::MatrixXd& M, Eigen::MatrixXd& M_inv,
               double damping_factor = 0.05) {
   Eigen::JacobiSVD<Eigen::MatrixXd> svd(
       M, Eigen::ComputeFullU | Eigen::ComputeFullV);
@@ -158,8 +158,8 @@ void PInverse(const Eigen::MatrixXd &M, Eigen::MatrixXd &M_inv,
   M_inv = Eigen::MatrixXd(svd.matrixV() * S_inv * svd.matrixU().transpose());
 }
 
-void TorqueSafetyGuardFn(std::array<double, 7> &tau_d_array,
-                         const std::array<double, 7> &max_torques) {
+void TorqueSafetyGuardFn(std::array<double, 7>& tau_d_array,
+                         const std::array<double, 7>& max_torques) {
   for (size_t i = 0; i < tau_d_array.size(); i++) {
     tau_d_array[i] =
         std::max(std::min(tau_d_array[i], max_torques[i]), -max_torques[i]);
@@ -348,7 +348,7 @@ void Franka::osc() {
   avoidance_weights_ << 1., 1., 1., 1., 1., 10., 10.;
 
   try {
-    this->robot.control([&](const franka::RobotState &robot_state,
+    this->robot.control([&](const franka::RobotState& robot_state,
                             franka::Duration period) -> franka::Torques {
       std::chrono::high_resolution_clock::time_point t1 =
           std::chrono::high_resolution_clock::now();
@@ -534,7 +534,7 @@ void Franka::osc() {
 
       return tau_d_rate_limited;
     });
-  } catch (const franka::Exception &e) {
+  } catch (const franka::Exception& e) {
     std::cerr << "Franka::osc() caught exception: " << e.what() << std::endl;
     std::cerr << "Attempting to recover from exception..." << std::endl;
     this->automatic_error_recovery();
@@ -570,7 +570,7 @@ void Franka::joint_controller() {
   joint_min_ << -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973;
 
   try {
-    this->robot.control([&](const franka::RobotState &robot_state,
+    this->robot.control([&](const franka::RobotState& robot_state,
                             franka::Duration period) -> franka::Torques {
       std::chrono::high_resolution_clock::time_point t1 =
           std::chrono::high_resolution_clock::now();
@@ -653,7 +653,7 @@ void Franka::joint_controller() {
 
       return tau_d_rate_limited;
     });
-  } catch (const franka::Exception &e) {
+  } catch (const franka::Exception& e) {
     std::cerr << "Franka::joint_controller() caught exception: " << e.what()
               << std::endl;
     std::cerr << "Attempting to recover from exception..." << std::endl;
@@ -682,7 +682,7 @@ void Franka::zero_torque_controller() {
 
   this->controller_time = 0.0;
   try {
-    this->robot.control([&](const franka::RobotState &robot_state,
+    this->robot.control([&](const franka::RobotState& robot_state,
                             franka::Duration period) -> franka::Torques {
       this->interpolator_mutex.lock();
       this->curr_state = robot_state;
@@ -694,7 +694,7 @@ void Franka::zero_torque_controller() {
       }
       return franka::Torques({0, 0, 0, 0, 0, 0, 0});
     });
-  } catch (const franka::Exception &e) {
+  } catch (const franka::Exception& e) {
     std::cerr << "Franka::zero_torque_controller() caught exception: "
               << e.what() << std::endl;
     std::cerr << "Attempting to recover from exception..." << std::endl;
diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp
index af1be351..6df949c7 100644
--- a/extensions/rcs_fr3/src/pybind/rcs.cpp
+++ b/extensions/rcs_fr3/src/pybind/rcs.cpp
@@ -99,12 +99,12 @@ PYBIND11_MODULE(_core, m) {
       .def_readonly("O_ddP_EE_c", &franka::RobotState::O_ddP_EE_c)
       .def_readonly("theta", &franka::RobotState::theta)
       .def_readonly("dtheta", &franka::RobotState::dtheta)
-    //   .def_readonly("current_errors", &franka::RobotState::current_errors)
-    //   .def_readonly("last_motion_errors",
-    //                 &franka::RobotState::last_motion_errors)
+      //   .def_readonly("current_errors", &franka::RobotState::current_errors)
+      //   .def_readonly("last_motion_errors",
+      //                 &franka::RobotState::last_motion_errors)
       .def_readonly("control_command_success_rate",
                     &franka::RobotState::control_command_success_rate)
-    //   .def_readonly("time", &franka::RobotState::time)
+      //   .def_readonly("time", &franka::RobotState::time)
       .def_readonly("robot_mode", &franka::RobotState::robot_mode);
 
   py::object robot_state =