Franky 1.0.2
A High-Level Motion API for Franka
Loading...
Searching...
No Matches
robot.hpp
Go to the documentation of this file.
1#pragma once
2
3#include <franka/control_types.h>
4#include <franka/duration.h>
5#include <franka/exception.h>
6#include <franka/model.h>
7#include <franka/robot.h>
8#include <franka/robot_state.h>
9
10#include <exception>
11#include <future>
12#include <optional>
13#include <stdexcept>
14#include <variant>
15
20#include "franky/model.hpp"
24#include "franky/robot_pose.hpp"
27#include "franky/types.hpp"
28#include "franky/util.hpp"
29
30namespace franky {
31
38struct InvalidMotionTypeException : std::runtime_error {
39 using std::runtime_error::runtime_error;
40};
41
47class Robot : public franka::Robot {
48 public:
52 struct Params {
59
64
69
75 franka::ControllerMode controller_mode{franka::ControllerMode::kJointImpedance};
76
82 franka::RealtimeConfig realtime_config{franka::RealtimeConfig::kEnforce};
83
87 double kalman_q_process_var = 0.0001;
88
92 double kalman_dq_process_var = 0.001;
93
98
103
107 double kalman_q_obs_var = 0.01;
108
112 double kalman_dq_obs_var = 0.1;
113
117 double kalman_q_d_obs_var = 0.0001;
118
122 double kalman_dq_d_obs_var = 0.0001;
123
127 double kalman_ddq_d_obs_var = 0.0001;
128
133 };
134
136 static constexpr size_t degrees_of_freedoms{7};
137
139 static constexpr double control_rate{0.001};
140
144 explicit Robot(const std::string &fci_hostname);
145
150 explicit Robot(const std::string &fci_hostname, const Params &params);
151
152 using franka::Robot::setCollisionBehavior;
153
161
173
201
206 bool recoverFromErrors();
207
212 [[nodiscard]] bool hasErrors();
213
219
225
231 auto s = state();
232 return {
233 {Affine(Eigen::Matrix4d::Map(s.O_T_EE.data())), ElbowState{s.elbow}}, RobotVelocity(s.O_dP_EE_c, s.delbow_c)};
234 }
235
241 auto s = state();
242 return {s.q, s.dq};
243 }
244
250
256
262
268
274
278 [[nodiscard]] bool is_in_control();
279
283 [[nodiscard]] std::string fci_hostname() const;
284
288 [[nodiscard]] std::optional<ControlSignalType> current_control_signal_type();
289
295 [[nodiscard]] std::shared_ptr<const Model> model() const { return model_; }
296
297#ifdef FRANKA_0_15
302 [[nodiscard]] std::string model_urdf() const { return model_urdf_; }
303#endif
304
308 bool joinMotion() {
309 std::unique_lock lock(*control_mutex_);
310 return joinMotionUnsafe(lock);
311 }
312
320 template <class Rep, class Period>
321 bool joinMotion(const std::chrono::duration<Rep, Period> &timeout) {
322 std::unique_lock lock(*control_mutex_);
324 }
325
331 [[nodiscard]]
332 bool pollMotion() {
333 return joinMotion(std::chrono::milliseconds(0));
334 }
335
336 // These helper functions are needed as the implicit template deduction does not work on subclasses of Motion
337
343 void move(const std::shared_ptr<Motion<franka::CartesianPose>> &motion, bool async = false) {
345 motion, [this](const ControlFunc<franka::CartesianPose> &m) { control(m, params_.controller_mode); }, async);
346 }
347
353 void move(const std::shared_ptr<Motion<franka::CartesianVelocities>> &motion, bool async = false) {
355 motion,
356 [this](const ControlFunc<franka::CartesianVelocities> &m) { control(m, params_.controller_mode); },
357 async);
358 }
359
365 void move(const std::shared_ptr<Motion<franka::JointPositions>> &motion, bool async = false) {
367 motion, [this](const ControlFunc<franka::JointPositions> &m) { control(m, params_.controller_mode); }, async);
368 }
369
375 void move(const std::shared_ptr<Motion<franka::JointVelocities>> &motion, bool async = false) {
377 motion, [this](const ControlFunc<franka::JointVelocities> &m) { control(m, params_.controller_mode); }, async);
378 }
379
385 void move(const std::shared_ptr<Motion<franka::Torques>> &motion, bool async = false) {
386 moveInternal<franka::Torques>(motion, [this](const ControlFunc<franka::Torques> &m) { control(m); }, async);
387 }
388
389 private:
390 std::shared_ptr<const Model> model_;
391#ifdef FRANKA_0_15
392 std::string model_urdf_;
393#endif
394
395 template <typename ControlSignalType>
396 using ControlFunc = std::function<ControlSignalType(const franka::RobotState &, franka::Duration)>;
397 using MotionGeneratorVariant = std::variant<
401
403 std::string fci_hostname_;
404 Params params_;
405 RobotState current_state_;
406 std::mutex state_mutex_;
407 std::shared_ptr<std::mutex> control_mutex_;
408 std::condition_variable control_finished_condition_;
409 std::exception_ptr control_exception_;
410 std::thread control_thread_;
411 MotionGeneratorVariant motion_generator_{std::nullopt};
412 bool motion_generator_running_{false};
413
414 [[nodiscard]] bool is_in_control_unsafe() const;
415
416 public:
417 // IMPORTANT: this has to come after control_mutex_ as otherwise control_mutex_ will be uninitialized when passed to
418 // the constructor of the DynamicsLimit class
419 // Limits provided by Franka for the FR3: https://frankaemika.github.io/docs/control_parameters.html
420 // clang-format off
425
430
435
440
445
450
455
460
465
470
475
480
481 // clang-format on
482
483 private:
484 template <class Rep = long, class Period = std::ratio<1>>
485 bool joinMotionUnsafe(
486 std::unique_lock<std::mutex> &lock,
487 const std::optional<std::chrono::duration<Rep, Period>> &timeout = std::nullopt) {
488 while (motion_generator_running_) {
489 if (timeout.has_value()) {
490 if (control_finished_condition_.wait_for(lock, timeout.value()) == std::cv_status::timeout) {
491 return false;
492 }
493 } else {
494 control_finished_condition_.wait(lock);
495 }
496 }
497 if (control_thread_.joinable()) control_thread_.join();
498 if (control_exception_ != nullptr) {
499 auto control_exception = control_exception_;
500 control_exception_ = nullptr;
501 std::rethrow_exception(control_exception);
502 }
503 return true;
504 }
505
506 template <typename ControlSignalType>
507 void moveInternal(
508 const std::shared_ptr<Motion<ControlSignalType>> &motion,
509 const std::function<void(const ControlFunc<ControlSignalType> &)> &control_func_executor, bool async) {
510 if (motion == nullptr) {
511 throw std::invalid_argument("The motion must not be null.");
512 }
513 { // Do not remove brace, it is needed to scope the lock
514 std::unique_lock lock(*control_mutex_);
515 if (is_in_control_unsafe() && motion_generator_running_) {
516 if (!std::holds_alternative<MotionGenerator<ControlSignalType>>(motion_generator_)) {
517 throw InvalidMotionTypeException(
518 "The type of motion cannot change during runtime. Please ensure that the "
519 "previous motion finished before using a new type of motion.");
520 }
521 std::get<MotionGenerator<ControlSignalType>>(motion_generator_).updateMotion(motion);
522 } else {
523 joinMotionUnsafe(lock);
524
525 motion_generator_.emplace<MotionGenerator<ControlSignalType>>(this, motion);
526 auto motion_generator = &std::get<MotionGenerator<ControlSignalType>>(motion_generator_);
527 motion_generator->registerUpdateCallback(
528 [this](const RobotState &robot_state, franka::Duration duration, franka::Duration time) {
529 std::lock_guard lock(state_mutex_);
530 current_state_ = robot_state;
531 });
532 motion_generator_running_ = true;
533 control_thread_ = std::thread([this, control_func_executor, motion_generator]() {
534 try {
535 bool done = false;
536 RobotStateEstimator robot_state_estimator(
537 params_.kalman_q_process_var,
538 params_.kalman_dq_process_var,
539 params_.kalman_ddq_process_var,
540 params_.kalman_control_process_var,
541 params_.kalman_q_obs_var,
542 params_.kalman_dq_obs_var,
543 params_.kalman_q_d_obs_var,
544 params_.kalman_dq_d_obs_var,
545 params_.kalman_ddq_d_obs_var,
546 params_.kalman_control_adaptation_rate);
547 while (!done) {
549 [this, motion_generator, &robot_state_estimator](const franka::RobotState &rs, franka::Duration d) {
550 return (*motion_generator)(robot_state_estimator.update(rs, *model_), d);
551 });
552 std::unique_lock lock(*control_mutex_);
553
554 // This code is just for the case that a new motion is set just after the old one terminates. If this
555 // happens, we need to continue with this motion, unless an exception occurs.
556 done = !motion_generator->has_new_motion();
557 if (motion_generator->has_new_motion()) {
558 motion_generator->resetTimeUnsafe();
559 } else {
560 done = true;
561 motion_generator_running_ = false;
562 control_finished_condition_.notify_all();
563 }
564 }
565 } catch (...) {
566 std::unique_lock lock(*control_mutex_);
567 control_exception_ = std::current_exception();
568 motion_generator_running_ = false;
569 control_finished_condition_.notify_all();
570 }
571 });
572 }
573 }
574 if (!async) joinMotion();
575 }
576};
577
578} // namespace franky
Definition cartesian_state.hpp:17
RobotPose pose() const
Definition cartesian_state.hpp:60
RobotVelocity velocity() const
Definition cartesian_state.hpp:67
A template class representing a dynamics limit with a maximum value.
Definition dynamics_limit.hpp:22
Elbow state of the robot.
Definition elbow_state.hpp:27
Joint state of a robot.
Definition joint_state.hpp:15
Vector7d velocity() const
The velocity component of the state.
Definition joint_state.hpp:49
Vector7d position() const
The position component of the state.
Definition joint_state.hpp:42
Helper class for handling motions and reactions.
Definition motion_generator.hpp:31
Base class for motions.
Definition motion.hpp:23
Relative dynamics factors.
Definition relative_dynamics_factor.hpp:13
A class representing a Franka robot.
Definition robot.hpp:47
DynamicsLimit< Vector7d > joint_acceleration_limit
Joint acceleration limit [rad/s²].
Definition robot.hpp:474
void move(const std::shared_ptr< Motion< franka::JointPositions > > &motion, bool async=false)
Execute the given motion.
Definition robot.hpp:365
bool joinMotion(const std::chrono::duration< Rep, Period > &timeout)
Wait for the current motion to finish with a timeout. Throw any exceptions that occurred during the m...
Definition robot.hpp:321
std::optional< ControlSignalType > current_control_signal_type()
The type of the current control signal.
Definition robot.cpp:117
void move(const std::shared_ptr< Motion< franka::CartesianPose > > &motion, bool async=false)
Execute the given motion.
Definition robot.hpp:343
std::string fci_hostname() const
The hostname of the robot.
Definition robot.cpp:115
Vector7d currentJointVelocities()
Returns the current joint velocities of the robot.
Definition robot.hpp:255
bool joinMotion()
Wait for the current motion to finish. Throw any exceptions that occurred during the motion.
Definition robot.hpp:308
static constexpr double control_rate
Definition robot.hpp:139
DynamicsLimit< double > elbow_acceleration_limit
Elbow acceleration limit [rad/s²].
Definition robot.hpp:449
RelativeDynamicsFactor relative_dynamics_factor()
Returns the current global relative dynamics factor of the robot.
Definition robot.cpp:128
Vector7d currentJointPositions()
Returns the current joint positions of the robot.
Definition robot.hpp:249
void move(const std::shared_ptr< Motion< franka::Torques > > &motion, bool async=false)
Execute the given motion.
Definition robot.hpp:385
CartesianState currentCartesianState()
Returns the current cartesian state of the robot.
Definition robot.hpp:230
DynamicsLimit< Vector7d > joint_velocity_limit
Joint velocity limit [rad/s].
Definition robot.hpp:469
RobotState state()
Returns the current state of the robot.
Definition robot.cpp:61
DynamicsLimit< double > translation_jerk_limit
Translational jerk limit [m/s³].
Definition robot.hpp:454
DynamicsLimit< double > translation_velocity_limit
Translational velocity limit [m/s].
Definition robot.hpp:424
bool recoverFromErrors()
Calls the automatic error recovery of the robot and returns whether the recovery was successful.
Definition robot.cpp:56
DynamicsLimit< double > elbow_velocity_limit
Elbow velocity limit [rad/s].
Definition robot.hpp:434
DynamicsLimit< double > rotation_velocity_limit
Rotational velocity limit [rad/s].
Definition robot.hpp:429
DynamicsLimit< double > elbow_jerk_limit
Elbow jerk limit [rad/s³].
Definition robot.hpp:464
bool is_in_control()
Whether the robot is currently in control, i.e. a motion is being executed.
Definition robot.cpp:110
void move(const std::shared_ptr< Motion< franka::JointVelocities > > &motion, bool async=false)
Execute the given motion.
Definition robot.hpp:375
DynamicsLimit< Vector7d > joint_jerk_limit
Joint jerk limit [rad/s³].
Definition robot.hpp:479
JointState currentJointState()
Returns the current joint state of the robot.
Definition robot.hpp:240
void setCollisionBehavior(const ScalarOrArray< 7 > &torque_threshold, const ScalarOrArray< 6 > &force_threshold)
Set the collision behavior of the robot.
Definition robot.cpp:72
static constexpr size_t degrees_of_freedoms
Definition robot.hpp:136
RobotVelocity currentCartesianVelocity()
Returns the current cartesian velocity of the robot.
Definition robot.hpp:224
DynamicsLimit< double > rotation_jerk_limit
Rotational jerk limit [rad/s³].
Definition robot.hpp:459
void move(const std::shared_ptr< Motion< franka::CartesianVelocities > > &motion, bool async=false)
Execute the given motion.
Definition robot.hpp:353
void setRelativeDynamicsFactor(const RelativeDynamicsFactor &relative_dynamics_factor)
Sets the global relative dynamics factor of the robot.
Definition robot.cpp:133
std::shared_ptr< const Model > model() const
The model of the robot.
Definition robot.hpp:295
DynamicsLimit< double > translation_acceleration_limit
Translational acceleration limit [m/s²].
Definition robot.hpp:439
bool hasErrors()
Returns whether the robot has errors.
Definition robot.cpp:54
RobotPose currentPose()
Returns the current pose of the robot.
Definition robot.hpp:218
DynamicsLimit< double > rotation_acceleration_limit
Rotational acceleration limit [rad/s²].
Definition robot.hpp:444
bool pollMotion()
Check whether the robot is still in motion. This function is non-blocking and returns immediately....
Definition robot.hpp:332
Cartesian pose of a robot.
Definition robot_pose.hpp:17
Cartesian velocity of a robot.
Definition robot_velocity.hpp:19
Definition dynamics_limit.cpp:8
std::array< double, dims > toStdD(const Eigen::Matrix< double, dims, 1 > &vector)
Definition util.hpp:18
Eigen::Vector< double, 7 > Vector7d
Definition types.hpp:12
ControlSignalType
Type of control signal.
Definition control_signal_type.hpp:8
std::variant< double, Array< dims > > ScalarOrArray
Definition types.hpp:22
Eigen::Affine3d Affine
Definition types.hpp:16
Exception thrown when an invalid motion type is used.
Definition robot.hpp:38
Global parameters for the robot.
Definition robot.hpp:52
double kalman_dq_process_var
Kalman parameter: process noise variance of the velocity.
Definition robot.hpp:92
RelativeDynamicsFactor relative_dynamics_factor
Relative dynamics factor for the robot.
Definition robot.hpp:58
double default_force_threshold
Default force threshold for collision behavior.
Definition robot.hpp:68
double kalman_control_adaptation_rate
Kalman parameter: rate of adaptation of the robot state to the desired robot state.
Definition robot.hpp:132
double kalman_dq_obs_var
Kalman parameter: observation noise variance of measured joint velocities.
Definition robot.hpp:112
double kalman_q_obs_var
Kalman parameter: observation noise variance of measured joint positions.
Definition robot.hpp:107
double kalman_dq_d_obs_var
Kalman parameter: observation noise variance of desired joint velocities.
Definition robot.hpp:122
franka::ControllerMode controller_mode
Default controller mode for the robot.
Definition robot.hpp:75
double kalman_control_process_var
Kalman parameter: process noise variance of the control signal.
Definition robot.hpp:102
double kalman_q_d_obs_var
Kalman parameter: observation noise variance of desired joint positions.
Definition robot.hpp:117
franka::RealtimeConfig realtime_config
Default realtime configuration for the robot.
Definition robot.hpp:82
double kalman_ddq_d_obs_var
Kalman parameter: observation noise variance of desired joint accelerations.
Definition robot.hpp:127
double default_torque_threshold
Default torque threshold for collision behavior.
Definition robot.hpp:63
double kalman_ddq_process_var
Kalman parameter: process noise variance of the acceleration.
Definition robot.hpp:97
double kalman_q_process_var
Kalman parameter: process noise variance of the position.
Definition robot.hpp:87
Full state of the robot.
Definition robot_state.hpp:20