Merge pull request #303 from RobotControlStack/juelg/fix-franka-controllers

fix(franka): controllers

Author: juelg

examples/teleop/franka.py

@@ -43,6 +43,7 @@
     "zed": "19928076",
 }
 MQ3_ADDR = "10.42.0.1"
+INCLUDE_DEPTH = False
 
 # DIGIT_DICT = {
 #     "digit_right_left": "D21182",
@@ -69,7 +70,7 @@
     mq3_addr=MQ3_ADDR,
     simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION,
     switched_left_right=False,
-    display_cameras=True,
+    display_cameras=False,
 )
 # config = GelloConfig(
 #     arms={
@@ -82,10 +83,10 @@
 
 def get_env():
     if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
-        from rcs_fr3.configs import DefaultFR3MultiHardwareEnv
+        from rcs_fr3.configs import FrankaDuoEnv
         from rcs_fr3.creators import HardwareCameraCreatorConfig
 
-        env_creator = DefaultFR3MultiHardwareEnv()
+        env_creator = FrankaDuoEnv()
         env_creator.left_ip = ROBOT2IP["left"]
         env_creator.right_ip = ROBOT2IP["right"]
         hw_cfg = env_creator.config()
@@ -135,15 +136,13 @@ def get_env():
             )
         hw_cfg.camera_cfgs = camera_cfgs or None
         hw_cfg.control_mode = config.operator_class.control_mode[0]
+        hw_cfg.wrapper_cfg.include_depth = INCLUDE_DEPTH
         hw_cfg.max_relative_movement = (
             0.5 if config.operator_class.control_mode[0] == ControlMode.JOINTS else (0.5, np.deg2rad(90))
         )
         hw_cfg.relative_to = config.operator_class.control_mode[1]
         hw_cfg.robot_to_shared_base_frame = robot2world
         env_rel = env_creator.create_env(hw_cfg)
-        # env_rel = StorageWrapper(
-        #     env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000
-        # )
         operator = GelloOperator(config) if isinstance(config, GelloConfig) else QuestOperator(config)
     else:
         # FR3
@@ -154,19 +153,21 @@ def get_env():
             async_control=True, realtime=True, frequency=RECORD_FPS, max_convergence_steps=500
         )
         sim_cfg_data.relative_to = RelativeTo.CONFIGURED_ORIGIN
+        sim_cfg_data.wrapper_cfg.include_depth = INCLUDE_DEPTH
         if sim_cfg_data.root_frame_objects is None:
             sim_cfg_data.root_frame_objects = {}
         # cfg.root_frame_objects["green_cube"] = (rcs.OBJECT_PATHS["green_cube"], Pose(translation=[0.5, 0, 0.5], quaternion=[0, 0, 0, 1]))
         sim_cfg_data.task_cfg = PickTaskConfig(robot_name="right")
 
         env_rel = scene.create_env(sim_cfg_data)
-        env_rel = StorageWrapper(
-            env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000
-        )
 
         sim = env_rel.get_wrapper_attr("sim")
         MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3)))
         operator = GelloOperator(config, sim) if isinstance(config, GelloConfig) else QuestOperator(config, sim)
+
+    env_rel = StorageWrapper(
+        env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000
+    )
     return env_rel, operator
 
 

extensions/rcs_fr3/src/hw/Franka.cpp

