A plugin to control the single_leg robot. More...

#include <single_leg_plugin.h>

Inheritance diagram for gazebo::SingleLegPlugin:

Collaboration diagram for gazebo::SingleLegPlugin:

Public Member Functions
	SingleLegPlugin ()
	Constructor. More...

virtual	~SingleLegPlugin ()
	Destructor. More...

virtual void	Load (physics::ModelPtr _model, sdf::ElementPtr _sdf)
	The Load function is called by Gazebo when the plugin is inserted into simulation. More...

Eigen::Matrix< double, 6, 1 >	GetBasePose ()
	Get current base pose for the robot. More...

Eigen::Matrix< double, 6, 1 >	GetBaseTwist ()
	Get current base twist for the robot. More...

double	GetJointForce (const std::string &_joint_name)
	Get currently applied force at a specific joint. More...

Eigen::Matrix< double, 3, 1 >	GetJointForces ()
	Get current joint forces for the robot. More...

double	GetJointVelocity (const std::string &_joint_name)
	Get current velocity at a specific joint. More...

Eigen::Matrix< double, 3, 1 >	GetJointVelocities ()
	Get current joint velocities for the robot. More...

double	GetJointPosition (const std::string &_joint_name)
	Get current position at a specific joint. More...

Eigen::Matrix< double, 3, 1 >	GetJointPositions ()
	Get current joint positions for the robot. More...

void	SetJointForce (const std::string &_joint_name, const double &_force)
	Apply force at a specific joint. More...

void	SetJointForces (const std::vector< double > &_forces)
	Apply joint forces. More...

void	SetJointVelocity (const std::string &_joint_name, const double &_vel)
	Set the velocity of the joint. More...

void	SetJointVelocities (const std::vector< double > &_vel)
	Set the velocity of the joints. More...

void	SetJointPositions (const std::vector< double > &_pos)
	Set the position of the joints. More...

void	OnJointStateMsg (const sensor_msgs::JointStateConstPtr &_msg)
	The OnJointStateMsg function handles an incoming joint state message from ROS. More...

void	OnForceMsg (const std_msgs::Float64MultiArrayConstPtr &_msg)
	The OnRosMsg function handles an incoming force message from ROS. More...

void	OnVelMsg (const std_msgs::Float64MultiArrayConstPtr &_msg)
	The OnRosMsg function handles an incoming velocity message from ROS. More...

void	OnPosMsg (const std_msgs::Float64MultiArrayConstPtr &_msg)
	The OnRosMsg function handles an incoming velocity message from ROS. More...

void	ProcessQueueThread ()
	The ProcessQueueThread function is a ROS helper function that processes messages. More...

void	PublishQueueThread ()
	The PublishQueueThread function is a ROS helper function that publish state messages. More...

void	printMap (const std::map< std::string, physics::JointPtr > &_map)

bool	ResetSimulation (const std_srvs::Empty::Request &_req, std_srvs::Empty::Response &_res)
	The ResetSimulation function handles an incoming reset simulation service request. More...

Protected Member Functions
void	InitRos ()
	The initRos function is called to initialize ROS. More...

void	InitRosQueueThreads ()
	The InitRosQueueThreads function is called to initialize the ROS Publish and Process Queue Threads. More...

bool	LoadParametersRos ()
	The LoadParametersRos function is called to load conffloatiguration from the parameter server. More...

void	InitJointControllers ()
	The InitJointControllers function is called to setup joint PID controllers. More...

void	InitJointConfiguration ()
	The InitJointConfiguration function is called to initialize the desired joint configuration. More...

Private Attributes
physics::ModelPtr	model
	Pointer to the model. More...

physics::WorldPtr	world
	Pointer to the world. More...

std::string	model_name
	Model name. More...

physics::LinkPtr	base
	Pointer to the body link. More...

std::map< std::string, physics::JointPtr >	joints
	Pointers to the joints. More...

std::vector< std::string >	joint_names
	Vector of joint names. More...

std::vector< double >	joint_config
	Vector of joint configuration. More...

std::vector< double >	vel_p_gains
	Vector of Velocity P-gains. More...

std::vector< double >	vel_i_gains
	Vector of Velocity I-gains. More...

std::vector< double >	vel_d_gains
	Vector of Velocity D-gains. More...

