This class wraps the functionality of libfranka for controlling Panda robots into the ros_control framework. More...
#include <franka_hw.h>
Classes | |
struct | CollisionConfig |
Public Member Functions | |
virtual bool | checkForConflict (const std::list< hardware_interface::ControllerInfo > &info) const override |
Checks whether a requested controller can be run, based on the resources and interfaces it claims. More... | |
virtual void | checkJointLimits () |
Checks the proximity of each joint to its joint position limits and prints a warning whenever a joint is close to a limit. More... | |
virtual void | control (const std::function< bool(const ros::Time &, const ros::Duration &)> &ros_callback) const |
Runs the currently active controller in a realtime loop. More... | |
virtual bool | controllerActive () const noexcept |
Indicates whether there is an active controller. More... | |
virtual void | doSwitch (const std::list< hardware_interface::ControllerInfo > &, const std::list< hardware_interface::ControllerInfo > &) override |
Performs controller switching (real-time capable). More... | |
virtual void | enforceLimits (const ros::Duration &period) |
Enforces limits on position, velocity, and torque level. More... | |
FrankaHW () | |
Default constructor. More... | |
virtual std::array< double, 7 > | getJointEffortCommand () const noexcept |
Gets the current joint torque command. More... | |
virtual std::array< double, 7 > | getJointPositionCommand () const noexcept |
Gets the current joint position command. More... | |
virtual std::array< double, 7 > | getJointVelocityCommand () const noexcept |
Gets the current joint velocity command. More... | |
virtual bool | init (ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh) override |
Initializes the FrankaHW class to be fully operational. More... | |
virtual bool | initParameters (ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh) |
Reads the parameterization of the hardware class from the ROS parameter server (e.g. More... | |
virtual void | initROSInterfaces (ros::NodeHandle &robot_hw_nh) |
Initializes the class in terms of ros_control interfaces. More... | |
virtual bool | prepareSwitch (const std::list< hardware_interface::ControllerInfo > &start_list, const std::list< hardware_interface::ControllerInfo > &stop_list) override |
Prepares switching between controllers (not real-time capable). More... | |
virtual void | read (const ros::Time &time, const ros::Duration &period) override |
Reads data from the franka robot. More... | |
virtual void | reset () |
Resets the limit interfaces. More... | |
virtual franka::Robot & | robot () const |
Getter for the libfranka robot instance. More... | |
virtual void | setupParameterCallbacks (ros::NodeHandle &robot_hw_nh) |
Initializes the callbacks for on-the-fly reading the parameters for rate limiting, internal controllers and cutoff frequency. More... | |
virtual void | update (const franka::RobotState &robot_state) |
Updates the controller interfaces from the given robot state. More... | |
virtual void | write (const ros::Time &time, const ros::Duration &period) override |
Writes data to the franka robot. More... | |
virtual | ~FrankaHW () override=default |
Public Member Functions inherited from hardware_interface::RobotHW | |
RobotHW () | |
virtual | ~RobotHW () |
Public Member Functions inherited from hardware_interface::InterfaceManager | |
T * | get () |
std::vector< std::string > | getInterfaceResources (std::string iface_type) const |
std::vector< std::string > | getNames () const |
void | registerInterface (T *iface) |
void | registerInterfaceManager (InterfaceManager *iface_man) |
Static Public Member Functions | |
static bool | commandHasNaN (const franka::Torques &command) |
Checks a command for NaN values. More... | |
static bool | commandHasNaN (const franka::JointPositions &command) |
Checks a command for NaN values. More... | |
static bool | commandHasNaN (const franka::JointVelocities &command) |
Checks a command for NaN values. More... | |
static bool | commandHasNaN (const franka::CartesianPose &command) |
Checks a command for NaN values. More... | |
static bool | commandHasNaN (const franka::CartesianVelocities &command) |
Checks a command for NaN values. More... | |
Protected Types | |
using | Callback = std::function< bool(const franka::RobotState &, franka::Duration)> |
Protected Types inherited from hardware_interface::InterfaceManager | |
typedef std::vector< InterfaceManager * > | InterfaceManagerVector |
typedef std::map< std::string, void * > | InterfaceMap |
typedef std::map< std::string, std::vector< std::string > > | ResourceMap |
typedef std::map< std::string, size_t > | SizeMap |
Protected Member Functions | |
template<typename T > | |
T | controlCallback (const T &command, Callback ros_callback, const franka::RobotState &robot_state, franka::Duration time_step) |
Callback for the libfranka control loop. More... | |
virtual void | initRobot () |
Uses the robot_ip_ to connect to the robot via libfranka and loads the libfranka model. More... | |
virtual bool | setRunFunction (const ControlMode &requested_control_mode, bool limit_rate, double cutoff_frequency, franka::ControllerMode internal_controller) |
Configures the run function which is used as libfranka control callback based on the requested control mode. More... | |
virtual void | setupFrankaCartesianPoseInterface (franka::CartesianPose &pose_cartesian_command) |
Configures and registers the command interface for Cartesian poses in ros_control. More... | |
virtual void | setupFrankaCartesianVelocityInterface (franka::CartesianVelocities &velocity_cartesian_command) |
Configures and registers the command interface for Cartesian velocities in ros_control. More... | |
virtual void | setupFrankaModelInterface (franka::RobotState &robot_state) |
Configures and registers the model interface offering kinematics and dynamics in ros_control. More... | |
virtual void | setupFrankaStateInterface (franka::RobotState &robot_state) |
Configures and registers the state interface offering the full franka::RobotState in ros_control. More... | |
template<typename T > | |
void | setupJointCommandInterface (std::array< double, 7 > &command, franka::RobotState &state, bool use_q_d, T &interface) |
Configures and registers a joint command interface for positions velocities or efforts in ros_control. More... | |
virtual void | setupJointStateInterface (franka::RobotState &robot_state) |
Configures and registers the joint state interface in ros_control. More... | |
template<typename T > | |
void | setupLimitInterface (joint_limits_interface::JointLimitsInterface< T > &limit_interface, hardware_interface::JointCommandInterface &command_interface) |
Configures a limit interface to enforce limits on effort, velocity or position level on joint commands. More... | |
Static Protected Member Functions | |
template<size_t size> | |
static bool | arrayHasNaN (const std::array< double, size > &array) |
Checks whether an array of doubles contains NaN values. More... | |
static std::vector< double > | getCollisionThresholds (const std::string &name, ros::NodeHandle &robot_hw_nh, const std::vector< double > &defaults) |
Parses a set of collision thresholds from the parameter server. More... | |
This class wraps the functionality of libfranka for controlling Panda robots into the ros_control framework.
Definition at line 38 of file franka_hw.h.
|
protected |
Definition at line 269 of file franka_hw.h.
franka_hw::FrankaHW::FrankaHW | ( | ) |
Default constructor.
Note: Be sure to call the init() method before operation.
Definition at line 45 of file franka_hw.cpp.
|
overridevirtualdefault |
|
inlinestaticprotected |
Checks whether an array of doubles contains NaN values.
[in] | command | array The array to check. |
Definition at line 265 of file franka_hw.h.
|
overridevirtual |
Checks whether a requested controller can be run, based on the resources and interfaces it claims.
[in] | info | Controllers to be running at the same time. |
Reimplemented from hardware_interface::RobotHW.
Reimplemented in franka_hw::FrankaCombinableHW.
Definition at line 219 of file franka_hw.cpp.
|
virtual |
Checks the proximity of each joint to its joint position limits and prints a warning whenever a joint is close to a limit.
Definition at line 300 of file franka_hw.cpp.
|
static |
Checks a command for NaN values.
[in] | command | The command to check. |
Definition at line 553 of file franka_hw.cpp.
|
static |
Checks a command for NaN values.
[in] | command | The command to check. |
Definition at line 557 of file franka_hw.cpp.
|
static |
Checks a command for NaN values.
[in] | command | The command to check. |
Definition at line 561 of file franka_hw.cpp.
|
static |
Checks a command for NaN values.
[in] | command | The command to check. |
Definition at line 565 of file franka_hw.cpp.
|
static |
Checks a command for NaN values.
[in] | command | The command to check. |
Definition at line 569 of file franka_hw.cpp.
|
virtual |
Runs the currently active controller in a realtime loop.
If no controller is active, the function immediately exits. When running a controller, the function only exits when ros_callback returns false.
[in] | ros_callback | A callback function that is executed at each time step and runs all ROS-side functionality of the hardware. Execution is stopped if it returns false. |
franka::ControlException | if an error related to torque control or motion generation occurred. |
franka::InvalidOperationException | if a conflicting operation is already running. |
franka::NetworkException | if the connection is lost, e.g. after a timeout. |
franka::RealtimeException | if realtime priority cannot be set for the current thread. |
Reimplemented in franka_hw::FrankaCombinableHW.
Definition at line 188 of file franka_hw.cpp.
|
inlineprotected |
Callback for the libfranka control loop.
This method is designed to incorporate a second callback named ros_callback that will be called on each iteration of the callback.
[in] | command | The datafield containing the command to send to the robot. |
[in] | ros_callback | An additional callback that is executed every time step. |
[in] | robot_state | The current robot state to compute commands with. |
[in] | time_step | Time since last call to the callback. |
std::invalid_argument | When a command contains NaN values. |
Definition at line 284 of file franka_hw.h.
|
virtualnoexcept |
Indicates whether there is an active controller.
Definition at line 184 of file franka_hw.cpp.
|
overridevirtual |
Performs controller switching (real-time capable).
Reimplemented from hardware_interface::RobotHW.
Definition at line 234 of file franka_hw.cpp.
|
virtual |
Enforces limits on position, velocity, and torque level.
[in] | period | The duration of the current cycle. |
Definition at line 211 of file franka_hw.cpp.
|
staticprotected |
Parses a set of collision thresholds from the parameter server.
The methods returns the default values if no parameter was found or the size of the array did not match the defaults dimension.
[in] | name | The name of the parameter to look for. |
[in] | robot_hw_nh | A node handle in the namespace of the robot hardware. |
[in] | defaults | A set of default values that also specify the size the parameter must have to be valid. |
Definition at line 573 of file franka_hw.cpp.
|
virtualnoexcept |
Gets the current joint torque command.
Definition at line 292 of file franka_hw.cpp.
|
virtualnoexcept |
Gets the current joint position command.
Definition at line 284 of file franka_hw.cpp.
|
virtualnoexcept |
Gets the current joint velocity command.
Definition at line 288 of file franka_hw.cpp.
|
overridevirtual |
Initializes the FrankaHW class to be fully operational.
This involves parsing required configurations from the ROS parameter server, connecting to the robot and setting up interfaces for the ros_control framework.
[in] | root_nh | A node handle in the root namespace of the control node. |
[in] | robot_hw_nh | A node handle in the namespace of the robot hardware. |
Reimplemented from hardware_interface::RobotHW.
Definition at line 59 of file franka_hw.cpp.
|
virtual |
Reads the parameterization of the hardware class from the ROS parameter server (e.g.
arm_id, robot_ip joint_names etc.)
[in] | root_nh | A node handle in the root namespace of the control node. |
[in] | robot_hw_nh | A node handle in the namespace of the robot hardware. |
Definition at line 82 of file franka_hw.cpp.
|
protectedvirtual |
Uses the robot_ip_ to connect to the robot via libfranka and loads the libfranka model.
Reimplemented in franka_hw::FrankaCombinableHW.
Definition at line 510 of file franka_hw.cpp.
|
virtual |
Initializes the class in terms of ros_control interfaces.
Note: You have to call initParameters beforehand. Use the complete initialization routine init() method to control robots.
[in] | robot_hw_nh | A node handle in the namespace of the robot hardware. |
Reimplemented in franka_hw::FrankaCombinableHW.
Definition at line 490 of file franka_hw.cpp.
|
overridevirtual |
Prepares switching between controllers (not real-time capable).
[in] | start_list | Controllers requested to be started. |
[in] | stop_list | Controllers requested to be stopped. |
Reimplemented from hardware_interface::RobotHW.
Definition at line 243 of file franka_hw.cpp.
|
overridevirtual |
Reads data from the franka robot.
[in] | time | The current time. |
[in] | period | The time passed since the last call to read. |
Reimplemented from hardware_interface::RobotHW.
Reimplemented in franka_hw::FrankaCombinableHW.
Definition at line 343 of file franka_hw.cpp.
|
virtual |
Resets the limit interfaces.
Definition at line 296 of file franka_hw.cpp.
|
virtual |
Getter for the libfranka robot instance.
Definition at line 334 of file franka_hw.cpp.
|
protectedvirtual |
Configures the run function which is used as libfranka control callback based on the requested control mode.
[in] | requested_control_mode | The control mode to configure (e.g. torque/position/velocity etc.) |
[in] | limit_rate | Flag to enable/disable rate limiting to smoothen the commands. |
[in] | cutoff_frequency | The cutoff frequency applied for command smoothing. |
[in] | internal_controller | The internal controller to use when using position or velocity modes. |
Reimplemented in franka_hw::FrankaCombinableHW.
Definition at line 399 of file franka_hw.cpp.
|
protectedvirtual |
Configures and registers the command interface for Cartesian poses in ros_control.
[in] | pose_cartesian_command | The data field holding the command to execute. |
Definition at line 374 of file franka_hw.cpp.
|
protectedvirtual |
Configures and registers the command interface for Cartesian velocities in ros_control.
[in] | velocity_cartesian_command | The data field holding the command to execute. |
Definition at line 382 of file franka_hw.cpp.
|
protectedvirtual |
Configures and registers the model interface offering kinematics and dynamics in ros_control.
[in] | robot_state | A reference to the data field storing the current robot state. This state is used to evaluate model qunatities (by default) at the current state. |
Definition at line 391 of file franka_hw.cpp.
|
protectedvirtual |
Configures and registers the state interface offering the full franka::RobotState in ros_control.
[in] | robot_state | The data field holding the updated robot state. |
Definition at line 368 of file franka_hw.cpp.
|
inlineprotected |
Configures and registers a joint command interface for positions velocities or efforts in ros_control.
[in] | command | The data field holding the command to execute. |
[in] | state | The data field holding the updated robot state. |
[in] | use_q_d | Flag to configure using desired values as joint_states. |
[out] | interface | The command interface to configure. |
Definition at line 366 of file franka_hw.h.
|
protectedvirtual |
Configures and registers the joint state interface in ros_control.
[in] | robot_state | The data field holding the updated robot state. |
Definition at line 359 of file franka_hw.cpp.
|
inlineprotected |
Configures a limit interface to enforce limits on effort, velocity or position level on joint commands.
[out] | limit_interface | The limit interface to set up. |
[out] | command_interface | The command interface to hook the limit interface to. |
Definition at line 314 of file franka_hw.h.
|
virtual |
Initializes the callbacks for on-the-fly reading the parameters for rate limiting, internal controllers and cutoff frequency.
[in] | robot_hw_nh | A node handle in the namespace of the robot hardware. |
Definition at line 524 of file franka_hw.cpp.
|
virtual |
Updates the controller interfaces from the given robot state.
[in] | robot_state | Current robot state. |
Definition at line 179 of file franka_hw.cpp.
|
overridevirtual |
Writes data to the franka robot.
[in] | time | The current time. |
[in] | period | The time passed since the last call to write. |
Reimplemented from hardware_interface::RobotHW.
Reimplemented in franka_hw::FrankaCombinableHW.
Definition at line 349 of file franka_hw.cpp.
|
protected |
Definition at line 500 of file franka_hw.h.
|
protected |
Definition at line 464 of file franka_hw.h.
|
protected |
Definition at line 507 of file franka_hw.h.
|
protected |
Definition at line 508 of file franka_hw.h.
|
protected |
Definition at line 485 of file franka_hw.h.
|
protected |
Definition at line 492 of file franka_hw.h.
|
protected |
Definition at line 469 of file franka_hw.h.
|
protected |
Definition at line 475 of file franka_hw.h.
|
protected |
Definition at line 472 of file franka_hw.h.
|
protected |
Definition at line 470 of file franka_hw.h.
|
protected |
Definition at line 466 of file franka_hw.h.
|
protected |
Definition at line 471 of file franka_hw.h.
|
protected |
Definition at line 512 of file franka_hw.h.
|
protected |
Definition at line 510 of file franka_hw.h.
|
protected |
Definition at line 511 of file franka_hw.h.
|
protected |
Definition at line 506 of file franka_hw.h.
|
protected |
Definition at line 503 of file franka_hw.h.
|
protected |
Definition at line 499 of file franka_hw.h.
|
protected |
Definition at line 465 of file franka_hw.h.
|
protected |
Definition at line 482 of file franka_hw.h.
|
protected |
Definition at line 477 of file franka_hw.h.
|
protected |
Definition at line 497 of file franka_hw.h.
|
protected |
Definition at line 486 of file franka_hw.h.
|
protected |
Definition at line 493 of file franka_hw.h.
|
protected |
Definition at line 483 of file franka_hw.h.
|
protected |
Definition at line 490 of file franka_hw.h.
|
protected |
Definition at line 467 of file franka_hw.h.
|
protected |
Definition at line 473 of file franka_hw.h.
|
protected |
Definition at line 504 of file franka_hw.h.
|
protected |
Definition at line 496 of file franka_hw.h.
|
protected |
Definition at line 501 of file franka_hw.h.
|
protected |
Definition at line 479 of file franka_hw.h.
|
protected |
Definition at line 480 of file franka_hw.h.
|
protected |
Definition at line 489 of file franka_hw.h.
|
protected |
Definition at line 478 of file franka_hw.h.
|
protected |
Definition at line 513 of file franka_hw.h.
|
protected |
Definition at line 502 of file franka_hw.h.
|
protected |
Definition at line 487 of file franka_hw.h.
|
protected |
Definition at line 494 of file franka_hw.h.
|
protected |
Definition at line 484 of file franka_hw.h.
|
protected |
Definition at line 491 of file franka_hw.h.
|
protected |
Definition at line 468 of file franka_hw.h.
|
protected |
Definition at line 474 of file franka_hw.h.