robot.hpp

Go to the documentation of this file.

#pragma once
 
#include <franka/control_types.h>
#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/robot.h>
#include <franka/robot_state.h>
 
#include <exception>
#include <future>
#include <optional>
#include <stdexcept>
#include <variant>
 
#include "franky/cartesian_state.hpp"
#include "franky/control_signal_type.hpp"
#include "franky/dynamics_limit.hpp"
#include "franky/joint_state.hpp"
#include "franky/model.hpp"
#include "franky/motion/motion.hpp"
#include "franky/motion/motion_generator.hpp"
#include "franky/relative_dynamics_factor.hpp"
#include "franky/robot_pose.hpp"
#include "franky/robot_state_estimator.hpp"
#include "franky/robot_velocity.hpp"
#include "franky/types.hpp"
#include "franky/util.hpp"
#include "franky/wait_free_triple_buffer.hpp"
 
namespace franky {
 
struct InvalidMotionTypeException : std::runtime_error {
  using std::runtime_error::runtime_error;
};
 
struct MotionReuseException : std::runtime_error {
  using std::runtime_error::runtime_error;
};
 
class Robot : public franka::Robot {
 public:
  struct Params {
    RelativeDynamicsFactor relative_dynamics_factor{1.0};
 
    double default_torque_threshold{20.0};
 
    double default_force_threshold{30.0};
 
    franka::ControllerMode controller_mode{franka::ControllerMode::kJointImpedance};
 
    franka::RealtimeConfig realtime_config{franka::RealtimeConfig::kEnforce};
 
    double kalman_q_process_var = 0.0001;
 
    double kalman_dq_process_var = 0.001;
 
    double kalman_ddq_process_var = 0.1;
 
    double kalman_control_process_var = 1;
 
    double kalman_q_obs_var = 0.01;
 
    double kalman_dq_obs_var = 0.1;
 
    double kalman_q_d_obs_var = 0.0001;
 
    double kalman_dq_d_obs_var = 0.0001;
 
    double kalman_ddq_d_obs_var = 0.0001;
 
    double kalman_control_adaptation_rate = 0.1;
  };
 
  static constexpr size_t degrees_of_freedoms{7};
 
  static constexpr double control_rate{0.001};
 
  explicit Robot(const std::string &fci_hostname);
 
  explicit Robot(const std::string &fci_hostname, const Params &params);
 
  using franka::Robot::setCollisionBehavior;
 
  void setCollisionBehavior(const ScalarOrArray<7> &torque_threshold, const ScalarOrArray<6> &force_threshold);
 
  void setCollisionBehavior(
      const ScalarOrArray<7> &lower_torque_threshold, const ScalarOrArray<7> &upper_torque_threshold,
      const ScalarOrArray<6> &lower_force_threshold, const ScalarOrArray<6> &upper_force_threshold);
 
  void setCollisionBehavior(
      const ScalarOrArray<7> &lower_torque_threshold_acceleration,
      const ScalarOrArray<7> &upper_torque_threshold_acceleration,
      const ScalarOrArray<7> &lower_torque_threshold_nominal, const ScalarOrArray<7> &upper_torque_threshold_nominal,
      const ScalarOrArray<6> &lower_force_threshold_acceleration,
      const ScalarOrArray<6> &upper_force_threshold_acceleration, const ScalarOrArray<6> &lower_force_threshold_nominal,
      const ScalarOrArray<6> &upper_force_threshold_nominal);
 
  bool recoverFromErrors();
 
  [[nodiscard]] bool hasErrors();
 
  [[nodiscard]] RobotPose currentPose() { return currentCartesianState().pose(); }
 
  [[nodiscard]] RobotVelocity currentCartesianVelocity() { return currentCartesianState().velocity(); }
 
  [[nodiscard]] CartesianState currentCartesianState() {
    auto s = state();
    return {
        {Affine(Eigen::Matrix4d::Map(s.O_T_EE.data())), ElbowState{s.elbow}}, RobotVelocity(s.O_dP_EE_c, s.delbow_c)};
  }
 
  [[nodiscard]] JointState currentJointState() {
    auto s = state();
    return {s.q, s.dq};
  }
 
  [[nodiscard]] Vector7d currentJointPositions() { return currentJointState().position(); }
 
  [[nodiscard]] Vector7d currentJointVelocities() { return currentJointState().velocity(); }
 
  [[nodiscard]] RobotState state();
 
  [[nodiscard]] RelativeDynamicsFactor relative_dynamics_factor();
 
  [[nodiscard]] RelativeDynamicsFactor relative_dynamics_factor_rt();
 
  void setRelativeDynamicsFactor(const RelativeDynamicsFactor &relative_dynamics_factor);
 
  [[nodiscard]] bool is_in_control();
 
  [[nodiscard]] std::string fci_hostname() const;
 
  [[nodiscard]] std::optional<ControlSignalType> current_control_signal_type();
 
