A custom implementation of a gazebo_ros_control plugin, which is able to simulate franka interfaces in Gazebo. More...

#include <franka_hw_sim.h>

Inheritance diagram for franka_gazebo::FrankaHWSim:

[legend]

Public Member Functions
void	doSwitch (const std::list< hardware_interface::ControllerInfo > &start_list, const std::list< hardware_interface::ControllerInfo > &stop_list) override
	Switches the control mode of the robot arm. More...

void	eStopActive (const bool active) override
	Set the emergency stop state (not yet implemented) More...

	FrankaHWSim ()
	Create a new FrankaHWSim instance. More...

bool	initSim (const std::string &robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent, const urdf::Model *const urdf, std::vector< transmission_interface::TransmissionInfo > transmissions) override
	Initialize the simulated robot hardware and parse all supported transmissions. More...

bool	prepareSwitch (const std::list< hardware_interface::ControllerInfo > &start_list, const std::list< hardware_interface::ControllerInfo > &) override
	Check (in non-realtime) if given controllers could be started and stopped from the current state of the RobotHW with regard to necessary hardware interface switches and prepare the switching. More...

void	readSim (ros::Time time, ros::Duration period) override
	Fetch data from the Gazebo simulation and pass it on to the hardware interfaces. More...

void	writeSim (ros::Time time, ros::Duration period) override
	Pass the data send from controllers via the hardware interfaces onto the simulation. More...

Public Member Functions inherited from gazebo_ros_control::RobotHWSim
virtual	~RobotHWSim ()

Public Member Functions inherited from hardware_interface::RobotHW
virtual bool	checkForConflict (const std::list< ControllerInfo > &info) const

virtual bool	checkForConflict (const std::list< ControllerInfo > &info) const

virtual bool	init (ros::NodeHandle &, ros::NodeHandle &)

virtual void	read (const ros::Time &, const ros::Duration &)

virtual void	read (const ros::Time &, const ros::Duration &)

virtual SwitchState	switchResult () const

virtual SwitchState	switchResult (const ControllerInfo &) const

virtual void	write (const ros::Time &, const ros::Duration &)

virtual void	write (const ros::Time &, const ros::Duration &)

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	forControlledJoint (const std::list< hardware_interface::ControllerInfo > &controllers, const std::function< void(franka_gazebo::Joint &joint, const ControlMethod &)> &f)

void	guessEndEffector (const ros::NodeHandle &nh, const urdf::Model &urdf)

void	initEffortCommandHandle (const std::shared_ptr< franka_gazebo::Joint > &joint)

void	initFrankaModelHandle (const std::string &robot, const urdf::Model &urdf, const transmission_interface::TransmissionInfo &transmission, double singularity_threshold)

void	initFrankaStateHandle (const std::string &robot, const urdf::Model &urdf, const transmission_interface::TransmissionInfo &transmission)

void	initJointStateHandle (const std::shared_ptr< franka_gazebo::Joint > &joint)

void	initPositionCommandHandle (const std::shared_ptr< franka_gazebo::Joint > &joint)

void	initServices (ros::NodeHandle &nh)

void	initVelocityCommandHandle (const std::shared_ptr< franka_gazebo::Joint > &joint)

template<int N>
std::array< double, N >	readArray (std::string param, std::string name="")

bool	readParameters (const ros::NodeHandle &nh, const urdf::Model &urdf)

void	restartControllers ()

void	updateRobotState (ros::Time time)

void	updateRobotStateDynamics ()

Static Private Member Functions
static double	positionControl (Joint &joint, double setpoint, const ros::Duration &period)

static Eigen::Matrix3d	shiftInertiaTensor (Eigen::Matrix3d I, double m, Eigen::Vector3d p)
	Shift the moment of inertia tensor by a given offset. More...

static Eigen::Matrix3d	skewMatrix (const Eigen::Vector3d &vec)
	Helper function for generating a skew symmetric matrix for a given input vector such that: $\mathbf{0} = \mathbf{M} \cdot \mathrm{vec}$ . More...

static double	velocityControl (Joint &joint, double setpoint, const ros::Duration &period)

Private Attributes
std::unique_ptr< actionlib::SimpleActionServer< franka_msgs::ErrorRecoveryAction > >	action_recovery_

std::string	arm_id_

hardware_interface::EffortJointInterface	eji_

franka_hw::FrankaModelInterface	fmi_