std::vector< double >	pos_p_gains
	Vector of Position P-gains. More...

std::vector< double >	pos_i_gains
	Vector of Position I-gains. More...

std::vector< double >	pos_d_gains
	Vector of Position D-gains. More...

ControlMode	controlMode
	Control mode indicator { position = 1, velocity = 2, torque = 3 }. More...

std::unique_ptr< ros::NodeHandle >	rosNode
	Node used for ROS transport. More...

ros::ServiceServer	resetSimService
	ROS Reset Simulation Service. More...

ros::Publisher	genCoordPub
	ROS Generalized Coordinates Publisher. More...

ros::Publisher	genVelPub
	ROS Generalized Velocities Publisher. More...

ros::Publisher	jointForcesPub
	ROS Joint Forces (Torques) Publisher. More...

ros::Subscriber	jointStateSub
	ROS Joint State Subscriber. More...

ros::Subscriber	forceSub
	ROS Force Subscriber. More...

ros::Subscriber	velSub
	ROS Velocity Subscriber. More...

ros::Subscriber	posSub
	ROS Position Subscriber. More...

ros::CallbackQueue	rosProcessQueue
	ROS callbackque that helps process messages. More...

ros::CallbackQueue	rosPublishQueue
	ROS callbackque that helps publish messages. More...

std::thread	rosProcessQueueThread
	Thread running the rosProcessQueue. More...

std::thread	rosPublishQueueThread
	Thread running the rosPublishQueue. More...

Detailed Description

A plugin to control the single_leg robot.

Definition at line 68 of file single_leg_plugin.h.

Constructor & Destructor Documentation

◆ SingleLegPlugin()

gazebo::SingleLegPlugin::SingleLegPlugin ( )

Constructor.

Definition at line 34 of file single_leg_plugin.cpp.

◆ ~SingleLegPlugin()

gazebo::SingleLegPlugin::~SingleLegPlugin ( )

virtual

Destructor.

Definition at line 37 of file single_leg_plugin.cpp.

Member Function Documentation

◆ Load()

void gazebo::SingleLegPlugin::Load	(	physics::ModelPtr	_model,
		sdf::ElementPtr	_sdf
	)

virtual

The Load function is called by Gazebo when the plugin is inserted into simulation.

Parameters

[in]	_model	A pointer to the model that the plugin is attached to.
[in]	_sdf	A pointer to the plugin's SDF element.

Definition at line 51 of file single_leg_plugin.cpp.

◆ GetBasePose()

Eigen::Matrix< double, 6, 1 > gazebo::SingleLegPlugin::GetBasePose ( )

Get current base pose for the robot.

Returns: Returns base pose: q_b = (x, y, z, roll, pitch, yaw) in the world frame.

Definition at line 96 of file single_leg_plugin.cpp.

◆ GetBaseTwist()

Eigen::Matrix< double, 6, 1 > gazebo::SingleLegPlugin::GetBaseTwist ( )

Get current base twist for the robot.

Returns: Returns base twist: u_b = (x_vel, y_vel, z_vel, roll_rate, pitch_rate, yaw_rate) in the world frame.

Definition at line 113 of file single_leg_plugin.cpp.

◆ GetJointForce()

double gazebo::SingleLegPlugin::GetJointForce ( const std::string & _joint_name )

Get currently applied force at a specific joint.

Parameters

[in] _joint_name Desired joint

Returns: Current joint force

Definition at line 133 of file single_leg_plugin.cpp.

◆ GetJointForces()

Eigen::Matrix< double, 3, 1 > gazebo::SingleLegPlugin::GetJointForces ( )

Get current joint forces for the robot.

Returns: Returns joint velocities : tau_r //TODO maybe the use of tau_r needs change

Definition at line 142 of file single_leg_plugin.cpp.

◆ GetJointVelocity()

double gazebo::SingleLegPlugin::GetJointVelocity ( const std::string & _joint_name )

Get current velocity at a specific joint.

Parameters

[in] _joint_name Desired joint

Returns: Current joint velocity

Definition at line 154 of file single_leg_plugin.cpp.

◆ GetJointVelocities()

Eigen::Matrix< double, 3, 1 > gazebo::SingleLegPlugin::GetJointVelocities ( )

Get current joint velocities for the robot.

Returns: Returns joint velocities : dot_q_r