  [[nodiscard]] std::shared_ptr<const Model> model() const { return model_; }
 
#ifdef FRANKA_0_15
  [[nodiscard]] std::string model_urdf() const { return model_urdf_; }
#endif
 
  bool joinMotion() {
    std::unique_lock lock(*control_mutex_);
    return joinMotionUnsafe(lock);
  }
 
  template <class Rep, class Period>
  bool joinMotion(const std::chrono::duration<Rep, Period> &timeout) {
    std::unique_lock lock(*control_mutex_);
    return joinMotionUnsafe<Rep, Period>(lock, timeout);
  }
 
  [[nodiscard]]
  bool pollMotion() {
    return joinMotion(std::chrono::milliseconds(0));
  }
 
  // These helper functions are needed as the implicit template deduction does
  // not work on subclasses of Motion
 
  void move(
      const std::shared_ptr<Motion<franka::CartesianPose>> &motion, bool async = false, bool limit_rate = false,
      double cutoff_frequency = franka::kDefaultCutoffFrequency) {
    moveInternal<franka::CartesianPose>(
        motion,
        [this, limit_rate, cutoff_frequency](const ControlFunc<franka::CartesianPose> &m) {
          control(m, params_.controller_mode, limit_rate, cutoff_frequency);
        },
        async);
  }
 
  void move(
      const std::shared_ptr<Motion<franka::CartesianVelocities>> &motion, bool async = false, bool limit_rate = false,
      double cutoff_frequency = franka::kDefaultCutoffFrequency) {
    moveInternal<franka::CartesianVelocities>(
        motion,
        [this, limit_rate, cutoff_frequency](const ControlFunc<franka::CartesianVelocities> &m) {
          control(m, params_.controller_mode, limit_rate, cutoff_frequency);
        },
        async);
  }
 
  void move(
      const std::shared_ptr<Motion<franka::JointPositions>> &motion, bool async = false, bool limit_rate = false,
      double cutoff_frequency = franka::kDefaultCutoffFrequency) {
    moveInternal<franka::JointPositions>(
        motion,
        [this, limit_rate, cutoff_frequency](const ControlFunc<franka::JointPositions> &m) {
          control(m, params_.controller_mode, limit_rate, cutoff_frequency);
        },
        async);
  }
 
  void move(
      const std::shared_ptr<Motion<franka::JointVelocities>> &motion, bool async = false, bool limit_rate = false,
      double cutoff_frequency = franka::kDefaultCutoffFrequency) {
    moveInternal<franka::JointVelocities>(
        motion,
        [this, limit_rate, cutoff_frequency](const ControlFunc<franka::JointVelocities> &m) {
          control(m, params_.controller_mode, limit_rate, cutoff_frequency);
        },
        async);
  }
 
  void move(
      const std::shared_ptr<Motion<franka::Torques>> &motion, bool async = false, bool limit_rate = false,
      double cutoff_frequency = franka::kDefaultCutoffFrequency) {
    moveInternal<franka::Torques>(
        motion,
        [this, limit_rate, cutoff_frequency](const ControlFunc<franka::Torques> &m) {
          control(m, limit_rate, cutoff_frequency);
        },
        async);
  }
 
 private:
  std::shared_ptr<const Model> model_;
#ifdef FRANKA_0_15
  std::string model_urdf_;
#endif
 
  template <typename ControlSignalType>
  using ControlFunc = std::function<ControlSignalType(const franka::RobotState &, franka::Duration)>;
  using MotionGeneratorVariant = std::variant<
      std::nullopt_t, MotionGenerator<franka::Torques>, MotionGenerator<franka::JointVelocities>,
      MotionGenerator<franka::JointPositions>, MotionGenerator<franka::CartesianVelocities>,
      MotionGenerator<franka::CartesianPose>>;
 
  std::string fci_hostname_;
  Params params_;
  // Written by the real-time thread while in control and by user threads (under
  // control_mutex_) otherwise; control_mutex_ synchronizes the writer handover.
  WaitFreeTripleBuffer<RobotState> state_buffer_;
  // Serializes user-side readers of state_buffer_. Never taken by the real-time thread.
  std::mutex state_mutex_;
  std::shared_ptr<std::mutex> control_mutex_;
  std::condition_variable control_finished_condition_;
  std::exception_ptr control_exception_;
  std::thread control_thread_;
  MotionGeneratorVariant motion_generator_{std::nullopt};
  bool motion_generator_running_{false};
  WaitFreeTripleBuffer<RelativeDynamicsFactor> relative_dynamics_factor_handle_;
 
  [[nodiscard]] bool is_in_control_unsafe() const;
 
 public:
  // IMPORTANT: this has to come after control_mutex_ as otherwise
  // control_mutex_ will be uninitialized when passed to the constructor of the
  // DynamicsLimit class Limits provided by Franka for the FR3:
  // https://frankaemika.github.io/docs/control_parameters.html
  // clang-format off
  DynamicsLimit<double> translation_velocity_limit;
 
