A hardware class for a Panda robot based on the ros_control framework. More...

#include <franka_combinable_hw.h>

Inheritance diagram for franka_hw::FrankaCombinableHW:

[legend]

Public Member Functions
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...

void	connect () override
	Create a libfranka robot, connecting the hardware class to the master controller. More...

void	control (const std::function< bool(const ros::Time &, const ros::Duration &)> &ros_callback=[](const ros::Time &, const ros::Duration &) { return true;}) override
	Runs the currently active controller in a realtime loop. More...

bool	controllerNeedsReset () const noexcept
	Returns whether the controller needs to be reset e.g. More...

bool	disconnect () override
	Tries to disconnect the hardware class from the robot, freeing it for e.g. More...

	FrankaCombinableHW ()
	Creates an instance of FrankaCombinableHW. More...

std::string	getArmID () const noexcept
	Getter method for the arm_id which is used to distinguish between multiple instances of FrankaCombinableHW. More...

bool	hasError () const noexcept
	Getter for the error flag of the class. More...

bool	init (ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh) override
	Initializes the FrankaHW class to be fully operational. More...

void	initROSInterfaces (ros::NodeHandle &robot_hw_nh) override
	Initializes the class in terms of ros_control interfaces. More...

void	read (const ros::Time &, const ros::Duration &) override
	Reads data from the franka robot. More...

void	resetError ()
	Recovers the libfranka robot, resets the error flag and publishes the error state. More...

void	triggerError ()
	Triggers a stop of the controlLoop. More...

void	write (const ros::Time &, const ros::Duration &period) override
	Writes data to the franka robot. More...

Public Member Functions inherited from franka_hw::FrankaHW
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 bool	connected ()
	Checks whether the hardware class is connected to a robot. 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	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 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	reset ()
	Resets the limit interfaces. More...

virtual franka::Robot &	robot () const
	Getter for the libfranka robot instance. More...

virtual std::mutex &	robotMutex ()
	Getter for the mutex protecting access to the libfranka::robot class. 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	~FrankaHW () override=default

Public Member Functions inherited from hardware_interface::RobotHW
virtual SwitchState	switchResult () const

virtual SwitchState	switchResult (const ControllerInfo &) const

virtual	~RobotHW ()=default

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)

Private Member Functions
void	controlLoop ()

void	initRobot () override
	Uses the robot_ip_ to connect to the robot via libfranka and loads the libfranka model. More...

template<typename T >
T	libfrankaUpdateCallback (const T &command, const franka::RobotState &robot_state, franka::Duration time_step)

void	publishErrorState (bool error)

bool	setRunFunction (const ControlMode &requested_control_mode, bool limit_rate, double cutoff_frequency, franka::ControllerMode internal_controller) override
	Configures the run function which is used as libfranka control callback based on the requested control mode. More...

void	setupServicesAndActionServers (ros::NodeHandle &node_handle)

Private Attributes
std::unique_ptr< std::thread >	control_loop_thread_

std::atomic_bool	controller_needs_reset_ {false}

std::atomic_bool	error_recovered_ {false}

std::atomic_bool	has_error_ {false}

ros::Publisher	has_error_pub_

std::unique_ptr< actionlib::SimpleActionServer< franka_msgs::ErrorRecoveryAction > >	recovery_action_server_

ros::NodeHandle	robot_hw_nh_

std::unique_ptr< ServiceContainer >	services_

Additional Inherited Members
Public Types inherited from hardware_interface::RobotHW
enum	SwitchState { SwitchState::DONE, SwitchState::ONGOING, SwitchState::ERROR }

Static Public Member Functions inherited from franka_hw::FrankaHW
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...

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::Torques &command)
	Checks a command for NaN values. More...

static std::vector< double >	getCollisionThresholds (const std::string &name, const ros::NodeHandle &robot_hw_nh, const std::vector< double > &defaults)
	Parses a set of collision thresholds from the parameter server. More...

Protected Types inherited from franka_hw::FrankaHW
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 inherited from franka_hw::FrankaHW
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	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 inherited from franka_hw::FrankaHW
template<size_t size>
static bool	arrayHasNaN (const std::array< double, size > &array)
	Checks whether an array of doubles contains NaN values. More...