Definition at line 162 of file single_leg_plugin.cpp.

◆ GetJointPosition()

double gazebo::SingleLegPlugin::GetJointPosition ( const std::string & _joint_name )

Get current position at a specific joint.

Parameters

[in] _joint_name Desired joint

Returns: Current joint position

Definition at line 174 of file single_leg_plugin.cpp.

◆ GetJointPositions()

Eigen::Matrix< double, 3, 1 > gazebo::SingleLegPlugin::GetJointPositions ( )

Get current joint positions for the robot.

Returns: Returns joint configuration : q_r

Definition at line 182 of file single_leg_plugin.cpp.

◆ SetJointForce()

void gazebo::SingleLegPlugin::SetJointForce	(	const std::string &	_joint_name,
		const double &	_force
	)

Apply force at a specific joint.

Parameters

[in]	_joint_name	Desired joint
[in]	_force	Force to apply

Definition at line 195 of file single_leg_plugin.cpp.

◆ SetJointForces()

void gazebo::SingleLegPlugin::SetJointForces ( const std::vector< double > & _forces )

Apply joint forces.

Parameters

[in] _forces Forces to apply

Definition at line 204 of file single_leg_plugin.cpp.

◆ SetJointVelocity()

void gazebo::SingleLegPlugin::SetJointVelocity	(	const std::string &	_joint_name,
		const double &	_vel
	)

Set the velocity of the joint.

Parameters

[in]	_joint_name	Desired joint
[in]	_vel	New target velocity

Definition at line 231 of file single_leg_plugin.cpp.

◆ SetJointVelocities()

void gazebo::SingleLegPlugin::SetJointVelocities ( const std::vector< double > & _vel )

Set the velocity of the joints.

Parameters

[in] _vel New target velocity

Definition at line 240 of file single_leg_plugin.cpp.

◆ SetJointPositions()

void gazebo::SingleLegPlugin::SetJointPositions ( const std::vector< double > & _pos )

Set the position of the joints.

Parameters

[in] _pos New target position vector in radians

Definition at line 267 of file single_leg_plugin.cpp.

◆ OnJointStateMsg()

void gazebo::SingleLegPlugin::OnJointStateMsg ( const sensor_msgs::JointStateConstPtr & _msg )

The OnJointStateMsg function handles an incoming joint state message from ROS.

Parameters

[in]

_msg

A message holding data to describe the state of a set of controlled joints. The state is defined by

the position of the joint,
the velocity of the joint and
the effort (torque) that is applied in the joint.

Definition at line 294 of file single_leg_plugin.cpp.

◆ OnForceMsg()

void gazebo::SingleLegPlugin::OnForceMsg ( const std_msgs::Float64MultiArrayConstPtr & _msg )

The OnRosMsg function handles an incoming force message from ROS.

Parameters

[in] _msg A float array used to set the force of the joints.

Definition at line 313 of file single_leg_plugin.cpp.

◆ OnVelMsg()

void gazebo::SingleLegPlugin::OnVelMsg ( const std_msgs::Float64MultiArrayConstPtr & _msg )

The OnRosMsg function handles an incoming velocity message from ROS.

Parameters

[in] _msg A float array used to set the velocity of the joints.

Definition at line 319 of file single_leg_plugin.cpp.

◆ OnPosMsg()

void gazebo::SingleLegPlugin::OnPosMsg ( const std_msgs::Float64MultiArrayConstPtr & _msg )

The OnRosMsg function handles an incoming velocity message from ROS.

Parameters

[in] _msg A float array used to set the position of the joints.

Definition at line 325 of file single_leg_plugin.cpp.

◆ ProcessQueueThread()

void gazebo::SingleLegPlugin::ProcessQueueThread ( )

The ProcessQueueThread function is a ROS helper function that processes messages.

Definition at line 331 of file single_leg_plugin.cpp.

◆ PublishQueueThread()

void gazebo::SingleLegPlugin::PublishQueueThread ( )

The PublishQueueThread function is a ROS helper function that publish state messages.

Definition at line 341 of file single_leg_plugin.cpp.

◆ printMap()

void gazebo::SingleLegPlugin::printMap ( const std::map< std::string, physics::JointPtr > & _map )

◆ InitRos()

void gazebo::SingleLegPlugin::InitRos ( )