franka_hw::FrankaStateInterface	fsi_

std::array< double, 3 >	gravity_earth_

std::map< std::string, std::shared_ptr< franka_gazebo::Joint > >	joints_

hardware_interface::JointStateInterface	jsi_

std::vector< double >	lower_force_thresholds_nominal_

std::unique_ptr< franka_hw::ModelBase >	model_

hardware_interface::PositionJointInterface	pji_

gazebo::physics::ModelPtr	robot_

bool	robot_initialized_
	If gazebo::Joint::GetForceTorque() yielded already a non-zero value. More...

ros::Publisher	robot_initialized_pub_

franka::RobotState	robot_state_

ros::ServiceServer	service_collision_behavior_

ros::ServiceClient	service_controller_list_

ros::ServiceClient	service_controller_switch_

ros::ServiceServer	service_set_ee_

ros::ServiceServer	service_set_k_

ros::ServiceServer	service_set_load_

ros::ServiceServer	service_user_stop_

boost::sml::sm< franka_gazebo::StateMachine, boost::sml::thread_safe< std::mutex > >	sm_

double	tau_ext_lowpass_filter_

std::vector< double >	upper_force_thresholds_nominal_

std::unique_ptr< ControllerVerifier >	verifier_

hardware_interface::VelocityJointInterface	vji_

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

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 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 custom implementation of a gazebo_ros_control plugin, which is able to simulate franka interfaces in Gazebo.

Specifically it supports the following hardware transmission types

transmission_interface/SimpleTransmission

hardware_interface/JointStateInterface
hardware_interface/EffortJointInterface
hardware_interface/PositionJointInterface
hardware_interface/VelocityJointInterface

franka_hw/FrankaStateInterface

franka_hw/FrankaModelInterface

See also: http://gazebosim.org/tutorials/?tut=ros_control#Advanced:customgazebo_ros_controlSimulationPlugins

Definition at line 50 of file franka_hw_sim.h.

Constructor & Destructor Documentation

◆ FrankaHWSim()

franka_gazebo::FrankaHWSim::FrankaHWSim ( )

Create a new FrankaHWSim instance.

Definition at line 30 of file franka_hw_sim.cpp.

Member Function Documentation

◆ doSwitch()

void franka_gazebo::FrankaHWSim::doSwitch	(	const std::list< hardware_interface::ControllerInfo > &	start_list,
		const std::list< hardware_interface::ControllerInfo > &	stop_list
	)

overridevirtual

Switches the control mode of the robot arm.

Parameters

start_list	list of controllers to start
stop_list	list of controllers to stop

Reimplemented from hardware_interface::RobotHW.

Definition at line 726 of file franka_hw_sim.cpp.

◆ eStopActive()

void franka_gazebo::FrankaHWSim::eStopActive ( const bool active )

overridevirtual

Set the emergency stop state (not yet implemented)

Parameters

[in] active does currently nothing

Reimplemented from gazebo_ros_control::RobotHWSim.

Definition at line 509 of file franka_hw_sim.cpp.

◆ forControlledJoint()

void franka_gazebo::FrankaHWSim::forControlledJoint	(	const std::list< hardware_interface::ControllerInfo > &	controllers,
		const std::function< void(franka_gazebo::Joint &joint, const ControlMethod &)> &	f
	)

private

Definition at line 744 of file franka_hw_sim.cpp.

◆ guessEndEffector()

void franka_gazebo::FrankaHWSim::guessEndEffector	(	const ros::NodeHandle &	nh,
		const urdf::Model &	urdf
	)

private

Definition at line 564 of file franka_hw_sim.cpp.

◆ initEffortCommandHandle()

void franka_gazebo::FrankaHWSim::initEffortCommandHandle ( const std::shared_ptr< franka_gazebo::Joint > & joint )

private

Definition at line 238 of file franka_hw_sim.cpp.

◆ initFrankaModelHandle()

void franka_gazebo::FrankaHWSim::initFrankaModelHandle	(	const std::string &	robot,
		const urdf::Model &	urdf,
		const transmission_interface::TransmissionInfo &	transmission,
		double	singularity_threshold
	)

private

Definition at line 280 of file franka_hw_sim.cpp.

◆ initFrankaStateHandle()

void franka_gazebo::FrankaHWSim::initFrankaStateHandle	(	const std::string &	robot,
		const urdf::Model &	urdf,
		const transmission_interface::TransmissionInfo &	transmission
	)