Protected Attributes inherited from franka_hw::FrankaHW
std::string	arm_id_

CollisionConfig	collision_config_

std::atomic_bool	controller_active_ {false}

ControlMode	current_control_mode_ = ControlMode::None

franka::Torques	effort_joint_command_libfranka_

franka::Torques	effort_joint_command_ros_

hardware_interface::EffortJointInterface	effort_joint_interface_ {}

joint_limits_interface::EffortJointSoftLimitsInterface	effort_joint_limit_interface_ {}

FrankaModelInterface	franka_model_interface_ {}

FrankaPoseCartesianInterface	franka_pose_cartesian_interface_ {}

FrankaStateInterface	franka_state_interface_ {}

FrankaVelocityCartesianInterface	franka_velocity_cartesian_interface_ {}

std::function< double()>	get_cutoff_frequency_

std::function< franka::ControllerMode()>	get_internal_controller_

std::function< bool()>	get_limit_rate_

bool	initialized_ {false}

double	joint_limit_warning_threshold_ {0.1}

std::array< std::string, 7 >	joint_names_

hardware_interface::JointStateInterface	joint_state_interface_ {}

std::mutex	libfranka_cmd_mutex_

std::mutex	libfranka_state_mutex_

std::unique_ptr< franka_hw::ModelBase >	model_

franka::CartesianPose	pose_cartesian_command_libfranka_

franka::CartesianPose	pose_cartesian_command_ros_

franka::JointPositions	position_joint_command_libfranka_

franka::JointPositions	position_joint_command_ros_

hardware_interface::PositionJointInterface	position_joint_interface_ {}

joint_limits_interface::PositionJointSoftLimitsInterface	position_joint_limit_interface_ {}

franka::RealtimeConfig	realtime_config_

std::unique_ptr< franka::Robot >	robot_

std::string	robot_ip_

std::mutex	robot_mutex_

franka::RobotState	robot_state_libfranka_ {}

franka::RobotState	robot_state_ros_ {}

std::mutex	ros_cmd_mutex_

std::mutex	ros_state_mutex_

std::function< void(franka::Robot &, Callback)>	run_function_

urdf::Model	urdf_model_

franka::CartesianVelocities	velocity_cartesian_command_libfranka_

franka::CartesianVelocities	velocity_cartesian_command_ros_

franka::JointVelocities	velocity_joint_command_libfranka_

franka::JointVelocities	velocity_joint_command_ros_

hardware_interface::VelocityJointInterface	velocity_joint_interface_ {}

joint_limits_interface::VelocityJointSoftLimitsInterface	velocity_joint_limit_interface_ {}

Protected Attributes inherited from hardware_interface::InterfaceManager
std::vector< ResourceManagerBase * >	interface_destruction_list_

InterfaceManagerVector	interface_managers_

InterfaceMap	interfaces_

InterfaceMap	interfaces_combo_

SizeMap	num_ifaces_registered_

ResourceMap	resources_

Detailed Description

A hardware class for a Panda robot based on the ros_control framework.

This class is ready to be combined with other hardware classes e.g. to control multiple robots from a single controller. Note: This class allows for torque (effort) control only due to the lack of synchronization between master controllers of different robots. For more information see the documentation at https://frankaemika.github.io/docs/franka_ros.html .

Definition at line 30 of file franka_combinable_hw.h.

Constructor & Destructor Documentation

◆ FrankaCombinableHW()

franka_hw::FrankaCombinableHW::FrankaCombinableHW ( )

Creates an instance of FrankaCombinableHW.

Definition at line 19 of file franka_combinable_hw.cpp.

Member Function Documentation

◆ checkForConflict()

bool franka_hw::FrankaCombinableHW::checkForConflict ( const std::list< hardware_interface::ControllerInfo > & info ) const

overridevirtual

Checks whether a requested controller can be run, based on the resources and interfaces it claims.

Note: FrankaCombinableHW allows torque control only.

Parameters

[in] info Controllers to be running at the same time.

Returns: True in case of a conflict, false in case of valid controllers.

Reimplemented from franka_hw::FrankaHW.