@@ -73,7 +73,7 @@ FrankaConfig* Franka::get_config() {
 
 FrankaState* Franka::get_state() {
   franka::RobotState current_robot_state;
-  if (this->running_controller == Controller::none) {
+  if (this->running_controller.load() == Controller::none) {
     current_robot_state = this->robot.readOnce();
   } else {
     std::lock_guard<std::mutex> lock(this->interpolator_mutex);
@@ -100,7 +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) {
+  if (this->running_controller.load() == Controller::none) {
     this->curr_state = this->robot.readOnce();
     x = common::Pose(this->curr_state.O_T_EE);
   } else {
@@ -127,7 +127,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) {
+  if (this->running_controller.load() == Controller::none) {
     this->curr_state = this->robot.readOnce();
     joints = common::Vector7d(this->curr_state.q.data());
   } else {
@@ -182,15 +182,15 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) {
   int policy_rate = 20;
   int traj_rate = 500;
 
-  if (this->running_controller == Controller::none) {
+  if (this->running_controller.load() == Controller::none) {
     this->controller_time = 0.0;
     this->get_joint_position();
     this->joint_interpolator = common::LinearJointPositionTrajInterpolator();
-  } else if (this->running_controller != Controller::jsc) {
+  } else if (this->running_controller.load() != Controller::jsc) {
     // runtime error
     throw std::runtime_error(
         "Controller type must but joint space but is " +
-        std::to_string(this->running_controller) +
+        std::to_string(static_cast<int>(this->running_controller.load())) +
         ". To change controller type stop the current controller first.");
   } else {
     this->interpolator_mutex.lock();
@@ -202,8 +202,8 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) {
       policy_rate, traj_rate, traj_interpolation_time_fraction);
 
   // if not thread is running, then start
-  if (this->running_controller == Controller::none) {
-    this->running_controller = Controller::jsc;
+  if (this->running_controller.load() == Controller::none) {
+    this->running_controller.store(Controller::jsc);
     this->control_thread = std::thread(&Franka::joint_controller, this);
   } else {
     this->interpolator_mutex.unlock();
@@ -229,28 +229,35 @@ void Franka::osc_set_cartesian_position(
   int policy_rate = 20;
   int traj_rate = 500;
 
-  if (this->running_controller == Controller::none) {
+  if (this->running_controller.load() == Controller::none) {
     this->controller_time = 0.0;
+    {
+      std::lock_guard<std::mutex> lock(this->interpolator_mutex);
+      this->curr_state = this->robot.readOnce();
+    }
     this->traj_interpolator = common::LinearPoseTrajInterpolator();
-  } else if (this->running_controller != Controller::osc) {
+  } else if (this->running_controller.load() != Controller::osc) {
     throw std::runtime_error(
         "Controller type must but osc but is " +
-        std::to_string(this->running_controller) +
+        std::to_string(static_cast<int>(this->running_controller.load())) +
         ". To change controller type stop the current controller first.");
   } else {
     this->interpolator_mutex.lock();
   }
 
   common::Pose curr_pose(this->curr_state.O_T_EE);
+  if (!this->m_cfg.tcp_offset_configured_in_desk) {
+    curr_pose = curr_pose * this->m_cfg.tcp_offset;
+  }
   this->traj_interpolator.reset(
       this->controller_time, curr_pose.translation(), curr_pose.quaternion(),
       desired_pose_EE_in_base_frame.translation(),
       desired_pose_EE_in_base_frame.quaternion(), policy_rate, traj_rate,
       traj_interpolation_time_fraction);
 
   // if not thread is running, then start
-  if (this->running_controller == Controller::none) {
-    this->running_controller = Controller::osc;
+  if (this->running_controller.load() == Controller::none) {
+    this->running_controller.store(Controller::osc);
     this->control_thread = std::thread(&Franka::osc, this);
   } else {
     this->interpolator_mutex.unlock();
@@ -259,10 +266,11 @@ void Franka::osc_set_cartesian_position(
 
 // method to stop thread
 void Franka::stop_control_thread() {
-  if (this->control_thread.has_value() &&
-      this->running_controller != Controller::none) {
-    this->running_controller = Controller::none;
-    this->control_thread->join();
+  if (this->control_thread.has_value()) {
+    this->running_controller.store(Controller::none);
+    if (this->control_thread->joinable()) {
+      this->control_thread->join();
+    }
     this->control_thread.reset();
   }
 }
@@ -291,7 +299,10 @@ void Franka::osc() {
   // translation: [150.0, 150.0, 150.0]
   // rotation: 250.0
 
-  Eigen::Matrix<double, 3, 3> Kp_p, Kp_r, Kd_p, Kd_r;
+  Eigen::Matrix<double, 3, 3> Kp_p = Eigen::Matrix3d::Zero();
+  Eigen::Matrix<double, 3, 3> Kp_r = Eigen::Matrix3d::Zero();
+  Eigen::Matrix<double, 3, 3> Kd_p = Eigen::Matrix3d::Zero();
+  Eigen::Matrix<double, 3, 3> Kd_r = Eigen::Matrix3d::Zero();
   Eigen::Matrix<double, 7, 1> static_q_task_;
   Eigen::Matrix<double, 7, 1> residual_mass_vec_;
   Eigen::Array<double, 7, 1> joint_max_;
@@ -323,7 +334,7 @@ void Franka::osc() {
           std::chrono::high_resolution_clock::now();
 
       // torques handler
-      if (this->running_controller == Controller::none) {
+      if (this->running_controller.load() == 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);
       }
@@ -335,7 +346,6 @@ void Franka::osc() {
       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;
@@ -376,9 +386,15 @@ void Franka::osc() {
       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()));
+      // Express OSC feedback in the same TCP frame exposed by the public
+      // Cartesian API.
+      common::Pose T_EE_in_base_frame_pose =
+          this->m_cfg.tcp_offset_configured_in_desk
+              ? common::Pose(robot_state.O_T_EE)
+              : common::Pose(robot_state.O_T_EE) * this->m_cfg.tcp_offset;
+      Eigen::Affine3d T_EE_in_base_frame =
+          T_EE_in_base_frame_pose.affine_matrix();
+
       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());
 
@@ -487,7 +503,7 @@ void Franka::osc() {
   }
 
   // Ensure we mark the controller as stopped so we can restart later
-  this->running_controller = Controller::none;
+  this->running_controller.store(Controller::none);
 }
 
 void Franka::joint_controller() {
@@ -524,14 +540,13 @@ void Franka::joint_controller() {
           std::chrono::high_resolution_clock::now();
 
       // torques handler
-      if (this->running_controller == Controller::none) {
+      if (this->running_controller.load() == 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);
 
       this->interpolator_mutex.lock();
       this->curr_state = robot_state;
@@ -588,16 +603,18 @@ void Franka::joint_controller() {
     std::lock_guard<std::mutex> lock(this->exception_mutex);
     this->background_exception = std::current_exception();
   }
+
+  this->running_controller.store(Controller::none);
 }
 
 void Franka::zero_torque_guiding() {
   this->check_for_background_errors();
-  if (this->running_controller != Controller::none) {
+  if (this->running_controller.load() != 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->running_controller.store(Controller::ztc);
   this->control_thread = std::thread(&Franka::zero_torque_controller, this);
 }
 
@@ -617,7 +634,7 @@ void Franka::zero_torque_controller() {
       this->curr_state = robot_state;
       this->controller_time += period.toSec();
       this->interpolator_mutex.unlock();
-      if (this->running_controller == Controller::none) {
+      if (this->running_controller.load() == Controller::none) {
         // stop
         return franka::MotionFinished(franka::Torques({0, 0, 0, 0, 0, 0, 0}));
       }
@@ -627,6 +644,8 @@ void Franka::zero_torque_controller() {
     std::lock_guard<std::mutex> lock(this->exception_mutex);
     this->background_exception = std::current_exception();
   }
+
+  this->running_controller.store(Controller::none);
 }
 
 void Franka::move_home() {
@@ -714,8 +733,12 @@ std::optional<std::shared_ptr<common::Kinematics>> Franka::get_ik() {
 
 void Franka::set_cartesian_position(const common::Pose& x) {
   // pose is assumed to be in the robots coordinate frame
+  common::Pose target_pose = x;
+  if (!this->m_cfg.tcp_offset_configured_in_desk) {
+    target_pose = target_pose * this->m_cfg.tcp_offset.inverse();
+  }
   if (this->m_cfg.async_control) {
-    this->osc_set_cartesian_position(x);
+    this->osc_set_cartesian_position(target_pose);
     return;
   }
   // TODO: this should handled with tcp offset config
@@ -733,10 +756,11 @@ void Franka::set_cartesian_position(const common::Pose& x) {
     // if gripper is attached the tcp offset will automatically be applied
     // by libfranka
     this->robot.setEE(nominal_end_effector_frame_value.affine_array());
-    this->set_cartesian_position_internal(x, 1.0, std::nullopt, std::nullopt);
+    this->set_cartesian_position_internal(target_pose, 1.0, std::nullopt,
+                                          std::nullopt);
 
   } else if (this->m_cfg.ik_solver == IKSolver::rcs_ik) {
-    this->set_cartesian_position_ik(x);
+    this->set_cartesian_position_ik(target_pose);
   }
 }
 
@@ -786,14 +810,17 @@ void Franka::set_cartesian_position_internal(const common::Pose& pose,
 
   this->robot.control(
       [&force_stop_condition, &initial_elbow, &elbow, &max_force, &time,
-       &max_time, &initial_pose, &should_stop,
+       &max_time, &initial_pose, &should_stop, this,
        &pose](const franka::RobotState& state,
               franka::Duration time_step) -> franka::CartesianPose {
         time += time_step.toSec();
         if (time == 0) {
           initial_elbow = state.elbow_c;
 
-          initial_pose = common::Pose(state.O_T_EE);
+          initial_pose =
+              this->m_cfg.tcp_offset_configured_in_desk
+                  ? common::Pose(state.O_T_EE)
+                  : common::Pose(state.O_T_EE) * this->m_cfg.tcp_offset;
         }
         auto new_elbow = initial_elbow;
         const double progress = time / max_time;

extensions/rcs_fr3/src/hw/Franka.h

@@ -4,6 +4,7 @@
 #include <franka/robot.h>
 #include <franka/robot_state.h>
 
+#include <atomic>
 #include <cmath>
 #include <memory>
 #include <mutex>
@@ -43,10 +44,6 @@ struct FrankaConfig : common::RobotConfig {
   bool async_control = false;
   bool tcp_offset_configured_in_desk = true;
   bool ignore_realtime = false;
-  std::optional<common::VectorXd> q_home =
-      (common::VectorXd(7) << 0.0, -M_PI_4, 0.0, -3.0 * M_PI_4, 0.0, M_PI_2,
-       M_PI_4)
-          .finished();
   size_t dof = 7;
   Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor> joint_limits =
       (Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor>(2, 7) <<
@@ -89,7 +86,7 @@ class Franka : public common::Robot {
   common::LinearJointPositionTrajInterpolator joint_interpolator;
   franka::RobotState curr_state;
   std::mutex interpolator_mutex;
-  Controller running_controller = Controller::none;
+  std::atomic<Controller> running_controller{Controller::none};
   std::exception_ptr background_exception = nullptr;
   std::mutex exception_mutex;
   void osc();

extensions/rcs_fr3/src/rcs_fr3/__main__.py

@@ -23,10 +23,11 @@ def home(
     ip: Annotated[str, typer.Argument(help="IP of the robot")],
     shut: Annotated[bool, typer.Option("-s", help="Should the robot be shut down")] = False,
     unlock: Annotated[bool, typer.Option("-u", help="unlocks the robot")] = False,
+    fh: Annotated[bool, typer.Option("-h", help="franka hand open")] = False,
 ):
     """Moves the FR3 to home position"""
     user, pw = load_creds_franka_desk()
-    rcs_fr3.desk.home(ip, user, pw, shut, unlock)
+    rcs_fr3.desk.home(ip, user, pw, shut, unlock, fh)
 
 
 @fr3_app.command()