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:

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	control (const std::function< bool(const ros::Time &, const ros::Duration &)> &ros_callback=[](const ros::Time &, const ros::Duration &){return true;}) const 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...

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

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	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 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 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
	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)

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_

ServiceContainer	services_

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

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

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::Model >	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_

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
boost::ptr_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

franka_hw::FrankaCombinableHW::FrankaCombinableHW ( )

Creates an instance of FrankaCombinableHW.

Definition at line 14 of file franka_combinable_hw.cpp.

Member Function Documentation

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 121 of file franka_combinable_hw.cpp.

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; } ) const

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 112 of file franka_combinable_hw.cpp.

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 188 of file franka_combinable_hw.cpp.

void franka_hw::FrankaCombinableHW::controlLoop ( )

private

Definition at line 42 of file franka_combinable_hw.cpp.

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 165 of file franka_combinable_hw.cpp.

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 174 of file franka_combinable_hw.cpp.

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 31 of file franka_combinable_hw.cpp.

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 16 of file franka_combinable_hw.cpp.

template<typename T >

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

inlineprivate

Definition at line 125 of file franka_combinable_hw.h.

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

private

Definition at line 36 of file franka_combinable_hw.cpp.

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 146 of file franka_combinable_hw.cpp.

void franka_hw::FrankaCombinableHW::resetError ( )

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

Definition at line 178 of file franka_combinable_hw.cpp.

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 192 of file franka_combinable_hw.cpp.

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

private

Definition at line 89 of file franka_combinable_hw.cpp.

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 169 of file franka_combinable_hw.cpp.

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 151 of file franka_combinable_hw.cpp.

Member Data Documentation

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

private

Definition at line 160 of file franka_combinable_hw.h.

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

private

Definition at line 167 of file franka_combinable_hw.h.

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

private

Definition at line 166 of file franka_combinable_hw.h.

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

private

Definition at line 164 of file franka_combinable_hw.h.

ros::Publisher franka_hw::FrankaCombinableHW::has_error_pub_

private

Definition at line 165 of file franka_combinable_hw.h.

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

private

Definition at line 163 of file franka_combinable_hw.h.

ServiceContainer franka_hw::FrankaCombinableHW::services_

private

Definition at line 161 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

Member Function Documentation

Member Data Documentation