libfranka  0.14.1
FCI C++ API
robot.h
Go to the documentation of this file.
1 // Copyright (c) 2023 Franka Robotics GmbH
2 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
3 #pragma once
4 
5 #include <functional>
6 #include <memory>
7 #include <mutex>
8 #include <string>
9 
10 #include <franka/control_types.h>
11 #include <franka/duration.h>
12 #include <franka/lowpass_filter.h>
13 #include <franka/robot_model_base.h>
14 #include <franka/robot_state.h>
15 #include <research_interface/robot/service_types.h>
16 #include <franka/commands/get_robot_model_command.hpp>
17 
22 namespace franka {
23 
24 class Model;
25 
26 class ActiveControlBase;
27 
68 class Robot {
69  public:
73  using ServerVersion = uint16_t;
74 
87  explicit Robot(const std::string& franka_address,
88  RealtimeConfig realtime_config = RealtimeConfig::kEnforce,
89  size_t log_size = 50);
90 
96  Robot(Robot&& other) noexcept;
97 
105  Robot& operator=(Robot&& other) noexcept;
106 
110  virtual ~Robot() noexcept;
111 
175  void control(std::function<Torques(const RobotState&, franka::Duration)> control_callback,
176  bool limit_rate = false,
177  double cutoff_frequency = kDefaultCutoffFrequency);
178 
203  void control(
204  std::function<Torques(const RobotState&, franka::Duration)> control_callback,
205  std::function<JointPositions(const RobotState&, franka::Duration)> motion_generator_callback,
206  bool limit_rate = false,
207  double cutoff_frequency = kDefaultCutoffFrequency);
208 
233  void control(
234  std::function<Torques(const RobotState&, franka::Duration)> control_callback,
235  std::function<JointVelocities(const RobotState&, franka::Duration)> motion_generator_callback,
236  bool limit_rate = false,
237  double cutoff_frequency = kDefaultCutoffFrequency);
238 
263  void control(
264  std::function<Torques(const RobotState&, franka::Duration)> control_callback,
265  std::function<CartesianPose(const RobotState&, franka::Duration)> motion_generator_callback,
266  bool limit_rate = false,
267  double cutoff_frequency = kDefaultCutoffFrequency);
268 
293  void control(std::function<Torques(const RobotState&, franka::Duration)> control_callback,
294  std::function<CartesianVelocities(const RobotState&, franka::Duration)>
295  motion_generator_callback,
296  bool limit_rate = false,
297  double cutoff_frequency = kDefaultCutoffFrequency);
298 
321  void control(
322  std::function<JointPositions(const RobotState&, franka::Duration)> motion_generator_callback,
323  ControllerMode controller_mode = ControllerMode::kJointImpedance,
324  bool limit_rate = false,
325  double cutoff_frequency = kDefaultCutoffFrequency);
326 
349  void control(
350  std::function<JointVelocities(const RobotState&, franka::Duration)> motion_generator_callback,
351  ControllerMode controller_mode = ControllerMode::kJointImpedance,
352  bool limit_rate = false,
353  double cutoff_frequency = kDefaultCutoffFrequency);
354 
377  void control(
378  std::function<CartesianPose(const RobotState&, franka::Duration)> motion_generator_callback,
379  ControllerMode controller_mode = ControllerMode::kJointImpedance,
380  bool limit_rate = false,
381  double cutoff_frequency = kDefaultCutoffFrequency);
382 
405  void control(std::function<CartesianVelocities(const RobotState&, franka::Duration)>
406  motion_generator_callback,
407  ControllerMode controller_mode = ControllerMode::kJointImpedance,
408  bool limit_rate = false,
409  double cutoff_frequency = kDefaultCutoffFrequency);
410 
435  void read(std::function<bool(const RobotState&)> read_callback);
436 
449  virtual RobotState readOnce();
450 
465  auto getRobotModel() -> std::string;
466 
503  void setCollisionBehavior(const std::array<double, 7>& lower_torque_thresholds_acceleration,
504  const std::array<double, 7>& upper_torque_thresholds_acceleration,
505  const std::array<double, 7>& lower_torque_thresholds_nominal,
506  const std::array<double, 7>& upper_torque_thresholds_nominal,
507  const std::array<double, 6>& lower_force_thresholds_acceleration,
508  const std::array<double, 6>& upper_force_thresholds_acceleration,
509  const std::array<double, 6>& lower_force_thresholds_nominal,
510  const std::array<double, 6>& upper_force_thresholds_nominal);
511 
538  void setCollisionBehavior(const std::array<double, 7>& lower_torque_thresholds,
539  const std::array<double, 7>& upper_torque_thresholds,
540  const std::array<double, 6>& lower_force_thresholds,
541  const std::array<double, 6>& upper_force_thresholds);
542 
555  const std::array<double, 7>& K_theta); // NOLINT(readability-identifier-naming)
556 
572  const std::array<double, 6>& K_x); // NOLINT(readability-identifier-naming)
573 
588  void setGuidingMode(const std::array<bool, 6>& guiding_mode, bool elbow);
589 
602  void setK(const std::array<double, 16>& EE_T_K); // NOLINT(readability-identifier-naming)
603 
619  void setEE(const std::array<double, 16>& NE_T_EE); // NOLINT(readability-identifier-naming)
620 
637  void setLoad(double load_mass,
638  const std::array<double, 3>& F_x_Cload, // NOLINT(readability-identifier-naming)
639  const std::array<double, 9>& load_inertia);
640 
650 
662  virtual std::unique_ptr<ActiveControlBase> startTorqueControl();
663 
678  const research_interface::robot::Move::ControllerMode& control_type);
679 
693  const research_interface::robot::Move::ControllerMode& control_type);
694 
708  const research_interface::robot::Move::ControllerMode& control_type);
709 
724  const research_interface::robot::Move::ControllerMode& control_type);
725 
735  void stop();
736 
750 
751  // Loads the model library for the unittests mockRobotModel
752  Model loadModel(std::unique_ptr<RobotModelBase> robot_model);
753 
759  ServerVersion serverVersion() const noexcept;
760 
762  Robot(const Robot&) = delete;
763  Robot& operator=(const Robot&) = delete;
765 
766  class Impl;
767 
768  protected:
774  Robot(std::shared_ptr<Impl> robot_impl);
775 
779  Robot() = default;
780 
781  private:
796  template <typename T>
797  std::unique_ptr<ActiveControlBase> startControl(
798  const research_interface::robot::Move::ControllerMode& controller_type);
799 
800  std::shared_ptr<Impl> impl_;
801  std::mutex control_mutex_;
802 };
803 
804 } // namespace franka
Robot dynamic parameters computed from the URDF model with Pinocchio.
Definition: robot_model_base.h:10
Allows the user to read the state of a Robot and to send new control commands after starting a contro...
Definition: active_control_base.h:27
Stores values for Cartesian pose motion generation.
Definition: control_types.h:127
Stores values for Cartesian velocity motion generation.
Definition: control_types.h:211
Represents a duration with millisecond resolution.
Definition: duration.h:19
Stores values for joint position motion generation.
Definition: control_types.h:72
Stores values for joint velocity motion generation.
Definition: control_types.h:99
Calculates poses of joints and dynamic properties of the robot.
Definition: model.h:52
Maintains a network connection to the robot, provides the current robot state, gives access to the mo...
Definition: robot.h:68
void control(std::function< Torques(const RobotState &, franka::Duration)> control_callback, bool limit_rate=false, double cutoff_frequency=kDefaultCutoffFrequency)
Starts a control loop for sending joint-level torque commands.
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)
Changes the collision behavior.
Model loadModel()
Loads the model library from the robot.
Robot & operator=(Robot &&other) noexcept
Move-assigns this Robot from another Robot instance.
Robot(Robot &&other) noexcept
Move-constructs a new Robot instance.
virtual std::unique_ptr< ActiveControlBase > startCartesianPoseControl(const research_interface::robot::Move::ControllerMode &control_type)
Starts a new cartesian position motion generator.
ServerVersion serverVersion() const noexcept
Returns the software version reported by the connected server.
auto getRobotModel() -> std::string
void stop()
Stops all currently running motions.
void setGuidingMode(const std::array< bool, 6 > &guiding_mode, bool elbow)
Locks or unlocks guiding mode movement in (x, y, z, roll, pitch, yaw).
virtual std::unique_ptr< ActiveControlBase > startTorqueControl()
Starts a new torque controller.
void read(std::function< bool(const RobotState &)> read_callback)
Starts a loop for reading the current robot state.
virtual std::unique_ptr< ActiveControlBase > startCartesianVelocityControl(const research_interface::robot::Move::ControllerMode &control_type)
Starts a new cartesian velocity motion generator.
void setJointImpedance(const std::array< double, 7 > &K_theta)
Sets the impedance for each joint in the internal controller.
virtual ~Robot() noexcept
Closes the connection.
void setCartesianImpedance(const std::array< double, 6 > &K_x)
Sets the Cartesian stiffness/compliance (for x, y, z, roll, pitch, yaw) in the internal controller.
virtual std::unique_ptr< ActiveControlBase > startJointPositionControl(const research_interface::robot::Move::ControllerMode &control_type)
Starts a new joint position motion generator.
void setK(const std::array< double, 16 > &EE_T_K)
Sets the transformation from end effector frame to stiffness frame.
uint16_t ServerVersion
Version of the robot server.
Definition: robot.h:73
virtual RobotState readOnce()
Waits for a robot state update and returns it.
Robot(const std::string &franka_address, RealtimeConfig realtime_config=RealtimeConfig::kEnforce, size_t log_size=50)
Establishes a connection with the robot.
virtual std::unique_ptr< ActiveControlBase > startJointVelocityControl(const research_interface::robot::Move::ControllerMode &control_type)
Starts a new joint velocity motion generator.
void setEE(const std::array< double, 16 > &NE_T_EE)
Sets the transformation from nominal end effector to end effector frame.
void automaticErrorRecovery()
Runs automatic error recovery on the robot.
void setLoad(double load_mass, const std::array< double, 3 > &F_x_Cload, const std::array< double, 9 > &load_inertia)
Sets dynamic parameters of a payload.
Stores joint-level torque commands without gravity and friction.
Definition: control_types.h:45
Contains helper types for returning motion generation and joint-level torque commands.
ControllerMode
Available controller modes for a franka::Robot.
Definition: control_types.h:19
RealtimeConfig
Used to decide whether to enforce realtime mode for a control loop thread.
Definition: control_types.h:26
Contains the franka::Duration type.
Contains functions for filtering signals with a low-pass filter.
Contains the franka::RobotState types.
Describes the robot state.
Definition: robot_state.h:34