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

#include <tetrapod_plugin.h>

Inheritance diagram for gazebo::TetrapodPlugin:

Collaboration diagram for gazebo::TetrapodPlugin:

Public Types
enum	ControlMode { position = 1 , velocity = 2 , torque = 3 }
	Control mode enumerator. More...

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

virtual	~TetrapodPlugin ()
	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, 12, 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, 12, 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, 12, 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	SetJointPosition (const std::string &_joint_name, const double &_pos)
	Set the position of the joint. 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	OnFlJointStateMsg (const sensor_msgs::JointStateConstPtr &_msg)
	The OnFlJointStateMsg function handles an incoming joint state message for the front left leg 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. 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	basePosePub
	ROS Base Pose Publisher. More...

ros::Publisher	baseTwistPub
	ROS Base Twist Publisher. More...

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

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

ros::Publisher	jointStatePub
	ROS Joint State Publisher. 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 tetrapod robot.

Definition at line 67 of file tetrapod_plugin.h.

Member Enumeration Documentation

◆ ControlMode

enum gazebo::TetrapodPlugin::ControlMode

Control mode enumerator.

Enumerator
position
velocity
torque

Definition at line 70 of file tetrapod_plugin.h.

Constructor & Destructor Documentation

◆ TetrapodPlugin()

gazebo::TetrapodPlugin::TetrapodPlugin ( )

Constructor.

Definition at line 34 of file tetrapod_plugin.cpp.

◆ ~TetrapodPlugin()

gazebo::TetrapodPlugin::~TetrapodPlugin ( )

virtual

Destructor.

Definition at line 37 of file tetrapod_plugin.cpp.

Member Function Documentation

◆ Load()

void gazebo::TetrapodPlugin::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 tetrapod_plugin.cpp.

◆ GetBasePose()

Eigen::Matrix< double, 6, 1 > gazebo::TetrapodPlugin::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 tetrapod_plugin.cpp.

◆ GetBaseTwist()

Eigen::Matrix< double, 6, 1 > gazebo::TetrapodPlugin::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 tetrapod_plugin.cpp.

◆ GetJointForce()

double gazebo::TetrapodPlugin::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 tetrapod_plugin.cpp.

◆ GetJointForces()

Eigen::Matrix< double, 12, 1 > gazebo::TetrapodPlugin::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 146 of file tetrapod_plugin.cpp.

◆ GetJointVelocity()

double gazebo::TetrapodPlugin::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 174 of file tetrapod_plugin.cpp.

◆ GetJointVelocities()

Eigen::Matrix< double, 12, 1 > gazebo::TetrapodPlugin::GetJointVelocities ( )

Get current joint velocities for the robot.

Returns: Returns joint velocities : dot_q_r

Definition at line 182 of file tetrapod_plugin.cpp.

◆ GetJointPosition()

double gazebo::TetrapodPlugin::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 210 of file tetrapod_plugin.cpp.

◆ GetJointPositions()

Eigen::Matrix< double, 12, 1 > gazebo::TetrapodPlugin::GetJointPositions ( )

Get current joint positions for the robot.

Returns: Returns joint configuration : q_r

Definition at line 218 of file tetrapod_plugin.cpp.

◆ SetJointForce()

void gazebo::TetrapodPlugin::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 247 of file tetrapod_plugin.cpp.

◆ SetJointForces()

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

Apply joint forces.

Parameters

[in] _forces Forces to apply

Definition at line 256 of file tetrapod_plugin.cpp.

◆ SetJointVelocity()

void gazebo::TetrapodPlugin::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 283 of file tetrapod_plugin.cpp.

◆ SetJointVelocities()

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

Set the velocity of the joints.

Parameters

[in] _vel New target velocity

Definition at line 292 of file tetrapod_plugin.cpp.

◆ SetJointPosition()

void gazebo::TetrapodPlugin::SetJointPosition	(	const std::string &	_joint_name,
		const double &	_pos
	)

Set the position of the joint.

Parameters

[in]	_joint_name	Desired joint
[in]	_pos	New target position in radians

Definition at line 319 of file tetrapod_plugin.cpp.

◆ SetJointPositions()

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

Set the position of the joints.

Parameters

[in] _pos New target position vector in radians

Definition at line 328 of file tetrapod_plugin.cpp.

◆ OnJointStateMsg()

