Class Robot
Defined in File robot.h
Class Documentation
-
class Robot
Maintains a network connection to the robot, provides the current robot state, gives access to the model library and allows to control the robot.
- Base frame O
The base frame is located at the center of the robot’s base. Its z-axis is identical with the axis of rotation of the first joint.
- Flange frame F
The flange frame is located at the center of the flange surface. Its z-axis is identical with the axis of rotation of the last joint. This frame is fixed and cannot be changed.
- Nominal end effector frame NE
The nominal end effector frame is configured outside of libfranka (in DESK) and cannot be changed here. It may be used to set end effector frames which are rarely changed.
- end effector frame EE
By default, the end effector frame EE is the same as the nominal end effector frame NE (i.e. the transformation between NE and EE is the identity transformation). It may be used to set end effector frames which are changed more frequently (such as a tool that is grasped with the end effector). With Robot::setEE, a custom transformation matrix can be set.
- Stiffness frame K
This frame describes the transformation from the end effector frame EE to the stiffness frame K. The stiffness frame is used for Cartesian impedance control, and for measuring and applying forces. The values set using Robot::setCartesianImpedance are used in the direction of the stiffness frame. It can be set with Robot::setK. This frame allows to modify the compliance behavior of the robot (e.g. to have a low stiffness around a specific point which is not the end effector). The stiffness frame does not affect where the robot will move to. The user should always command the desired end effector frame (and not the desired stiffness frame).
Note
The members of this class are threadsafe.
Motion generation and joint-level torque commands
The following methods allow to perform motion generation and/or send joint-level torque commands without gravity and friction by providing callback functions.
Only one of these methods can be active at the same time; a franka::ControlException is thrown otherwise.
When a robot state is received, the callback function is used to calculate the response: the desired values for that time step. After sending back the response, the callback function will be called again with the most recently received robot state. Since the robot is controlled with a 1 kHz frequency, the callback functions have to compute their result in a short time frame in order to be accepted. Callback functions take two parameters:
A franka::RobotState showing the current robot state.
A franka::Duration to indicate the time since the last callback invocation. Thus, the duration is zero on the first invocation of the callback function!
The following incomplete example shows the general structure of a callback function:
double time = 0.0; auto control_callback = [&time](const franka::RobotState& robot_state, franka::Duration time_step) -> franka::JointPositions { time += time_step.toSec(); // Update time at the beginning of the callback. franka::JointPositions output = getJointPositions(time); if (time >= max_time) { // Return MotionFinished at the end of the trajectory. return franka::MotionFinished(output); } return output; }
-
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.
Sets realtime priority for the current thread. Cannot be executed while another control or motion generator loop is active.
See also
Robot::Robot to change behavior if realtime priority cannot be set.
- Parameters:
control_callback – [in] Callback function providing joint-level torque commands. See here for more details.
limit_rate – [in] True if rate limiting should be activated. False by default. This could distort your motion!
cutoff_frequency – [in] Cutoff frequency for a first order low-pass filter applied on the user commanded signal. Set to franka::kMaxCutoffFrequency to disable.
- Throws:
ControlException – if an error related to torque control or motion generation occurred.
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
RealtimeException – if realtime priority cannot be set for the current thread.
std::invalid_argument – if joint-level torque commands are NaN or infinity.
-
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)
Starts a control loop for sending joint-level torque commands and joint positions.
Sets realtime priority for the current thread. Cannot be executed while another control or motion generator loop is active.
See also
Robot::Robot to change behavior if realtime priority cannot be set.
- Parameters:
control_callback – [in] Callback function providing joint-level torque commands. See here for more details.
motion_generator_callback – [in] Callback function for motion generation. See here for more details.
limit_rate – [in] True if rate limiting should be activated. False by default. This could distort your motion!
cutoff_frequency – [in] Cutoff frequency for a first order low-pass filter applied on the user commanded signal. Set to franka::kMaxCutoffFrequency to disable.
- Throws:
ControlException – if an error related to torque control or motion generation occurred.
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
RealtimeException – if realtime priority cannot be set for the current thread.
std::invalid_argument – if joint-level torque or joint position commands are NaN or infinity.
-
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)
Starts a control loop for sending joint-level torque commands and joint velocities.
Sets realtime priority for the current thread. Cannot be executed while another control or motion generator loop is active.
See also
Robot::Robot to change behavior if realtime priority cannot be set.
- Parameters:
control_callback – [in] Callback function providing joint-level torque commands. See here for more details.
motion_generator_callback – [in] Callback function for motion generation. See here for more details.
limit_rate – [in] True if rate limiting should be activated. False by default. This could distort your motion!
cutoff_frequency – [in] Cutoff frequency for a first order low-pass filter applied on the user commanded signal. Set to franka::kMaxCutoffFrequency to disable.
- Throws:
ControlException – if an error related to torque control or motion generation occurred.
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
RealtimeException – if realtime priority cannot be set for the current thread.
std::invalid_argument – if joint-level torque or joint velocity commands are NaN or infinity.
-
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)
Starts a control loop for sending joint-level torque commands and Cartesian poses.
Sets realtime priority for the current thread. Cannot be executed while another control or motion generator loop is active.
See also
Robot::Robot to change behavior if realtime priority cannot be set.
- Parameters:
control_callback – [in] Callback function providing joint-level torque commands. See here for more details.
motion_generator_callback – [in] Callback function for motion generation. See here for more details.
limit_rate – [in] True if rate limiting should be activated. False by default. This could distort your motion!
cutoff_frequency – [in] Cutoff frequency for a first order low-pass filter applied on the user commanded signal. Set to franka::kMaxCutoffFrequency to disable.
- Throws:
ControlException – if an error related to torque control or motion generation occurred.
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
RealtimeException – if realtime priority cannot be set for the current thread.
std::invalid_argument – if joint-level torque or Cartesian pose command elements are NaN or infinity.
-
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)
Starts a control loop for sending joint-level torque commands and Cartesian velocities.
Sets realtime priority for the current thread. Cannot be executed while another control or motion generator loop is active.
See also
Robot::Robot to change behavior if realtime priority cannot be set.
- Parameters:
control_callback – [in] Callback function providing joint-level torque commands. See here for more details.
motion_generator_callback – [in] Callback function for motion generation. See here for more details.
limit_rate – [in] True if rate limiting should be activated. False by default. This could distort your motion!
cutoff_frequency – [in] Cutoff frequency for a first order low-pass filter applied on the user commanded signal. Set to franka::kMaxCutoffFrequency to disable.
- Throws:
ControlException – if an error related to torque control or motion generation occurred.
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
RealtimeException – if realtime priority cannot be set for the current thread.
std::invalid_argument – if joint-level torque or Cartesian velocity command elements are NaN or infinity.
-
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)
Starts a control loop for a joint position motion generator with a given controller mode.
Sets realtime priority for the current thread. Cannot be executed while another control or motion generator loop is active.
See also
Robot::Robot to change behavior if realtime priority cannot be set.
- Parameters:
motion_generator_callback – [in] Callback function for motion generation. See here for more details.
controller_mode – [in] Controller to use to execute the motion.
limit_rate – [in] True if rate limiting should be activated. False by default. This could distort your motion!
cutoff_frequency – [in] Cutoff frequency for a first order low-pass filter applied on the user commanded signal. Set to franka::kMaxCutoffFrequency to disable.
- Throws:
ControlException – if an error related to motion generation occurred.
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
RealtimeException – if realtime priority cannot be set for the current thread.
std::invalid_argument – if joint position commands are NaN or infinity.
-
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)
Starts a control loop for a joint velocity motion generator with a given controller mode.
Sets realtime priority for the current thread. Cannot be executed while another control or motion generator loop is active.
See also
Robot::Robot to change behavior if realtime priority cannot be set.
- Parameters:
motion_generator_callback – [in] Callback function for motion generation. See here for more details.
controller_mode – [in] Controller to use to execute the motion.
limit_rate – [in] True if rate limiting should be activated. False by default. This could distort your motion!
cutoff_frequency – [in] Cutoff frequency for a first order low-pass filter applied on the user commanded signal. Set to franka::kMaxCutoffFrequency to disable.
- Throws:
ControlException – if an error related to motion generation occurred.
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
RealtimeException – if realtime priority cannot be set for the current thread.
std::invalid_argument – if joint velocity commands are NaN or infinity.
-
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)
Starts a control loop for a Cartesian pose motion generator with a given controller mode.
Sets realtime priority for the current thread. Cannot be executed while another control or motion generator loop is active.
See also
Robot::Robot to change behavior if realtime priority cannot be set.
- Parameters:
motion_generator_callback – [in] Callback function for motion generation. See here for more details.
controller_mode – [in] Controller to use to execute the motion.
limit_rate – [in] True if rate limiting should be activated. False by default. This could distort your motion!
cutoff_frequency – [in] Cutoff frequency for a first order low-pass filter applied on the user commanded signal. Set to franka::kMaxCutoffFrequency to disable.
- Throws:
ControlException – if an error related to motion generation occurred.
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
RealtimeException – if realtime priority cannot be set for the current thread.
std::invalid_argument – if Cartesian pose command elements are NaN or infinity.
-
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)
Starts a control loop for a Cartesian velocity motion generator with a given controller mode.
Sets realtime priority for the current thread. Cannot be executed while another control or motion generator loop is active.
See also
Robot::Robot to change behavior if realtime priority cannot be set.
- Parameters:
motion_generator_callback – [in] Callback function for motion generation. See here for more details.
controller_mode – [in] Controller to use to execute the motion.
limit_rate – [in] True if rate limiting should be activated. False by default. This could distort your motion!
cutoff_frequency – [in] Cutoff frequency for a first order low-pass filter applied on the user commanded signal. Set to franka::kMaxCutoffFrequency to disable.
- Throws:
ControlException – if an error related to motion generation occurred.
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
RealtimeException – if realtime priority cannot be set for the current thread.
std::invalid_argument – if Cartesian velocity command elements are NaN or infinity.
Commands
Commands are executed by communicating with the robot over the network. These functions should therefore not be called from within control or motion generator loops.
-
auto getRobotModel() -> std::string
- Throws:
CommandException – if the Control reports an error.
NetworkException – if the connection is lost, e.g. after a timeout.
- Returns:
std::string Provides the URDF model of the attached robot arm as json string
-
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.
Set separate torque and force boundaries for acceleration/deceleration and constant velocity movement phases.
Forces or torques between lower and upper threshold are shown as contacts in the RobotState. Forces or torques above the upper threshold are registered as collision and cause the robot to stop moving.
See also
See also
See also
See also
See also
Robot::automaticErrorRecovery for performing a reset after a collision.
- Parameters:
lower_torque_thresholds_acceleration – [in] Contact torque thresholds during acceleration/deceleration for each joint in
.upper_torque_thresholds_acceleration – [in] Collision torque thresholds during acceleration/deceleration for each joint in
.lower_torque_thresholds_nominal – [in] Contact torque thresholds for each joint in
.upper_torque_thresholds_nominal – [in] Collision torque thresholds for each joint in
.lower_force_thresholds_acceleration – [in] Contact force thresholds during acceleration/deceleration for
in .upper_force_thresholds_acceleration – [in] Collision force thresholds during acceleration/deceleration for
in .lower_force_thresholds_nominal – [in] Contact force thresholds for
in .upper_force_thresholds_nominal – [in] Collision force thresholds for
in .
- Throws:
CommandException – if the Control reports an error.
NetworkException – if the connection is lost, e.g. after a timeout.
-
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)
Changes the collision behavior.
Set common torque and force boundaries for acceleration/deceleration and constant velocity movement phases.
Forces or torques between lower and upper threshold are shown as contacts in the RobotState. Forces or torques above the upper threshold are registered as collision and cause the robot to stop moving.
See also
See also
See also
See also
See also
Robot::automaticErrorRecovery for performing a reset after a collision.
- Parameters:
lower_torque_thresholds – [in] Contact torque thresholds for each joint in
.upper_torque_thresholds – [in] Collision torque thresholds for each joint in
.lower_force_thresholds – [in] Contact force thresholds for
in .upper_force_thresholds – [in] Collision force thresholds for
in .
- Throws:
CommandException – if the Control reports an error.
NetworkException – if the connection is lost, e.g. after a timeout.
-
void setJointImpedance(const std::array<double, 7> &K_theta)
Sets the impedance for each joint in the internal controller.
User-provided torques are not affected by this setting.
- Parameters:
K_theta – [in] Joint impedance values
- Throws:
CommandException – if the Control reports an error.
NetworkException – if the connection is lost, e.g. after a timeout.
-
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.
The values set using Robot::setCartesianImpedance are used in the direction of the stiffness frame, which can be set with Robot::setK.
Inputs received by the torque controller are not affected by this setting.
- Parameters:
K_x – [in] Cartesian impedance values
- Throws:
CommandException – if the Control reports an error.
NetworkException – if the connection is lost, e.g. after a timeout.
-
void setGuidingMode(const std::array<bool, 6> &guiding_mode, bool elbow)
Locks or unlocks guiding mode movement in (x, y, z, roll, pitch, yaw).
If a flag is set to true, movement is unlocked.
Note
Guiding mode can be enabled by pressing the two opposing buttons near the robot’s flange.
- Parameters:
guiding_mode – [in] Unlocked movement in (x, y, z, R, P, Y) in guiding mode.
elbow – [in] True if the elbow is free in guiding mode, false otherwise.
- Throws:
CommandException – if the Control reports an error.
NetworkException – if the connection is lost, e.g. after a timeout.
-
void setK(const std::array<double, 16> &EE_T_K)
Sets the transformation
from end effector frame to stiffness frame.The transformation matrix is represented as a vectorized 4x4 matrix in column-major format.
See also
Robot for an explanation of the stiffness frame.
- Parameters:
EE_T_K – [in] Vectorized EE-to-K transformation matrix
, column-major.- Throws:
CommandException – if the Control reports an error.
NetworkException – if the connection is lost, e.g. after a timeout.
-
void setEE(const std::array<double, 16> &NE_T_EE)
Sets the transformation
from nominal end effector to end effector frame.The transformation matrix is represented as a vectorized 4x4 matrix in column-major format.
See also
RobotState::NE_T_EE for end effector pose in nominal end effector frame
NE”.
@see RobotState::O_T_EE for end effector pose in @ref o-frame “world base frame O”.
@see RobotState::F_T_EE for end effector pose in @ref f-frame “flange frame F”.
- Parameters:
NE_T_EE – [in] Vectorized NE-to-EE transformation matrix
, column-major.- Throws:
CommandException – if the Control reports an error.
NetworkException – if the connection is lost, e.g. after a timeout.
-
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.
Note
This is not for setting end effector parameters, which have to be set in the administrator’s interface.
- Parameters:
load_mass – [in] Mass of the load in
.F_x_Cload – [in] Translation from flange to center of mass of load
in .load_inertia – [in] Inertia matrix
in , column-major.
- Throws:
CommandException – if the Control reports an error.
NetworkException – if the connection is lost, e.g. after a timeout.
-
void automaticErrorRecovery()
Runs automatic error recovery on the robot.
Automatic error recovery e.g. resets the robot after a collision occurred.
- Throws:
CommandException – if the Control reports an error.
NetworkException – if the connection is lost, e.g. after a timeout.
-
virtual std::unique_ptr<ActiveControlBase> startTorqueControl()
Starts a new torque controller
- Throws:
ControlException – if an error related to torque control or motion generation occurred.
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
std::invalid_argument – if joint-level torque commands are NaN or infinity.
- Returns:
unique_ptr of ActiveTorqueControl for the started motion
-
virtual std::unique_ptr<ActiveControlBase> startJointPositionControl(const research_interface::robot::Move::ControllerMode &control_type)
Starts a new joint position motion generator
- Parameters:
control_type – research_interface::robot::Move::ControllerMode control type for the operation
- Throws:
ControlException – if an error related to torque control or motion generation occurred.
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
std::invalid_argument – if joint-level torque commands are NaN or infinity.
- Returns:
unique_ptr of ActiveMotionGenerator for the started motion
-
virtual std::unique_ptr<ActiveControlBase> startJointVelocityControl(const research_interface::robot::Move::ControllerMode &control_type)
Starts a new joint velocity motion generator
- Parameters:
control_type – research_interface::robot::Move::ControllerMode control type for the operation
- Throws:
ControlException – if an error related to torque control or motion generation occurred.
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
std::invalid_argument – if joint-level torque commands are NaN or infinity.
- Returns:
unique_ptr of ActiveMotionGenerator for the started motion
-
virtual std::unique_ptr<ActiveControlBase> startCartesianPoseControl(const research_interface::robot::Move::ControllerMode &control_type)
Starts a new cartesian position motion generator
- Parameters:
control_type – research_interface::robot::Move::ControllerMode control type for the operation
- Throws:
ControlException – if an error related to torque control or motion generation occurred.
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
std::invalid_argument – if joint-level torque commands are NaN or infinity.
- Returns:
unique_ptr of ActiveMotionGenerator for the started motion
-
virtual std::unique_ptr<ActiveControlBase> startCartesianVelocityControl(const research_interface::robot::Move::ControllerMode &control_type)
Starts a new cartesian velocity motion generator
- Parameters:
control_type – research_interface::robot::Move::ControllerMode control type for the operation
- Throws:
ControlException – if an error related to torque control or motion generation occurred.
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
std::invalid_argument – if joint-level torque commands are NaN or infinity.
- Returns:
unique_ptr of ActiveMotionGenerator for the started motion
-
void stop()
Stops all currently running motions.
If a control or motion generator loop is running in another thread, it will be preempted with a franka::ControlException.
- Throws:
CommandException – if the Control reports an error.
NetworkException – if the connection is lost, e.g. after a timeout.
Public Types
-
using ServerVersion = uint16_t
Version of the robot server.
Public Functions
-
explicit Robot(const std::string &franka_address, RealtimeConfig realtime_config = RealtimeConfig::kEnforce, size_t log_size = 50)
Establishes a connection with the robot.
- Parameters:
franka_address – [in] IP/hostname of the robot.
realtime_config – [in] if set to Enforce, an exception will be thrown if realtime priority cannot be set when required. Setting realtime_config to Ignore disables this behavior.
log_size – [in] sets how many last states should be kept for logging purposes. The log is provided when a ControlException is thrown.
- Throws:
NetworkException – if the connection is unsuccessful.
IncompatibleVersionException – if this version of
libfranka
is not supported.
-
Robot(Robot &&other) noexcept
Move-constructs a new Robot instance.
- Parameters:
other – [in] Other Robot instance.
-
virtual ~Robot() noexcept
Closes the connection.
-
void read(std::function<bool(const RobotState&)> read_callback)
Starts a loop for reading the current robot state.
Cannot be executed while a control or motion generator loop is running.
This minimal example will print the robot state 100 times:
franka::Robot robot("robot.franka.de"); size_t count = 0; robot.read([&count](const franka::RobotState& robot_state) { std::cout << robot_state << std::endl; return count++ < 100; });
- Parameters:
read_callback – [in] Callback function for robot state reading.
- Throws:
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
-
virtual RobotState readOnce()
Waits for a robot state update and returns it.
Cannot be executed while a control or motion generator loop is running.
See also
Robot::read for a way to repeatedly receive the robot state.
- Throws:
InvalidOperationException – if a conflicting operation is already running.
NetworkException – if the connection is lost, e.g. after a timeout.
- Returns:
Current robot state.
-
Model loadModel()
Loads the model library from the robot.
- Throws:
ModelException – if the model library cannot be loaded.
NetworkException – if the connection is lost, e.g. after a timeout.
- Returns:
Model instance.
-
Model loadModel(std::unique_ptr<RobotModelBase> robot_model)
-
ServerVersion serverVersion() const noexcept
Returns the software version reported by the connected server.
- Returns:
Software version of the connected server.