Definition at line 173 of file franka_combinable_hw.cpp.

◆ connect()

void franka_hw::FrankaCombinableHW::connect ( )

overridevirtual

Create a libfranka robot, connecting the hardware class to the master controller.

Note: While the robot is connected, no DESK based tasks can be executed.

Reimplemented from franka_hw::FrankaHW.

Definition at line 149 of file franka_combinable_hw.cpp.

◆ control()

void franka_hw::FrankaCombinableHW::control ( const std::function< bool(const ros::Time &, const ros::Duration &)> & ros_callback = [](const ros::Time&, const ros::Duration&) { return true; } )

overridevirtual

Runs the currently active controller in a realtime loop.

If no controller is active, the function immediately exits.

Parameters

[in] ros_callback A callback function that is executed at each time step.

Exceptions

franka::ControlException	if an error related to torque control 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 from franka_hw::FrankaHW.

Definition at line 164 of file franka_combinable_hw.cpp.

◆ controllerNeedsReset()

bool franka_hw::FrankaCombinableHW::controllerNeedsReset ( ) const

noexcept

Returns whether the controller needs to be reset e.g.

after error recovery.

Returns: A copy of the controller_needs_reset flag.

Definition at line 242 of file franka_combinable_hw.cpp.

◆ controlLoop()

void franka_hw::FrankaCombinableHW::controlLoop ( )

private

Definition at line 52 of file franka_combinable_hw.cpp.

◆ disconnect()

bool franka_hw::FrankaCombinableHW::disconnect ( )

overridevirtual

Tries to disconnect the hardware class from the robot, freeing it for e.g.

DESK-based tasks. Note: Disconnecting is only possible when no controller is actively running.

Returns: true if successfully disconnected, false otherwise.

Reimplemented from franka_hw::FrankaHW.

Definition at line 154 of file franka_combinable_hw.cpp.

◆ getArmID()

std::string franka_hw::FrankaCombinableHW::getArmID ( ) const

noexcept

Getter method for the arm_id which is used to distinguish between multiple instances of FrankaCombinableHW.

Returns: A copy of the arm_id string identifying the class instance.

Definition at line 217 of file franka_combinable_hw.cpp.

◆ hasError()

bool franka_hw::FrankaCombinableHW::hasError ( ) const

noexcept

Getter for the error flag of the class.

Returns: True in case of an error false otherwise.

Definition at line 226 of file franka_combinable_hw.cpp.

◆ init()

bool franka_hw::FrankaCombinableHW::init	(	ros::NodeHandle &	root_nh,
		ros::NodeHandle &	robot_hw_nh
	)

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.

Parameters

