refactor: rename fr3 class to franka

- rename fr3 c++ classes to franka as they are now shared between fr3 and panda
- added config subclasses for both robots
- added fixed pybind11-stubgen fork which should be removed as soon as the PR is accepted

Author: juelg

extensions/rcs_fr3/README.md

@@ -16,13 +16,13 @@ DESK_PASSWORD=...
 ```python
 import rcs_fr3
 from rcs_fr3._core import hw
-from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk
-user, pw = load_creds_fr3_desk()
+from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_franka_desk
+user, pw = load_creds_franka_desk()
 with FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False):
     urdf_path = rcs.scenes["fr3_empty_world"].urdf
     ik = rcs.common.RL(str(urdf_path))
-    robot = hw.FR3(ROBOT_IP, ik)
-    robot_cfg = FR3Config()
+    robot = hw.Franka(ROBOT_IP, ik)
+    robot_cfg = hw.FR3Config()
     robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
     robot_cfg.ik_solver = IKSolver.rcs_ik
     robot.set_config(robot_cfg)

extensions/rcs_fr3/src/hw/CMakeLists.txt

@@ -1,5 +1,5 @@
 add_library(hw)
-target_sources(hw PRIVATE FR3.cpp FrankaHand.cpp FR3MotionGenerator.cpp)
+target_sources(hw PRIVATE Franka.cpp FrankaHand.cpp FrankaMotionGenerator.cpp)
 target_link_libraries(
   hw
   PUBLIC franka rcs pinocchio::all Eigen3::Eigen

extensions/rcs_fr3/src/hw/FR3.cpp extensions/rcs_fr3/src/hw/Franka.cppextensions/rcs_fr3/src/hw/FR3.cpp renamed to extensions/rcs_fr3/src/hw/Franka.cpp

@@ -1,4 +1,4 @@
-#include "FR3.h"
+#include "Franka.h"
 
 #include <franka/duration.h>
 #include <franka/exception.h>
@@ -14,14 +14,14 @@
 #include <string>
 #include <thread>
 
-#include "FR3MotionGenerator.h"
+#include "FrankaMotionGenerator.h"
 #include "rcs/Pose.h"
 
 namespace rcs {
 namespace hw {
-FR3::FR3(const std::string &ip,
+Franka::Franka(const std::string &ip,
          std::optional<std::shared_ptr<common::Kinematics>> ik,
-         const std::optional<FR3Config> &cfg)
+         const std::optional<FrankaConfig> &cfg)
     : robot(ip), m_ik(ik) {
   // set collision behavior and impedance
   this->set_default_robot_behavior();
@@ -32,14 +32,14 @@ FR3::FR3(const std::string &ip,
   }  // else default constructor
 }
 
-FR3::~FR3() {}
+Franka::~Franka() {}
 
 /**
  * @brief Set the parameters for the robot
- * @param cfg The configuration for the robot, it should be a FR3Config type
+ * @param cfg The configuration for the robot, it should be a FrankaConfig type
  * otherwise the call will fail
  */
-bool FR3::set_config(const FR3Config &cfg) {
+bool Franka::set_config(const FrankaConfig &cfg) {
   this->cfg = cfg;
   this->cfg.speed_factor = std::min(std::max(cfg.speed_factor, 0.0), 1.0);
 
@@ -60,20 +60,20 @@ bool FR3::set_config(const FR3Config &cfg) {
   return true;
 }
 
-FR3Config *FR3::get_config() {
+FrankaConfig *Franka::get_config() {
   // copy config to heap
-  FR3Config *cfg = new FR3Config();
+  FrankaConfig *cfg = new FrankaConfig();
   *cfg = this->cfg;
   return cfg;
 }
 
-FR3State *FR3::get_state() {
+FrankaState *Franka::get_state() {
   // dummy state until we define a prober state
-  FR3State *state = new FR3State();
+  FrankaState *state = new FrankaState();
   return state;
 }
 
-void FR3::set_default_robot_behavior() {
+void Franka::set_default_robot_behavior() {
   this->robot.setCollisionBehavior({{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
                                    {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
                                    {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
@@ -86,7 +86,7 @@ void FR3::set_default_robot_behavior() {
   this->robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
 }
 
-common::Pose FR3::get_cartesian_position() {
+common::Pose Franka::get_cartesian_position() {
   common::Pose x;
   if (this->running_controller == Controller::none) {
     this->curr_state = this->robot.readOnce();
@@ -99,17 +99,17 @@ common::Pose FR3::get_cartesian_position() {
   return x;
 }
 
-void FR3::set_joint_position(const common::VectorXd &q) {
+void Franka::set_joint_position(const common::VectorXd &q) {
   if (this->cfg.async_control) {
     this->controller_set_joint_position(q);
     return;
   }
   // sync control
-  FR3MotionGenerator motion_generator(this->cfg.speed_factor, q);
+  FrankaMotionGenerator motion_generator(this->cfg.speed_factor, q);
   this->robot.control(motion_generator);
 }
 
-common::VectorXd FR3::get_joint_position() {
+common::VectorXd Franka::get_joint_position() {
   common::Vector7d joints;
   if (this->running_controller == Controller::none) {
     this->curr_state = this->robot.readOnce();
@@ -122,7 +122,7 @@ common::VectorXd FR3::get_joint_position() {
   return joints;
 }
 
-void FR3::set_guiding_mode(bool x, bool y, bool z, bool roll, bool pitch,
+void Franka::set_guiding_mode(bool x, bool y, bool z, bool roll, bool pitch,
                            bool yaw, bool elbow) {
   std::array<bool, 6> activated = {x, y, z, roll, pitch, yaw};
   this->robot.setGuidingMode(activated, elbow);
@@ -158,7 +158,7 @@ void TorqueSafetyGuardFn(std::array<double, 7> &tau_d_array, double min_torque,
   }
 }
 
-void FR3::controller_set_joint_position(const common::Vector7d &desired_q) {
+void Franka::controller_set_joint_position(const common::Vector7d &desired_q) {
   // from deoxys/config/osc-position-controller.yml
   double traj_interpolation_time_fraction = 1.0;  // in s
   // form deoxys/config/charmander.yml
@@ -186,13 +186,13 @@ void FR3::controller_set_joint_position(const common::Vector7d &desired_q) {
   // if not thread is running, then start
   if (this->running_controller == Controller::none) {
     this->running_controller = Controller::jsc;
-    this->control_thread = std::thread(&FR3::joint_controller, this);
+    this->control_thread = std::thread(&Franka::joint_controller, this);
   } else {
     this->interpolator_mutex.unlock();
   }
 }
 
-void FR3::osc_set_cartesian_position(
+void Franka::osc_set_cartesian_position(
     const common::Pose &desired_pose_EE_in_base_frame) {
   // from deoxys/config/osc-position-controller.yml
   double traj_interpolation_time_fraction = 1.0;
@@ -222,14 +222,14 @@ void FR3::osc_set_cartesian_position(
   // if not thread is running, then start
   if (this->running_controller == Controller::none) {
     this->running_controller = Controller::osc;
-    this->control_thread = std::thread(&FR3::osc, this);
+    this->control_thread = std::thread(&Franka::osc, this);
   } else {
     this->interpolator_mutex.unlock();
   }
 }
 
 // method to stop thread
-void FR3::stop_control_thread() {
+void Franka::stop_control_thread() {
   if (this->control_thread.has_value() &&
       this->running_controller != Controller::none) {
     this->running_controller = Controller::none;
@@ -238,7 +238,7 @@ void FR3::stop_control_thread() {
   }
 }
 
-void FR3::osc() {
+void Franka::osc() {
   franka::Model model = this->robot.loadModel();
 
   this->controller_time = 0.0;
@@ -451,7 +451,7 @@ void FR3::osc() {
   });
 }
 
-void FR3::joint_controller() {
+void Franka::joint_controller() {
   franka::Model model = this->robot.loadModel();
   this->controller_time = 0.0;
 
@@ -545,17 +545,17 @@ void FR3::joint_controller() {
   });
 }
 
-void FR3::zero_torque_guiding() {
+void Franka::zero_torque_guiding() {
   if (this->running_controller != Controller::none) {
     throw std::runtime_error(
         "A controller is currently running. Please stop it first.");
   }
   this->controller_time = 0.0;
   this->running_controller = Controller::ztc;
-  this->control_thread = std::thread(&FR3::zero_torque_controller, this);
+  this->control_thread = std::thread(&Franka::zero_torque_controller, this);
 }
 
-void FR3::zero_torque_controller() {
+void Franka::zero_torque_controller() {
   // high collision threshold values for high impedance
   robot.setCollisionBehavior(
       {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
@@ -578,26 +578,26 @@ void FR3::zero_torque_controller() {
   });
 }
 
-void FR3::move_home() {
+void Franka::move_home() {
   // sync
-  FR3MotionGenerator motion_generator(
+  FrankaMotionGenerator motion_generator(
       this->cfg.speed_factor,
-      common::robots_meta_config.at(common::RobotType::FR3).q_home);
+      common::robots_meta_config.at(this->cfg.robot_type).q_home);
   this->robot.control(motion_generator);
 }
 
-void FR3::automatic_error_recovery() { this->robot.automaticErrorRecovery(); }
+void Franka::automatic_error_recovery() { this->robot.automaticErrorRecovery(); }
 
-void FR3::reset() {
+void Franka::reset() {
   this->stop_control_thread();
   this->automatic_error_recovery();
 }
 
-void FR3::wait_milliseconds(int milliseconds) {
+void Franka::wait_milliseconds(int milliseconds) {
   std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds));
 }
 
-void FR3::double_tap_robot_to_continue() {
+void Franka::double_tap_robot_to_continue() {
   auto s = this->robot.readOnce();
   int touch_counter = false;
   bool can_be_touched_again = true;
@@ -651,11 +651,11 @@ double quintic_polynomial_speed_profile(double time, double start_time,
   // return (1 - std::cos(M_PI * progress)) / 2.0;
 }
 
-std::optional<std::shared_ptr<common::Kinematics>> FR3::get_ik() {
+std::optional<std::shared_ptr<common::Kinematics>> Franka::get_ik() {
   return this->m_ik;
 }
 
-void FR3::set_cartesian_position(const common::Pose &x) {
+void Franka::set_cartesian_position(const common::Pose &x) {
   // pose is assumed to be in the robots coordinate frame
   if (this->cfg.async_control) {
     this->osc_set_cartesian_position(x);
@@ -683,7 +683,7 @@ void FR3::set_cartesian_position(const common::Pose &x) {
   }
 }
 
-void FR3::set_cartesian_position_ik(const common::Pose &pose) {
+void Franka::set_cartesian_position_ik(const common::Pose &pose) {
   if (!this->m_ik.has_value()) {
     throw std::runtime_error(
         "No inverse kinematics was provided. Cannot use IK to set cartesian "
@@ -701,12 +701,12 @@ void FR3::set_cartesian_position_ik(const common::Pose &pose) {
   }
 }
 
-common::Pose FR3::get_base_pose_in_world_coordinates() {
+common::Pose Franka::get_base_pose_in_world_coordinates() {
   return this->cfg.world_to_robot.has_value() ? this->cfg.world_to_robot.value()
                                               : common::Pose();
 }
 
-void FR3::set_cartesian_position_internal(const common::Pose &pose,
+void Franka::set_cartesian_position_internal(const common::Pose &pose,
                                           double max_time,
                                           std::optional<double> elbow,
                                           std::optional<double> max_force) {

extensions/rcs_fr3/src/hw/FR3.h extensions/rcs_fr3/src/hw/Franka.hextensions/rcs_fr3/src/hw/FR3.h renamed to extensions/rcs_fr3/src/hw/Franka.h

@@ -1,5 +1,5 @@
-#ifndef RCS_FR3_H
-#define RCS_FR3_H
+#ifndef RCS_FRANKA_H
+#define RCS_FRANKA_H
 
 #include <franka/robot.h>
 
@@ -21,7 +21,7 @@ namespace hw {
 
 const double DEFAULT_SPEED_FACTOR = 0.2;
 
-struct FR3Load {
+struct FrankaLoad {
   double load_mass;
   std::optional<Eigen::Vector3d> f_x_cload;
   std::optional<Eigen::Matrix3d> load_inertia;
@@ -30,26 +30,32 @@ enum IKSolver { franka_ik = 0, rcs_ik };
 // modes: joint-space control, operational-space control, zero-torque
 // control
 enum Controller { none = 0, jsc, osc, ztc };
-struct FR3Config : common::RobotConfig {
+struct FrankaConfig : common::RobotConfig {
   // TODO: max force and elbow?
   // TODO: we can either write specific bindings for each, or we use python
   // dictionaries with these objects
   common::RobotType robot_type = common::RobotType::FR3;
   common::RobotPlatform robot_platform = common::RobotPlatform::HARDWARE;
-  IKSolver ik_solver = IKSolver::franka_ik;
+  IKSolver ik_solver = IKSolver::rcs_ik;
   double speed_factor = DEFAULT_SPEED_FACTOR;
-  std::optional<FR3Load> load_parameters = std::nullopt;
+  std::optional<FrankaLoad> load_parameters = std::nullopt;
   std::optional<common::Pose> nominal_end_effector_frame = std::nullopt;
   std::optional<common::Pose> world_to_robot = std::nullopt;
   bool async_control = false;
 };
 
-struct FR3State : common::RobotState {};
+struct FR3Config : FrankaConfig {};
+struct PandaConfig : FrankaConfig {
+  common::RobotType robot_type = common::RobotType::Panda;
+};
+
+
+struct FrankaState : common::RobotState {};
 
-class FR3 : public common::Robot {
+class Franka : public common::Robot {
  private:
   franka::Robot robot;
-  FR3Config cfg;
+  FrankaConfig cfg;
   std::optional<std::shared_ptr<common::Kinematics>> m_ik;
   std::optional<std::thread> control_thread = std::nullopt;
   common::LinearPoseTrajInterpolator traj_interpolator;
@@ -63,16 +69,16 @@ class FR3 : public common::Robot {
   void zero_torque_controller();
 
  public:
-  FR3(const std::string &ip,
+  Franka(const std::string &ip,
       std::optional<std::shared_ptr<common::Kinematics>> ik = std::nullopt,
-      const std::optional<FR3Config> &cfg = std::nullopt);
-  ~FR3() override;
+      const std::optional<FrankaConfig> &cfg = std::nullopt);
+  ~Franka() override;
 
-  bool set_config(const FR3Config &cfg);
+  bool set_config(const FrankaConfig &cfg);
 
-  FR3Config *get_config() override;
+  FrankaConfig *get_config() override;
 
-  FR3State *get_state() override;
+  FrankaState *get_state() override;
 
   void set_default_robot_behavior();
 
@@ -119,4 +125,4 @@ class FR3 : public common::Robot {
 }  // namespace hw
 }  // namespace rcs
 
-#endif  // RCS_FR3_H
+#endif  // RCS_FRANKA_H