fix(franka): error forward to python

- fixed error forward from cpp to python
- simplified fr3 desk helpers
- added timestamp to storage wrapper

Author: juelg

extensions/rcs_fr3/src/hw/Franka.cpp

@@ -100,6 +100,7 @@ void Franka::set_default_robot_behavior() {
 }
 
 common::Pose Franka::get_cartesian_position() {
+  this->check_for_background_errors();
   common::Pose x;
   if (this->running_controller == Controller::none) {
     this->curr_state = this->robot.readOnce();
@@ -123,6 +124,7 @@ void Franka::set_joint_position(const common::VectorXd& q) {
 }
 
 common::VectorXd Franka::get_joint_position() {
+  this->check_for_background_errors();
   common::Vector7d joints;
   if (this->running_controller == Controller::none) {
     this->curr_state = this->robot.readOnce();
@@ -210,6 +212,7 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) {
 void Franka::check_for_background_errors() {
   std::lock_guard<std::mutex> lock(this->exception_mutex);
   if (this->background_exception) {
+    this->stop_control_thread();
     std::exception_ptr ex = this->background_exception;
     this->background_exception = nullptr;
     std::rethrow_exception(ex);
@@ -587,6 +590,7 @@ void Franka::joint_controller() {
 }
 
 void Franka::zero_torque_guiding() {
+  this->check_for_background_errors();
   if (this->running_controller != Controller::none) {
     throw std::runtime_error(
         "A controller is currently running. Please stop it first.");
@@ -605,18 +609,23 @@ 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 (...) {
+    std::lock_guard<std::mutex> lock(this->exception_mutex);
+    this->background_exception = std::current_exception();
+  }
 }
 
 void Franka::move_home() {

extensions/rcs_fr3/src/rcs_fr3/desk.py

@@ -49,16 +49,10 @@ def load_creds_franka_desk(postfix: str = "") -> tuple[str, str]:
 
 def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False):
     with Desk.fci(ip, username, password, unlock=unlock):
-        robot_cfg = default_fr3_hw_robot_cfg()
-        robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
-        robot_cfg.speed_factor = 0.2
-        ik = rcs.common.Pin(
-            robot_cfg.kinematic_model_path,
-            robot_cfg.attachment_site,
-            urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
-        )
-        f = rcs_fr3.hw.Franka(ip, ik)
-        f.set_config(robot_cfg)
+        f = rcs_fr3.hw.Franka(ip)
+        config = rcs_fr3.hw.FR3Config()
+        config.speed_factor = 0.2
+        f.set_config(config)
         config_hand = rcs_fr3.hw.FHConfig()
         g = rcs_fr3.hw.FrankaHand(ip, config_hand)
         if shut:
@@ -70,15 +64,9 @@ def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False
 
 def info(ip: str, username: str, password: str, include_hand: bool = False):
     with Desk.fci(ip, username, password):
-        robot_cfg = default_fr3_hw_robot_cfg()
-        robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
+        robot_cfg = rcs_fr3.hw.FR3Config()
         robot_cfg.speed_factor = 0.2
-        ik = rcs.common.Pin(
-            robot_cfg.kinematic_model_path,
-            robot_cfg.attachment_site,
-            urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
-        )
-        f = rcs_fr3.hw.Franka(ip, ik)
+        f = rcs_fr3.hw.Franka(ip)
         f.set_config(robot_cfg)
         print("Robot info:")
         print("Current cartesian position:")

extensions/rcs_panda/src/rcs_panda/desk.py

@@ -49,15 +49,9 @@ def load_creds_franka_desk(postfix: str = "") -> tuple[str, str]:
 
 def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False):
     with Desk.fci(ip, username, password, unlock=unlock):
-        robot_cfg = default_panda_hw_robot_cfg()
-        robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
+        robot_cfg = rcs_panda.hw.PandaConfig()
         robot_cfg.speed_factor = 0.2
-        ik = rcs.common.Pin(
-            robot_cfg.kinematic_model_path,
-            robot_cfg.attachment_site,
-            urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
-        )
-        f = rcs_panda.hw.Franka(ip, ik)
+        f = rcs_panda.hw.Franka(ip)
         f.set_config(robot_cfg)
         config_hand = rcs_panda.hw.FHConfig()
         g = rcs_panda.hw.FrankaHand(ip, config_hand)
@@ -70,15 +64,9 @@ def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False
 
 def info(ip: str, username: str, password: str, include_hand: bool = False):
     with Desk.fci(ip, username, password):
-        robot_cfg = default_panda_hw_robot_cfg()
-        robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
+        robot_cfg = rcs_panda.hw.PandaConfig()
         robot_cfg.speed_factor = 0.2
-        ik = rcs.common.Pin(
-            robot_cfg.kinematic_model_path,
-            robot_cfg.attachment_site,
-            urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
-        )
-        f = rcs_panda.hw.Franka(ip, ik)
+        f = rcs_panda.hw.Franka(ip)
         f.set_config(robot_cfg)
         print("Robot info:")
         print("Current cartesian position:")

python/rcs/envs/storage_wrapper.py

@@ -263,6 +263,7 @@ def step(self, action):
                     "success": self._success,
                     "action": self._prev_action,
                     "instruction": self.instruction,
+                    "timestamp": datetime.datetime.now().timestamp(),
                 }
             )
             self._prev_action = action