private

Definition at line 253 of file franka_hw_sim.cpp.

◆ initJointStateHandle()

void franka_gazebo::FrankaHWSim::initJointStateHandle ( const std::shared_ptr< franka_gazebo::Joint > & joint )

private

Definition at line 233 of file franka_hw_sim.cpp.

◆ initPositionCommandHandle()

void franka_gazebo::FrankaHWSim::initPositionCommandHandle ( const std::shared_ptr< franka_gazebo::Joint > & joint )

private

Definition at line 243 of file franka_hw_sim.cpp.

◆ initServices()

void franka_gazebo::FrankaHWSim::initServices ( ros::NodeHandle & nh )

private

Definition at line 332 of file franka_hw_sim.cpp.

◆ initSim()

bool franka_gazebo::FrankaHWSim::initSim	(	const std::string &	robot_namespace,
		ros::NodeHandle	model_nh,
		gazebo::physics::ModelPtr	parent,
		const urdf::Model *const	urdf,
		std::vector< transmission_interface::TransmissionInfo >	transmissions
	)

overridevirtual

Initialize the simulated robot hardware and parse all supported transmissions.

Parameters

[in]	robot_namespace	the name of the robot passed inside the `<robotNamespace>` tag from the URDF
[in]	model_nh	root node handle of the node into which this plugin is loaded (usually Gazebo)
[in]	parent	the underlying gazebo model type of the robot which was added
[in]	urdf	the parsed URDF which should be added
[in]	transmissions	a list of transmissions of the model which should be simulated

Returns: true if initialization succeeds, false otherwise

Implements gazebo_ros_control::RobotHWSim.

Definition at line 32 of file franka_hw_sim.cpp.

◆ initVelocityCommandHandle()

void franka_gazebo::FrankaHWSim::initVelocityCommandHandle ( const std::shared_ptr< franka_gazebo::Joint > & joint )

private

Definition at line 248 of file franka_hw_sim.cpp.

◆ positionControl()

double franka_gazebo::FrankaHWSim::positionControl	(	Joint &	joint,
		double	setpoint,
		const ros::Duration &	period
	)

staticprivate

Definition at line 440 of file franka_hw_sim.cpp.

◆ prepareSwitch()

bool franka_gazebo::FrankaHWSim::prepareSwitch	(	const std::list< hardware_interface::ControllerInfo > &	start_list,
		const std::list< hardware_interface::ControllerInfo > &
	)

overridevirtual

Check (in non-realtime) if given controllers could be started and stopped from the current state of the RobotHW with regard to necessary hardware interface switches and prepare the switching.

Start and stop list are disjoint. This handles the check and preparation, the actual switch is commited in doSwitch().

Reimplemented from hardware_interface::RobotHW.

Definition at line 718 of file franka_hw_sim.cpp.

◆ readArray()

template<int N>

std::array<double, N> franka_gazebo::FrankaHWSim::readArray	(	std::string	param,
		std::string	name = `""`
	)

inlineprivate

Definition at line 189 of file franka_hw_sim.h.

◆ readParameters()

bool franka_gazebo::FrankaHWSim::readParameters	(	const ros::NodeHandle &	nh,
		const urdf::Model &	urdf
	)

private

Definition at line 511 of file franka_hw_sim.cpp.

◆ readSim()

void franka_gazebo::FrankaHWSim::readSim	(	ros::Time	time,
		ros::Duration	period
	)

overridevirtual

Fetch data from the Gazebo simulation and pass it on to the hardware interfaces.

This will e.g. read the joint positions, velocities and efforts and write them out to controllers via the JointStateInterface and/or franka_hw::FrankaStateInterface

Parameters

[in]	time	the current (simulated) ROS time
[in]	period	the time step at which the simulation is running

Implements gazebo_ros_control::RobotHWSim.

Definition at line 432 of file franka_hw_sim.cpp.

◆ restartControllers()

void franka_gazebo::FrankaHWSim::restartControllers ( )

private

Definition at line 402 of file franka_hw_sim.cpp.

◆ shiftInertiaTensor()

static Eigen::Matrix3d franka_gazebo::FrankaHWSim::shiftInertiaTensor	(	Eigen::Matrix3d	I,
		double	m,
		Eigen::Vector3d	p
	)

inlinestaticprivate

Shift the moment of inertia tensor by a given offset.

This method is based on Steiner's Parallel Axis Theorem