void gazebo::TetrapodPlugin::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 355 of file tetrapod_plugin.cpp.

◆ OnFlJointStateMsg()

void gazebo::TetrapodPlugin::OnFlJointStateMsg ( const sensor_msgs::JointStateConstPtr & _msg )

The OnFlJointStateMsg function handles an incoming joint state message for the front left leg 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 374 of file tetrapod_plugin.cpp.

◆ OnForceMsg()

void gazebo::TetrapodPlugin::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 399 of file tetrapod_plugin.cpp.

◆ OnVelMsg()

void gazebo::TetrapodPlugin::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 405 of file tetrapod_plugin.cpp.

◆ OnPosMsg()

void gazebo::TetrapodPlugin::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 411 of file tetrapod_plugin.cpp.

◆ ProcessQueueThread()

void gazebo::TetrapodPlugin::ProcessQueueThread ( )

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

Definition at line 417 of file tetrapod_plugin.cpp.

◆ PublishQueueThread()

void gazebo::TetrapodPlugin::PublishQueueThread ( )

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

Definition at line 427 of file tetrapod_plugin.cpp.

◆ printMap()

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

◆ InitRos()

void gazebo::TetrapodPlugin::InitRos ( )

protected

The initRos function is called to initialize ROS.

Definition at line 478 of file tetrapod_plugin.cpp.

◆ InitRosQueueThreads()

void gazebo::TetrapodPlugin::InitRosQueueThreads ( )

protected

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

Definition at line 624 of file tetrapod_plugin.cpp.

◆ LoadParametersRos()

bool gazebo::TetrapodPlugin::LoadParametersRos ( )

protected

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

Returns: True if parameters loaded successfully.

Definition at line 636 of file tetrapod_plugin.cpp.

◆ InitJointControllers()

void gazebo::TetrapodPlugin::InitJointControllers ( )

protected

The InitJointControllers function is called to setup joint PID controllers.

Definition at line 701 of file tetrapod_plugin.cpp.

◆ InitJointConfiguration()

void gazebo::TetrapodPlugin::InitJointConfiguration ( )

protected

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

Definition at line 718 of file tetrapod_plugin.cpp.

◆ ResetSimulation()

bool gazebo::TetrapodPlugin::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 740 of file tetrapod_plugin.cpp.

Member Data Documentation

◆ model

physics::ModelPtr gazebo::TetrapodPlugin::model

private

Pointer to the model.

Definition at line 226 of file tetrapod_plugin.h.

◆ world

physics::WorldPtr gazebo::TetrapodPlugin::world

private

Pointer to the world.

Definition at line 229 of file tetrapod_plugin.h.

◆ model_name

std::string gazebo::TetrapodPlugin::model_name

private

Model name.

Definition at line 232 of file tetrapod_plugin.h.

◆ base

physics::LinkPtr gazebo::TetrapodPlugin::base

private

Pointer to the body link.

Definition at line 235 of file tetrapod_plugin.h.

◆ joints

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

private

Pointers to the joints.

Definition at line 240 of file tetrapod_plugin.h.

◆ joint_names

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

private

Vector of joint names.

Definition at line 243 of file tetrapod_plugin.h.

◆ joint_config

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

private

Vector of joint configuration.

Definition at line 246 of file tetrapod_plugin.h.

◆ vel_p_gains

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

private

Vector of Velocity P-gains.

Definition at line 249 of file tetrapod_plugin.h.

◆ vel_i_gains

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

private

Vector of Velocity I-gains.

Definition at line 252 of file tetrapod_plugin.h.

◆ vel_d_gains

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

private

Vector of Velocity D-gains.

Definition at line 255 of file tetrapod_plugin.h.

◆ pos_p_gains

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

private

Vector of Position P-gains.

Definition at line 258 of file tetrapod_plugin.h.

◆ pos_i_gains

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

private

Vector of Position I-gains.

Definition at line 261 of file tetrapod_plugin.h.

◆ pos_d_gains

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

private

Vector of Position D-gains.

Definition at line 264 of file tetrapod_plugin.h.

◆ controlMode

ControlMode gazebo::TetrapodPlugin::controlMode

private

Control mode indicator.

Definition at line 267 of file tetrapod_plugin.h.

◆ rosNode

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

private

Node used for ROS transport.

Definition at line 270 of file tetrapod_plugin.h.

◆ resetSimService