[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.

Returns: True if successful, false otherwise.

Reimplemented from franka_hw::FrankaHW.

Definition at line 21 of file franka_combinable_hw.cpp.

◆ initRobot()

void franka_hw::FrankaCombinableHW::initRobot ( )

overrideprivatevirtual

Uses the robot_ip_ to connect to the robot via libfranka and loads the libfranka model.

Reimplemented from franka_hw::FrankaHW.

Definition at line 41 of file franka_combinable_hw.cpp.

◆ initROSInterfaces()

void franka_hw::FrankaCombinableHW::initROSInterfaces ( ros::NodeHandle & robot_hw_nh )

overridevirtual

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.

Parameters

[in] robot_hw_nh A node handle in the namespace of the robot hardware.

Returns: True if successful, false otherwise.

Reimplemented from franka_hw::FrankaHW.

Definition at line 26 of file franka_combinable_hw.cpp.

◆ libfrankaUpdateCallback()

template<typename T >

T franka_hw::FrankaCombinableHW::libfrankaUpdateCallback	(	const T &	command,
		const franka::RobotState &	robot_state,
		franka::Duration	time_step
	)

inlineprivate

Definition at line 148 of file franka_combinable_hw.h.

◆ publishErrorState()

void franka_hw::FrankaCombinableHW::publishErrorState ( bool error )

private

Definition at line 46 of file franka_combinable_hw.cpp.

◆ read()

void franka_hw::FrankaCombinableHW::read	(	const ros::Time &	time,
		const ros::Duration &	period
	)

overridevirtual

Reads data from the franka robot.

Parameters

[in]	time	The current time. Not used in this class.
[in]	period	The time passed since the last call to read. Not used in this class.

Reimplemented from franka_hw::FrankaHW.

Definition at line 198 of file franka_combinable_hw.cpp.

◆ resetError()

void franka_hw::FrankaCombinableHW::resetError ( )

Recovers the libfranka robot, resets the error flag and publishes the error state.

Definition at line 230 of file franka_combinable_hw.cpp.

◆ setRunFunction()

bool franka_hw::FrankaCombinableHW::setRunFunction	(	const ControlMode &	requested_control_mode,
		bool	limit_rate,
		double	cutoff_frequency,
		franka::ControllerMode	internal_controller
	)

overrideprivatevirtual

Configures the run function which is used as libfranka control callback based on the requested control mode.

Parameters

[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.

Returns: True if successful, false otherwise.

Reimplemented from franka_hw::FrankaHW.

Definition at line 246 of file franka_combinable_hw.cpp.

◆ setupServicesAndActionServers()

void franka_hw::FrankaCombinableHW::setupServicesAndActionServers ( ros::NodeHandle & node_handle )

private

Definition at line 108 of file franka_combinable_hw.cpp.

◆ triggerError()

void franka_hw::FrankaCombinableHW::triggerError ( )

Triggers a stop of the controlLoop.

This interface is used to stop all combined robots together when at one robot encounters an error.

Definition at line 221 of file franka_combinable_hw.cpp.

◆ write()

void franka_hw::FrankaCombinableHW::write	(	const ros::Time &	time,
		const ros::Duration &	period
	)

overridevirtual

Writes data to the franka robot.

Parameters

[in]	time	The current time. Not used in this class.
[in]	period	The time passed since the last call to write.

Reimplemented from franka_hw::FrankaHW.

Definition at line 203 of file franka_combinable_hw.cpp.

Member Data Documentation

◆ control_loop_thread_

std::unique_ptr<std::thread> franka_hw::FrankaCombinableHW::control_loop_thread_

private

Definition at line 183 of file franka_combinable_hw.h.

◆ controller_needs_reset_

std::atomic_bool franka_hw::FrankaCombinableHW::controller_needs_reset_ {false}

private

Definition at line 190 of file franka_combinable_hw.h.

◆ error_recovered_

std::atomic_bool franka_hw::FrankaCombinableHW::error_recovered_ {false}

private

Definition at line 189 of file franka_combinable_hw.h.

◆ has_error_

std::atomic_bool franka_hw::FrankaCombinableHW::has_error_ {false}

private

Definition at line 187 of file franka_combinable_hw.h.

◆ has_error_pub_

ros::Publisher franka_hw::FrankaCombinableHW::has_error_pub_

private

Definition at line 188 of file franka_combinable_hw.h.

◆ recovery_action_server_

std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction> > franka_hw::FrankaCombinableHW::recovery_action_server_

private

Definition at line 186 of file franka_combinable_hw.h.

◆ robot_hw_nh_

ros::NodeHandle franka_hw::FrankaCombinableHW::robot_hw_nh_

private

Definition at line 191 of file franka_combinable_hw.h.

◆ services_

std::unique_ptr<ServiceContainer> franka_hw::FrankaCombinableHW::services_

private

Definition at line 184 of file franka_combinable_hw.h.

The documentation for this class was generated from the following files:

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ FrankaCombinableHW()

Member Function Documentation

◆ checkForConflict()

◆ connect()

◆ control()

◆ controllerNeedsReset()

◆ controlLoop()

◆ disconnect()

◆ getArmID()

◆ hasError()

◆ init()

◆ initRobot()

◆ initROSInterfaces()

◆ libfrankaUpdateCallback()

◆ publishErrorState()

◆ read()

◆ resetError()

◆ setRunFunction()

◆ setupServicesAndActionServers()

◆ triggerError()

◆ write()

Member Data Documentation

◆ control_loop_thread_

◆ controller_needs_reset_

◆ error_recovered_

◆ has_error_

◆ has_error_pub_

◆ recovery_action_server_

◆ robot_hw_nh_

◆ services_