$\mathbf{I^{(p)}} = \mathbf{I} + m \tilde{p}^\top \tilde{p}$

where $\tilde{p}$ is the skewMatrix of p

Parameters

[in]	I	the inertia tensor defined in the original frame or center or mass of `m`
[in]	m	the mass of the body in
[in]	p	the offset vector to move the inertia tensor along starting from center of mass

Returns: the shifted inertia tensor $\mathbf{I^{\left( p \right)}}$

Definition at line 238 of file franka_hw_sim.h.

◆ skewMatrix()

static Eigen::Matrix3d franka_gazebo::FrankaHWSim::skewMatrix ( const Eigen::Vector3d & vec )

inlinestaticprivate

Helper function for generating a skew symmetric matrix for a given input vector such that: $\mathbf{0} = \mathbf{M} \cdot \mathrm{vec}$ .

Parameters

[in] vec the 3D input vector for which to generate the matrix for

Returns: $\mathbf{M}$ i.e. a skew symmetric matrix for vec

Definition at line 212 of file franka_hw_sim.h.

◆ updateRobotState()

void franka_gazebo::FrankaHWSim::updateRobotState ( ros::Time time )

private

Definition at line 638 of file franka_hw_sim.cpp.

◆ updateRobotStateDynamics()

void franka_gazebo::FrankaHWSim::updateRobotStateDynamics ( )

private

Definition at line 626 of file franka_hw_sim.cpp.

◆ velocityControl()

double franka_gazebo::FrankaHWSim::velocityControl	(	Joint &	joint,
		double	setpoint,
		const ros::Duration &	period
	)

staticprivate

Definition at line 464 of file franka_hw_sim.cpp.

◆ writeSim()

void franka_gazebo::FrankaHWSim::writeSim	(	ros::Time	time,
		ros::Duration	period
	)

overridevirtual

Pass the data send from controllers via the hardware interfaces onto the simulation.

This will e.g. write the joint commands (torques or forces) to the corresponding joint in Gazebo in each timestep. These commands are usually send via an EffortJointInterface

Parameters

[in]	time	the current (simulated) ROS time
[in]	period	the time step at which the simulation is running

Implements gazebo_ros_control::RobotHWSim.

Definition at line 470 of file franka_hw_sim.cpp.

Member Data Documentation

◆ action_recovery_

std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction> > franka_gazebo::FrankaHWSim::action_recovery_

private

Definition at line 158 of file franka_hw_sim.h.

◆ arm_id_

std::string franka_gazebo::FrankaHWSim::arm_id_

private

Definition at line 133 of file franka_hw_sim.h.

◆ eji_

hardware_interface::EffortJointInterface franka_gazebo::FrankaHWSim::eji_

private

Definition at line 138 of file franka_hw_sim.h.

◆ fmi_

franka_hw::FrankaModelInterface franka_gazebo::FrankaHWSim::fmi_

private

Definition at line 142 of file franka_hw_sim.h.

◆ fsi_

franka_hw::FrankaStateInterface franka_gazebo::FrankaHWSim::fsi_

private

Definition at line 141 of file franka_hw_sim.h.

◆ gravity_earth_

std::array<double, 3> franka_gazebo::FrankaHWSim::gravity_earth_

private

Definition at line 131 of file franka_hw_sim.h.

◆ joints_

std::map<std::string, std::shared_ptr<franka_gazebo::Joint> > franka_gazebo::FrankaHWSim::joints_

private

Definition at line 135 of file franka_hw_sim.h.

◆ jsi_

hardware_interface::JointStateInterface franka_gazebo::FrankaHWSim::jsi_

private

Definition at line 137 of file franka_hw_sim.h.

◆ lower_force_thresholds_nominal_

std::vector<double> franka_gazebo::FrankaHWSim::lower_force_thresholds_nominal_

private

Definition at line 160 of file franka_hw_sim.h.

◆ model_

std::unique_ptr<franka_hw::ModelBase> franka_gazebo::FrankaHWSim::model_

private

Definition at line 146 of file franka_hw_sim.h.

◆ pji_

hardware_interface::PositionJointInterface franka_gazebo::FrankaHWSim::pji_

private

Definition at line 139 of file franka_hw_sim.h.

◆ robot_

gazebo::physics::ModelPtr franka_gazebo::FrankaHWSim::robot_

private

Definition at line 134 of file franka_hw_sim.h.

◆ robot_initialized_

