robot.h

Go to the documentation of this file.

// Copyright (c) 2023 Franka Robotics GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#pragma once
 
#include <functional>
#include <memory>
#include <mutex>
#include <string>
 
#include <franka/control_types.h>
#include <franka/duration.h>
#include <franka/lowpass_filter.h>
#include <franka/robot_model_base.h>
#include <franka/robot_state.h>
#include <research_interface/robot/service_types.h>
#include <franka/commands/get_robot_model_command.hpp>
 
namespace franka {
 
class Model;
 
class ActiveControlBase;
 
class Robot {
 public:
  static constexpr size_t kNumJoints = 7;
 
  using ServerVersion = uint16_t;
 
  explicit Robot(const std::string& franka_address,
                 RealtimeConfig realtime_config = RealtimeConfig::kEnforce,
                 size_t log_size = 50);
 
  Robot(Robot&& other) noexcept;
 
  Robot& operator=(Robot&& other) noexcept;
 
  virtual ~Robot() noexcept;
 
  void control(std::function<Torques(const RobotState&, franka::Duration)> control_callback,
               bool limit_rate = false,
               double cutoff_frequency = kDefaultCutoffFrequency);
 
  void control(
      std::function<Torques(const RobotState&, franka::Duration)> control_callback,
      std::function<JointPositions(const RobotState&, franka::Duration)> motion_generator_callback,
      bool limit_rate = false,
      double cutoff_frequency = kDefaultCutoffFrequency);
 
  void control(
      std::function<Torques(const RobotState&, franka::Duration)> control_callback,
      std::function<JointVelocities(const RobotState&, franka::Duration)> motion_generator_callback,
      bool limit_rate = false,
      double cutoff_frequency = kDefaultCutoffFrequency);
 
  void control(
      std::function<Torques(const RobotState&, franka::Duration)> control_callback,
      std::function<CartesianPose(const RobotState&, franka::Duration)> motion_generator_callback,
      bool limit_rate = false,
      double cutoff_frequency = kDefaultCutoffFrequency);
 
  void control(std::function<Torques(const RobotState&, franka::Duration)> control_callback,
               std::function<CartesianVelocities(const RobotState&, franka::Duration)>
                   motion_generator_callback,
               bool limit_rate = false,
               double cutoff_frequency = kDefaultCutoffFrequency);
 
  void control(
      std::function<JointPositions(const RobotState&, franka::Duration)> motion_generator_callback,
      ControllerMode controller_mode = ControllerMode::kJointImpedance,
      bool limit_rate = false,
      double cutoff_frequency = kDefaultCutoffFrequency);
 
  void control(
      std::function<JointVelocities(const RobotState&, franka::Duration)> motion_generator_callback,
      ControllerMode controller_mode = ControllerMode::kJointImpedance,
      bool limit_rate = false,
      double cutoff_frequency = kDefaultCutoffFrequency);
 
  void control(
      std::function<CartesianPose(const RobotState&, franka::Duration)> motion_generator_callback,
      ControllerMode controller_mode = ControllerMode::kJointImpedance,
      bool limit_rate = false,
      double cutoff_frequency = kDefaultCutoffFrequency);
 
  void control(std::function<CartesianVelocities(const RobotState&, franka::Duration)>
                   motion_generator_callback,
               ControllerMode controller_mode = ControllerMode::kJointImpedance,
               bool limit_rate = false,
               double cutoff_frequency = kDefaultCutoffFrequency);
 
  void read(std::function<bool(const RobotState&)> read_callback);
 
  virtual RobotState readOnce();
 
  auto getRobotModel() -> std::string;
 
  auto getUpperJointVelocityLimits(const std::array<double, kNumJoints>& joint_positions)
      -> std::array<double, kNumJoints>;
 
  auto getLowerJointVelocityLimits(const std::array<double, kNumJoints>& joint_positions)
      -> std::array<double, kNumJoints>;
 
  void setCollisionBehavior(const std::array<double, 7>& lower_torque_thresholds_acceleration,
                            const std::array<double, 7>& upper_torque_thresholds_acceleration,
                            const std::array<double, 7>& lower_torque_thresholds_nominal,
                            const std::array<double, 7>& upper_torque_thresholds_nominal,
                            const std::array<double, 6>& lower_force_thresholds_acceleration,
                            const std::array<double, 6>& upper_force_thresholds_acceleration,
                            const std::array<double, 6>& lower_force_thresholds_nominal,
                            const std::array<double, 6>& upper_force_thresholds_nominal);
 
  void setCollisionBehavior(const std::array<double, 7>& lower_torque_thresholds,
                            const std::array<double, 7>& upper_torque_thresholds,
                            const std::array<double, 6>& lower_force_thresholds,
                            const std::array<double, 6>& upper_force_thresholds);
 
  void setJointImpedance(
      const std::array<double, 7>& K_theta);  // NOLINT(readability-identifier-naming)
 
  void setCartesianImpedance(
      const std::array<double, 6>& K_x);  // NOLINT(readability-identifier-naming)
 
  void setGuidingMode(const std::array<bool, 6>& guiding_mode, bool elbow);
 
  void setK(const std::array<double, 16>& EE_T_K);  // NOLINT(readability-identifier-naming)
 
  void setEE(const std::array<double, 16>& NE_T_EE);  // NOLINT(readability-identifier-naming)
 
  void setLoad(double load_mass,
               const std::array<double, 3>& F_x_Cload,  // NOLINT(readability-identifier-naming)
               const std::array<double, 9>& load_inertia);
 
  void automaticErrorRecovery();
 
  virtual std::unique_ptr<ActiveControlBase> startTorqueControl();
 
  virtual std::unique_ptr<ActiveControlBase> startJointPositionControl(
      const research_interface::robot::Move::ControllerMode& control_type);
 
  virtual std::unique_ptr<ActiveControlBase> startAsyncJointPositionControl(
      const research_interface::robot::Move::ControllerMode& control_type,
      const std::optional<std::vector<double>>& maximum_velocities);
 
  virtual std::unique_ptr<ActiveControlBase> startJointVelocityControl(
      const research_interface::robot::Move::ControllerMode& control_type);
 
  virtual std::unique_ptr<ActiveControlBase> startCartesianPoseControl(
      const research_interface::robot::Move::ControllerMode& control_type);
 
  virtual std::unique_ptr<ActiveControlBase> startCartesianVelocityControl(
      const research_interface::robot::Move::ControllerMode& control_type);
 
  void stop();
 
  Model loadModel();
 
  // Loads the model library for the unittests mockRobotModel
  Model loadModel(std::unique_ptr<RobotModelBase> robot_model);
 
  ServerVersion serverVersion() const noexcept;
 
  Robot(const Robot&) = delete;
  Robot& operator=(const Robot&) = delete;
 
  class Impl;
 
 protected:
  Robot(std::shared_ptr<Impl> robot_impl);
 
  Robot() = default;
 
 private:
  template <typename MotionGeneratorType>
  std::unique_ptr<ActiveControlBase> startControl(
      const research_interface::robot::Move::ControllerMode& controller_type);
 
  std::shared_ptr<Impl> impl_;
  std::mutex control_mutex_;
 
  template <typename MotionGeneratorType>
  auto startAsyncControl(const research_interface::robot::Move::ControllerMode& controller_type,
                         const std::optional<std::vector<double>>& maximum_velocities)
      -> std::unique_ptr<ActiveControlBase>;
};
 
}  // namespace franka