ros::ServiceServer gazebo::TetrapodPlugin::resetSimService

private

ROS Reset Simulation Service.

Definition at line 273 of file tetrapod_plugin.h.

◆ genCoordPub

ros::Publisher gazebo::TetrapodPlugin::genCoordPub

private

ROS Generalized Coordinates Publisher.

Definition at line 276 of file tetrapod_plugin.h.

◆ genVelPub

ros::Publisher gazebo::TetrapodPlugin::genVelPub

private

ROS Generalized Velocities Publisher.

Definition at line 279 of file tetrapod_plugin.h.

◆ basePosePub

ros::Publisher gazebo::TetrapodPlugin::basePosePub

private

ROS Base Pose Publisher.

Definition at line 282 of file tetrapod_plugin.h.

◆ baseTwistPub

ros::Publisher gazebo::TetrapodPlugin::baseTwistPub

private

ROS Base Twist Publisher.

Definition at line 285 of file tetrapod_plugin.h.

◆ jointForcesPub

ros::Publisher gazebo::TetrapodPlugin::jointForcesPub

private

ROS Joint Forces (Torques) Publisher.

Definition at line 288 of file tetrapod_plugin.h.

◆ jointStateCmdSub

ros::Subscriber gazebo::TetrapodPlugin::jointStateCmdSub

private

ROS Joint State Subscriber.

Definition at line 291 of file tetrapod_plugin.h.

◆ jointStatePub

ros::Publisher gazebo::TetrapodPlugin::jointStatePub

private

ROS Joint State Publisher.

Definition at line 294 of file tetrapod_plugin.h.

◆ forceSub

ros::Subscriber gazebo::TetrapodPlugin::forceSub

private

ROS Force Subscriber.

Definition at line 297 of file tetrapod_plugin.h.

◆ velSub

ros::Subscriber gazebo::TetrapodPlugin::velSub

private

ROS Velocity Subscriber.

Definition at line 300 of file tetrapod_plugin.h.

◆ posSub

ros::Subscriber gazebo::TetrapodPlugin::posSub

private

ROS Position Subscriber.

Definition at line 303 of file tetrapod_plugin.h.

◆ rosProcessQueue

ros::CallbackQueue gazebo::TetrapodPlugin::rosProcessQueue

private

ROS callbackque that helps process messages.

Definition at line 306 of file tetrapod_plugin.h.

◆ rosPublishQueue

ros::CallbackQueue gazebo::TetrapodPlugin::rosPublishQueue

private

ROS callbackque that helps publish messages.

Definition at line 309 of file tetrapod_plugin.h.

◆ rosProcessQueueThread

std::thread gazebo::TetrapodPlugin::rosProcessQueueThread

private

Thread running the rosProcessQueue.

Definition at line 312 of file tetrapod_plugin.h.

◆ rosPublishQueueThread

std::thread gazebo::TetrapodPlugin::rosPublishQueueThread

private

Thread running the rosPublishQueue.

Definition at line 315 of file tetrapod_plugin.h.

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

catkin_ws/src/simulator/tetrapod_gazebo/include/tetrapod_gazebo/tetrapod_plugin.h
catkin_ws/src/simulator/tetrapod_gazebo/src/tetrapod_plugin.cpp

Public Types

Public Member Functions

Protected Member Functions

Private Attributes

Detailed Description

Member Enumeration Documentation

◆ ControlMode

Constructor & Destructor Documentation

◆ TetrapodPlugin()

◆ ~TetrapodPlugin()

Member Function Documentation

◆ Load()

◆ GetBasePose()

◆ GetBaseTwist()

◆ GetJointForce()

◆ GetJointForces()

◆ GetJointVelocity()

◆ GetJointVelocities()

◆ GetJointPosition()

◆ GetJointPositions()

◆ SetJointForce()

◆ SetJointForces()

◆ SetJointVelocity()

◆ SetJointVelocities()

◆ SetJointPosition()

◆ SetJointPositions()

◆ OnJointStateMsg()

◆ OnFlJointStateMsg()

◆ 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

◆ basePosePub

◆ baseTwistPub

◆ jointForcesPub

◆ jointStateCmdSub

◆ jointStatePub

◆ forceSub

◆ velSub

◆ posSub

◆ rosProcessQueue

◆ rosPublishQueue

◆ rosProcessQueueThread

◆ rosPublishQueueThread