bool franka_gazebo::FrankaHWSim::robot_initialized_

private

If gazebo::Joint::GetForceTorque() yielded already a non-zero value.

Definition at line 127 of file franka_hw_sim.h.

◆ robot_initialized_pub_

ros::Publisher franka_gazebo::FrankaHWSim::robot_initialized_pub_

private

Definition at line 150 of file franka_hw_sim.h.

◆ robot_state_

franka::RobotState franka_gazebo::FrankaHWSim::robot_state_

private

Definition at line 145 of file franka_hw_sim.h.

◆ service_collision_behavior_

ros::ServiceServer franka_gazebo::FrankaHWSim::service_collision_behavior_

private

Definition at line 154 of file franka_hw_sim.h.

◆ service_controller_list_

ros::ServiceClient franka_gazebo::FrankaHWSim::service_controller_list_

private

Definition at line 156 of file franka_hw_sim.h.

◆ service_controller_switch_

ros::ServiceClient franka_gazebo::FrankaHWSim::service_controller_switch_

private

Definition at line 157 of file franka_hw_sim.h.

◆ service_set_ee_

ros::ServiceServer franka_gazebo::FrankaHWSim::service_set_ee_

private

Definition at line 151 of file franka_hw_sim.h.

◆ service_set_k_

ros::ServiceServer franka_gazebo::FrankaHWSim::service_set_k_

private

Definition at line 152 of file franka_hw_sim.h.

◆ service_set_load_

ros::ServiceServer franka_gazebo::FrankaHWSim::service_set_load_

private

Definition at line 153 of file franka_hw_sim.h.

◆ service_user_stop_

ros::ServiceServer franka_gazebo::FrankaHWSim::service_user_stop_

private

Definition at line 155 of file franka_hw_sim.h.

◆ sm_

boost::sml::sm<franka_gazebo::StateMachine, boost::sml::thread_safe<std::mutex> > franka_gazebo::FrankaHWSim::sm_

private

Definition at line 144 of file franka_hw_sim.h.

◆ tau_ext_lowpass_filter_

double franka_gazebo::FrankaHWSim::tau_ext_lowpass_filter_

private

Definition at line 148 of file franka_hw_sim.h.

◆ upper_force_thresholds_nominal_

std::vector<double> franka_gazebo::FrankaHWSim::upper_force_thresholds_nominal_

private

Definition at line 161 of file franka_hw_sim.h.

◆ verifier_

std::unique_ptr<ControllerVerifier> franka_gazebo::FrankaHWSim::verifier_

private

Definition at line 129 of file franka_hw_sim.h.

◆ vji_

hardware_interface::VelocityJointInterface franka_gazebo::FrankaHWSim::vji_

private

Definition at line 140 of file franka_hw_sim.h.

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

Public Member Functions

Private Member Functions

Static Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

transmission_interface/SimpleTransmission

franka_hw/FrankaStateInterface

franka_hw/FrankaModelInterface

Constructor & Destructor Documentation

◆ FrankaHWSim()

Member Function Documentation

◆ doSwitch()

◆ eStopActive()

◆ forControlledJoint()

◆ guessEndEffector()

◆ initEffortCommandHandle()

◆ initFrankaModelHandle()

◆ initFrankaStateHandle()

◆ initJointStateHandle()

◆ initPositionCommandHandle()

◆ initServices()

◆ initSim()

◆ initVelocityCommandHandle()

◆ positionControl()

◆ prepareSwitch()

◆ readArray()

◆ readParameters()

◆ readSim()

◆ restartControllers()

◆ shiftInertiaTensor()

◆ skewMatrix()

◆ updateRobotState()

◆ updateRobotStateDynamics()

◆ velocityControl()

◆ writeSim()

Member Data Documentation

◆ action_recovery_

◆ arm_id_

◆ eji_

◆ fmi_

◆ fsi_

◆ gravity_earth_

◆ joints_

◆ jsi_

◆ lower_force_thresholds_nominal_

◆ model_

◆ pji_

◆ robot_

◆ robot_initialized_

◆ robot_initialized_pub_

◆ robot_state_

◆ service_collision_behavior_

◆ service_controller_list_

◆ service_controller_switch_

◆ service_set_ee_

◆ service_set_k_

◆ service_set_load_

◆ service_user_stop_

◆ sm_

◆ tau_ext_lowpass_filter_

◆ upper_force_thresholds_nominal_

◆ verifier_

◆ vji_