protected

The initRos function is called to initialize ROS.

Definition at line 375 of file single_leg_plugin.cpp.

◆ InitRosQueueThreads()

void gazebo::SingleLegPlugin::InitRosQueueThreads ( )

protected

The InitRosQueueThreads function is called to initialize the ROS Publish and Process Queue Threads.

Definition at line 484 of file single_leg_plugin.cpp.

◆ LoadParametersRos()

bool gazebo::SingleLegPlugin::LoadParametersRos ( )

protected

The LoadParametersRos function is called to load conffloatiguration from the parameter server.

Returns: True if parameters loaded successfully.

Definition at line 496 of file single_leg_plugin.cpp.

◆ InitJointControllers()

void gazebo::SingleLegPlugin::InitJointControllers ( )

protected

The InitJointControllers function is called to setup joint PID controllers.

Definition at line 561 of file single_leg_plugin.cpp.

◆ InitJointConfiguration()

void gazebo::SingleLegPlugin::InitJointConfiguration ( )

protected

The InitJointConfiguration function is called to initialize the desired joint configuration.

Definition at line 578 of file single_leg_plugin.cpp.

◆ ResetSimulation()

bool gazebo::SingleLegPlugin::ResetSimulation	(	const std_srvs::Empty::Request &	_req,
		std_srvs::Empty::Response &	_res
	)

The ResetSimulation function handles an incoming reset simulation service request.

Parameters

[in]	_req	Service request.
[out]	_res	Service response.

Returns: Returns true if success, and false if not.

Definition at line 599 of file single_leg_plugin.cpp.

Member Data Documentation

◆ model

physics::ModelPtr gazebo::SingleLegPlugin::model

private

Pointer to the model.

Definition at line 211 of file single_leg_plugin.h.

◆ world

physics::WorldPtr gazebo::SingleLegPlugin::world

private

Pointer to the world.

Definition at line 214 of file single_leg_plugin.h.

◆ model_name

std::string gazebo::SingleLegPlugin::model_name

private

Model name.

Definition at line 217 of file single_leg_plugin.h.

◆ base

physics::LinkPtr gazebo::SingleLegPlugin::base

private

Pointer to the body link.

Definition at line 220 of file single_leg_plugin.h.

◆ joints

std::map<std::string, physics::JointPtr> gazebo::SingleLegPlugin::joints

private

Pointers to the joints.

Definition at line 225 of file single_leg_plugin.h.

◆ joint_names

std::vector<std::string> gazebo::SingleLegPlugin::joint_names

private

Vector of joint names.

Definition at line 228 of file single_leg_plugin.h.

◆ joint_config

std::vector<double> gazebo::SingleLegPlugin::joint_config

private

Vector of joint configuration.

Definition at line 231 of file single_leg_plugin.h.

◆ vel_p_gains

std::vector<double> gazebo::SingleLegPlugin::vel_p_gains

private

Vector of Velocity P-gains.

Definition at line 234 of file single_leg_plugin.h.

◆ vel_i_gains

std::vector<double> gazebo::SingleLegPlugin::vel_i_gains

private

Vector of Velocity I-gains.

Definition at line 237 of file single_leg_plugin.h.

◆ vel_d_gains

std::vector<double> gazebo::SingleLegPlugin::vel_d_gains

private

Vector of Velocity D-gains.

Definition at line 240 of file single_leg_plugin.h.

◆ pos_p_gains

std::vector<double> gazebo::SingleLegPlugin::pos_p_gains

private

Vector of Position P-gains.

Definition at line 243 of file single_leg_plugin.h.

◆ pos_i_gains

std::vector<double> gazebo::SingleLegPlugin::pos_i_gains

private

Vector of Position I-gains.

Definition at line 246 of file single_leg_plugin.h.

◆ pos_d_gains

std::vector<double> gazebo::SingleLegPlugin::pos_d_gains

private

Vector of Position D-gains.

Definition at line 249 of file single_leg_plugin.h.

◆ controlMode

ControlMode gazebo::SingleLegPlugin::controlMode

private

Control mode indicator { position = 1, velocity = 2, torque = 3 }.

Definition at line 253 of file single_leg_plugin.h.

◆ rosNode

std::unique_ptr<ros::NodeHandle> gazebo::SingleLegPlugin::rosNode

