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>
39 using std::runtime_error::runtime_error;
47class Robot :
public franka::Robot {
152 using franka::Robot::setCollisionBehavior;
309 std::unique_lock
lock(*control_mutex_);
310 return joinMotionUnsafe(
lock);
320 template <
class Rep,
class Period>
322 std::unique_lock
lock(*control_mutex_);
333 return joinMotion(std::chrono::milliseconds(0));
390 std::shared_ptr<const Model> model_;
395 template <
typename ControlSignalType>
396 using ControlFunc = std::function<
ControlSignalType(
const franka::RobotState &, franka::Duration)>;
397 using MotionGeneratorVariant = std::variant<
403 std::string fci_hostname_;
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};
414 [[
nodiscard]]
bool is_in_control_unsafe()
const;
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_) {
490 if (control_finished_condition_.wait_for(
lock,
timeout.value()) == std::cv_status::timeout) {
494 control_finished_condition_.wait(
lock);
497 if (control_thread_.joinable()) control_thread_.join();
498 if (control_exception_ !=
nullptr) {
500 control_exception_ =
nullptr;
506 template <
typename ControlSignalType>
511 throw std::invalid_argument(
"The motion must not be null.");
514 std::unique_lock
lock(*control_mutex_);
515 if (is_in_control_unsafe() && motion_generator_running_) {
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.");
521 std::get<MotionGenerator<ControlSignalType>>(motion_generator_).updateMotion(
motion);
523 joinMotionUnsafe(
lock);
526 auto motion_generator = &std::get<MotionGenerator<ControlSignalType>>(motion_generator_);
529 std::lock_guard
lock(state_mutex_);
532 motion_generator_running_ =
true;
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);
552 std::unique_lock
lock(*control_mutex_);
561 motion_generator_running_ =
false;
562 control_finished_condition_.notify_all();
566 std::unique_lock
lock(*control_mutex_);
567 control_exception_ = std::current_exception();
568 motion_generator_running_ =
false;
569 control_finished_condition_.notify_all();
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