  DynamicsLimit<double> rotation_velocity_limit;
 
  DynamicsLimit<double> elbow_velocity_limit;
 
  DynamicsLimit<double> translation_acceleration_limit;
 
  DynamicsLimit<double> rotation_acceleration_limit;
 
  DynamicsLimit<double> elbow_acceleration_limit;
 
  DynamicsLimit<double> translation_jerk_limit;
 
  DynamicsLimit<double> rotation_jerk_limit;
 
  DynamicsLimit<double> elbow_jerk_limit;
 
  DynamicsLimit<Vector7d> joint_velocity_limit;
 
  DynamicsLimit<Vector7d> joint_acceleration_limit;
 
  DynamicsLimit<Vector7d> joint_jerk_limit;
 
  // clang-format on
 
 private:
  template <class Rep = long, class Period = std::ratio<1>>
  bool joinMotionUnsafe(
      std::unique_lock<std::mutex> &lock,
      const std::optional<std::chrono::duration<Rep, Period>> &timeout = std::nullopt) {
    while (motion_generator_running_) {
      if (timeout.has_value()) {
        if (control_finished_condition_.wait_for(lock, timeout.value()) == std::cv_status::timeout) {
          return false;
        }
      } else {
        control_finished_condition_.wait(lock);
      }
    }
    if (control_thread_.joinable()) control_thread_.join();
    if (control_exception_ != nullptr) {
      auto control_exception = control_exception_;
      control_exception_ = nullptr;
      std::rethrow_exception(control_exception);
    }
    return true;
  }
 
  template <typename ControlSignalType>
  void moveInternal(
      const std::shared_ptr<Motion<ControlSignalType>> &motion,
      const std::function<void(const ControlFunc<ControlSignalType> &)> &control_func_executor, bool async) {
    if (motion == nullptr) {
      throw std::invalid_argument("The motion must not be null.");
    }
    // Fail-fast check in the user thread; the authoritative lock-free check is in Motion::init.
    if (motion->has_started()) {
      throw MotionReuseException(
          "This motion object has already been started before. Motions cannot be reused; create a new motion "
          "instance instead.");
    }
    {  // Do not remove brace, it is needed to scope the lock
      std::unique_lock lock(*control_mutex_);
      if (is_in_control_unsafe() && motion_generator_running_) {
        if (!std::holds_alternative<MotionGenerator<ControlSignalType>>(motion_generator_)) {
          throw InvalidMotionTypeException(
              "The type of motion cannot change during runtime. Please ensure "
              "that the "
              "previous motion finished before using a new type of motion.");
        }
        std::get<MotionGenerator<ControlSignalType>>(motion_generator_).updateMotion(motion);
      } else {
        joinMotionUnsafe(lock);
 
        motion_generator_.emplace<MotionGenerator<ControlSignalType>>(this, motion);
        auto motion_generator = &std::get<MotionGenerator<ControlSignalType>>(motion_generator_);
        motion_generator->registerUpdateCallback(
            [this](const RobotState &robot_state, franka::Duration duration, franka::Duration time) {
              state_buffer_.set(robot_state);
            });
        motion_generator_running_ = true;
        control_thread_ = std::thread([this, control_func_executor, motion_generator]() {
          try {
            bool done = false;
            RobotStateEstimator robot_state_estimator(
                params_.kalman_q_process_var,
                params_.kalman_dq_process_var,
                params_.kalman_ddq_process_var,
                params_.kalman_control_process_var,
                params_.kalman_q_obs_var,
                params_.kalman_dq_obs_var,
                params_.kalman_q_d_obs_var,
                params_.kalman_dq_d_obs_var,
                params_.kalman_ddq_d_obs_var,
                params_.kalman_control_adaptation_rate);
            while (!done) {
              control_func_executor(
                  [this, motion_generator, &robot_state_estimator](const franka::RobotState &rs, franka::Duration d) {
                    return (*motion_generator)(robot_state_estimator.update(rs, *model_), d);
                  });
              std::unique_lock lock(*control_mutex_);
 
              // This code is just for the case that a new motion is set just
              // after the old one terminates. If this happens, we need to
              // continue with this motion, unless an exception occurs.
              done = !motion_generator->has_new_motion();
              if (motion_generator->has_new_motion()) {
                motion_generator->resetTimeUnsafe();
              } else {
                done = true;
                motion_generator_running_ = false;
                control_finished_condition_.notify_all();
              }
            }
          } catch (...) {
            std::unique_lock lock(*control_mutex_);
            control_exception_ = std::current_exception();
            motion_generator_running_ = false;
            control_finished_condition_.notify_all();
          }
        });
      }
    }
    if (!async) joinMotion();
  }
};
 
}  // namespace franky