private

Node used for ROS transport.

Definition at line 256 of file single_leg_plugin.h.

◆ resetSimService

ros::ServiceServer gazebo::SingleLegPlugin::resetSimService

private

ROS Reset Simulation Service.

Definition at line 259 of file single_leg_plugin.h.

◆ genCoordPub

ros::Publisher gazebo::SingleLegPlugin::genCoordPub

private

ROS Generalized Coordinates Publisher.

Definition at line 262 of file single_leg_plugin.h.

◆ genVelPub

ros::Publisher gazebo::SingleLegPlugin::genVelPub

private

ROS Generalized Velocities Publisher.

Definition at line 265 of file single_leg_plugin.h.

◆ jointForcesPub

ros::Publisher gazebo::SingleLegPlugin::jointForcesPub

private

ROS Joint Forces (Torques) Publisher.

Definition at line 268 of file single_leg_plugin.h.

◆ jointStateSub

ros::Subscriber gazebo::SingleLegPlugin::jointStateSub

private

ROS Joint State Subscriber.

Definition at line 271 of file single_leg_plugin.h.

◆ forceSub

ros::Subscriber gazebo::SingleLegPlugin::forceSub

private

ROS Force Subscriber.

Definition at line 274 of file single_leg_plugin.h.

◆ velSub

ros::Subscriber gazebo::SingleLegPlugin::velSub

private

ROS Velocity Subscriber.

Definition at line 277 of file single_leg_plugin.h.

◆ posSub

ros::Subscriber gazebo::SingleLegPlugin::posSub

private

ROS Position Subscriber.

Definition at line 280 of file single_leg_plugin.h.

◆ rosProcessQueue

ros::CallbackQueue gazebo::SingleLegPlugin::rosProcessQueue

private

ROS callbackque that helps process messages.

Definition at line 283 of file single_leg_plugin.h.

◆ rosPublishQueue

ros::CallbackQueue gazebo::SingleLegPlugin::rosPublishQueue

private

ROS callbackque that helps publish messages.

Definition at line 286 of file single_leg_plugin.h.

◆ rosProcessQueueThread

std::thread gazebo::SingleLegPlugin::rosProcessQueueThread

private

Thread running the rosProcessQueue.

Definition at line 289 of file single_leg_plugin.h.

◆ rosPublishQueueThread

std::thread gazebo::SingleLegPlugin::rosPublishQueueThread

private

Thread running the rosPublishQueue.

Definition at line 292 of file single_leg_plugin.h.

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

catkin_ws/src/simulator/single_leg_gazebo/include/single_leg_gazebo/single_leg_plugin.h
catkin_ws/src/simulator/single_leg_gazebo/src/single_leg_plugin.cpp

Public Member Functions

Protected Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ SingleLegPlugin()

◆ ~SingleLegPlugin()

Member Function Documentation

◆ Load()

◆ GetBasePose()

◆ GetBaseTwist()

◆ GetJointForce()

◆ GetJointForces()

◆ GetJointVelocity()

◆ GetJointVelocities()

◆ GetJointPosition()

◆ GetJointPositions()

◆ SetJointForce()

◆ SetJointForces()

◆ SetJointVelocity()

◆ SetJointVelocities()

◆ SetJointPositions()

◆ OnJointStateMsg()

◆ OnForceMsg()

◆ OnVelMsg()

◆ OnPosMsg()

◆ ProcessQueueThread()

◆ PublishQueueThread()

◆ printMap()

◆ InitRos()

◆ InitRosQueueThreads()

◆ LoadParametersRos()

◆ InitJointControllers()

◆ InitJointConfiguration()

◆ ResetSimulation()

Member Data Documentation

◆ model

◆ world

◆ model_name

◆ base

◆ joints

◆ joint_names

◆ joint_config

◆ vel_p_gains

◆ vel_i_gains

◆ vel_d_gains

◆ pos_p_gains

◆ pos_i_gains

◆ pos_d_gains

◆ controlMode

◆ rosNode

◆ resetSimService

◆ genCoordPub

◆ genVelPub

◆ jointForcesPub

◆ jointStateSub

◆ forceSub

◆ velSub

◆ posSub

◆ rosProcessQueue

◆ rosPublishQueue

◆ rosProcessQueueThread

◆ rosPublishQueueThread