A plugin that implements the Robotiq 3-Finger Adaptative Gripper. The plugin exposes the next parameters via SDF tags: <side> Determines if we are controlling the left or right hand. This is a required parameter and the allowed values are 'left' or 'right' <kp_position> P gain for the PID that controls the position of the joints. This parameter is optional. <ki_position> I gain for the PID that controls the position of the joints. This parameter is optional. <kd_position> D gain for the PID that controls the position of the joints. This parameter is optional. <position_effort_min> Minimum output of the PID that controls the position of the joints. This parameter is optional <position_effort_max> Maximum output of the PID that controls the position of the joints. This parameter is optional <topic_command> ROS topic name used to send new commands to the hand. This parameter is optional. <topic_state> ROS topic name used to receive state from the hand. This parameter is optional. More...

#include <RobotiqHandPlugin.h>

Public Member Functions
void	Load (gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf)
	RobotiqHandPlugin ()
	Constructor.
virtual	~RobotiqHandPlugin ()
	Destructor.
Private Types
enum	GraspingMode { Basic = 0, Pinch, Wide, Scissor }
	Different grasping modes. More...
enum	State { Disabled = 0, Emergency, ICS, ICF, ChangeModeInProgress, Simplified }
	Hand states. More...
Private Member Functions
bool	FindJoints ()
	Grab pointers to all the joints.
void	GetAndPublishHandleState ()
	Publish Robotiq Hand state.
void	GetAndPublishJointState (const gazebo::common::Time &_curTime)
	Publish Robotiq Joint state.
bool	GetAndPushBackJoint (const std::string &_jointName, gazebo::physics::Joint_V &_joints)
	Internal helper to reduce code duplication. If the joint name is found, a pointer to the joint is added to a vector of joint pointers.
uint8_t	GetCurrentPosition (const gazebo::physics::JointPtr &_joint)
	Internal helper to get the actual position of the finger.
uint8_t	GetObjectDetection (const gazebo::physics::JointPtr &_joint, int _index, uint8_t _rPR, uint8_t _prevrPR)
	Internal helper to get the object detection value.
bool	IsHandFullyOpen ()
	Checks if the hand is fully open. return True when all the fingers are fully open or false otherwise.
void	ReleaseHand ()
	Fully open the hand at half of the maximum speed.
void	RosQueueThread ()
	ROS callback queue thread.
void	SetHandleCommand (const robotiq_s_model_articulated_msgs::SModelRobotOutput::ConstPtr &_msg)
	ROS topic callback to update Robotiq Hand Control Commands.
void	StopHand ()
	Stop the fingers.
void	UpdatePIDControl (double _dt)
	Update PID Joint controllers.
void	UpdateStates ()
	Update the controller.
bool	VerifyCommand (const robotiq_s_model_articulated_msgs::SModelRobotOutput::ConstPtr &_command)
	Verify that all the command fields are within the correct range.
bool	VerifyField (const std::string &_label, int _min, int _max, int _v)
	Verify that one command field is within the correct range.
Private Attributes
boost::thread	callbackQueueThread
	ROS callback queue thread.
boost::mutex	controlMutex
	Controller update mutex.
gazebo::physics::Joint_V	fingerJoints
	Vector containing all the actuated finger joints.
GraspingMode	graspingMode
	Grasping mode.
robotiq_s_model_articulated_msgs::SModelRobotOutput	handleCommand
	HandleControl message. Originally published by user but some of the fields might be internally modified. E.g.: When releasing the hand for.
robotiq_s_model_articulated_msgs::SModelRobotInput	handleState
	Robotiq Hand State.
State	handState
	Hand state.
std::vector< std::string >	jointNames
	Vector containing all the joint names.
gazebo::physics::Joint_V	joints
	Vector containing all the joints.
sensor_msgs::JointState	jointStates
	ROS joint state message.
gazebo::common::Time	lastControllerUpdateTime
	keep track of controller update sim-time.
robotiq_s_model_articulated_msgs::SModelRobotOutput	lastHandleCommand
	HandleControl message. Last command received before changing the grasping mode.
gazebo::physics::ModelPtr	model
	Parent model of the hand.
PubMultiQueue	pmq
gazebo::common::PID	posePID [NumJoints]
	PIDs used to control the finger positions.
robotiq_s_model_articulated_msgs::SModelRobotOutput	prevCommand
	Previous command received. We know if the hand is opening or closing by comparing the current command and the previous one.
ros::Publisher	pubHandleState
	ROS publisher for Robotiq Hand state.
PubQueue < robotiq_s_model_articulated_msgs::SModelRobotInput > ::Ptr	pubHandleStateQueue
	ROS publisher queue for Robotiq Hand state.
ros::Publisher	pubJointStates
	Joint state publisher (rviz visualization).
PubQueue < sensor_msgs::JointState > ::Ptr	pubJointStatesQueue
	ROS publisher queue for joint states.
boost::scoped_ptr < ros::NodeHandle >	rosNode
	ROS NodeHandle.
ros::CallbackQueue	rosQueue
	ROS callback queue.
sdf::ElementPtr	sdf
	Pointer to the SDF of this plugin.
std::string	side
	Used to select between 'left' or 'right' hand.
ros::Subscriber	subHandleCommand
	ROS control interface.
gazebo::event::ConnectionPtr	updateConnection
	gazebo world update connection.
robotiq_s_model_articulated_msgs::SModelRobotOutput	userHandleCommand
	Original HandleControl message (published by user and unmodified).
gazebo::physics::WorldPtr	world
	World pointer.
Static Private Attributes
static const std::string	DefaultLeftTopicCommand = "/left_hand/command"
	Default topic name for sending control updates to the left hand.
static const std::string	DefaultLeftTopicState = "/left_hand/state"
	Default topic name for receiving state updates from the left hand.
static const std::string	DefaultRightTopicCommand = "/right_hand/command"
	Default topic name for sending control updates to the right hand.
static const std::string	DefaultRightTopicState = "/right_hand/state"
	Default topic name for receiving state updates from the right hand.
static const double	MaxVelocity = 0.88
	Max. joint speed (rad/s). Finger is 125mm and tip speed is 110mm/s.
static const double	MinVelocity = 0.176
	Min. joint speed (rad/s). Finger is 125mm and tip speed is 22mm/s.
static const int	NumJoints = 5
	Number of joints in the hand. The three fingers can do abduction/adduction. Fingers 1 and 2 can do circumduction in one axis.
static const double	PoseTolerance = 0.002
	Position tolerance. If the difference between target position and current position is within this value we'll conclude that the joint reached its target (rad).
static const double	VelTolerance = 0.002
	Velocity tolerance. Below this value we assume that the joint is stopped (rad/s).
Friends
class	gazebo::common::PID

Detailed Description

A plugin that implements the Robotiq 3-Finger Adaptative Gripper. The plugin exposes the next parameters via SDF tags: <side> Determines if we are controlling the left or right hand. This is a required parameter and the allowed values are 'left' or 'right' <kp_position> P gain for the PID that controls the position of the joints. This parameter is optional. <ki_position> I gain for the PID that controls the position of the joints. This parameter is optional. <kd_position> D gain for the PID that controls the position of the joints. This parameter is optional. <position_effort_min> Minimum output of the PID that controls the position of the joints. This parameter is optional <position_effort_max> Maximum output of the PID that controls the position of the joints. This parameter is optional <topic_command> ROS topic name used to send new commands to the hand. This parameter is optional. <topic_state> ROS topic name used to receive state from the hand. This parameter is optional.

Definition at line 56 of file RobotiqHandPlugin.h.

Member Enumeration Documentation

enum RobotiqHandPlugin::GraspingMode [private]

Different grasping modes.

Enumerator:

Basic
Pinch
Wide
Scissor

Definition at line 74 of file RobotiqHandPlugin.h.

enum RobotiqHandPlugin::State [private]

Hand states.

Enumerator:

Disabled
Emergency
ICS
ICF
ChangeModeInProgress
Simplified

Definition at line 63 of file RobotiqHandPlugin.h.

Constructor & Destructor Documentation

RobotiqHandPlugin::RobotiqHandPlugin ( )

Constructor.

Definition at line 53 of file RobotiqHandPlugin.cpp.

RobotiqHandPlugin::~RobotiqHandPlugin ( ) [virtual]

Destructor.

Definition at line 70 of file RobotiqHandPlugin.cpp.

Member Function Documentation

bool RobotiqHandPlugin::FindJoints ( ) [private]

Grab pointers to all the joints.

Returns:: true on success, false otherwise.

Definition at line 773 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::GetAndPublishHandleState ( ) [private]

Publish Robotiq Hand state.

Definition at line 525 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::GetAndPublishJointState ( const gazebo::common::Time & _curTime ) [private]

Publish Robotiq Joint state.

Definition at line 642 of file RobotiqHandPlugin.cpp.

bool RobotiqHandPlugin::GetAndPushBackJoint	(	const std::string &	_jointName,
		gazebo::physics::Joint_V &	_joints
	)		`[private]`

Internal helper to reduce code duplication. If the joint name is found, a pointer to the joint is added to a vector of joint pointers.

Parameters:

[in]	_jointName	Joint name.
[out]	_joints	Vector of joint pointers.

Returns:: True when the joint was found or false otherwise.

Definition at line 756 of file RobotiqHandPlugin.cpp.

uint8_t RobotiqHandPlugin::GetCurrentPosition ( const gazebo::physics::JointPtr & _joint ) [private]

Internal helper to get the actual position of the finger.

Parameters:

[in] _joint Finger joint.

Returns:: The actual position of the finger. 0 is the minimum position (fully open) and 255 is the maximum position (fully closed).

Definition at line 506 of file RobotiqHandPlugin.cpp.

uint8_t RobotiqHandPlugin::GetObjectDetection	(	const gazebo::physics::JointPtr &	_joint,
		int	_index,
		uint8_t	_rPR,
		uint8_t	_prevrPR
	)		`[private]`

Internal helper to get the object detection value.

Parameters:

[in]	_joint	Finger joint.
[in]	_index	Index of the position PID for this joint.
[in]	_rPR	Current position request.
[in]	_prevrPR	Previous position request.

Returns:: The information on possible object contact: 0 Finger is in motion (only meaningful if gGTO = 1). 1 Finger has stopped due to a contact while opening. 2 Finger has stopped due to a contact while closing. 3 Finger is at the requested position.

Definition at line 467 of file RobotiqHandPlugin.cpp.

bool RobotiqHandPlugin::IsHandFullyOpen ( ) [private]

Checks if the hand is fully open. return True when all the fingers are fully open or false otherwise.

Definition at line 319 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::Load	(	gazebo::physics::ModelPtr	_parent,
		sdf::ElementPtr	_sdf
	)

Definition at line 80 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::ReleaseHand ( ) [private]

Fully open the hand at half of the maximum speed.

Definition at line 296 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::RosQueueThread ( ) [private]

ROS callback queue thread.

Definition at line 878 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::SetHandleCommand ( const robotiq_s_model_articulated_msgs::SModelRobotOutput::ConstPtr & _msg ) [private]

ROS topic callback to update Robotiq Hand Control Commands.

Parameters:

[in] _msg Incoming ROS message with the next hand command.

Definition at line 277 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::StopHand ( ) [private]

Stop the fingers.

Definition at line 310 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::UpdatePIDControl ( double _dt ) [private]

Update PID Joint controllers.

Parameters:

[in] _dt time step size since last update.

Definition at line 657 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::UpdateStates ( ) [private]

Update the controller.

Definition at line 339 of file RobotiqHandPlugin.cpp.

bool RobotiqHandPlugin::VerifyCommand ( const robotiq_s_model_articulated_msgs::SModelRobotOutput::ConstPtr & _command ) [private]

Verify that all the command fields are within the correct range.

Parameters:

[in] _command Robot output message.

Returns:: True if all the fields are withing the correct range or false otherwise.

Definition at line 253 of file RobotiqHandPlugin.cpp.

bool RobotiqHandPlugin::VerifyField	(	const std::string &	_label,
		int	_min,
		int	_max,
		int	_v
	)		`[private]`

Verify that one command field is within the correct range.

Parameters:

[in]	_label	Label of the field. E.g.: rACT, rMOD.
[in]	_min	Minimum value.
[in]	_max	Maximum value.
[in]	_v	Value to be verified.

Returns:: True when the value is within the limits or false otherwise.

Definition at line 240 of file RobotiqHandPlugin.cpp.

Friends And Related Function Documentation

friend class gazebo::common::PID [friend]

Definition at line 61 of file RobotiqHandPlugin.h.

Member Data Documentation

boost::thread RobotiqHandPlugin::callbackQueueThread [private]

ROS callback queue thread.

Definition at line 208 of file RobotiqHandPlugin.h.

boost::mutex RobotiqHandPlugin::controlMutex [private]

Controller update mutex.

Definition at line 242 of file RobotiqHandPlugin.h.

const std::string RobotiqHandPlugin::DefaultLeftTopicCommand = "/left_hand/command" [static, private]

Default topic name for sending control updates to the left hand.

Definition at line 190 of file RobotiqHandPlugin.h.

const std::string RobotiqHandPlugin::DefaultLeftTopicState = "/left_hand/state" [static, private]

Default topic name for receiving state updates from the left hand.

Definition at line 193 of file RobotiqHandPlugin.h.

const std::string RobotiqHandPlugin::DefaultRightTopicCommand = "/right_hand/command" [static, private]

Default topic name for sending control updates to the right hand.

Definition at line 196 of file RobotiqHandPlugin.h.

const std::string RobotiqHandPlugin::DefaultRightTopicState = "/right_hand/state" [static, private]

Default topic name for receiving state updates from the right hand.

Definition at line 199 of file RobotiqHandPlugin.h.

gazebo::physics::Joint_V RobotiqHandPlugin::fingerJoints [private]

Vector containing all the actuated finger joints.

Definition at line 281 of file RobotiqHandPlugin.h.

GraspingMode RobotiqHandPlugin::graspingMode [private]

Grasping mode.

Definition at line 245 of file RobotiqHandPlugin.h.

robotiq_s_model_articulated_msgs::SModelRobotOutput RobotiqHandPlugin::handleCommand [private]

HandleControl message. Originally published by user but some of the fields might be internally modified. E.g.: When releasing the hand for.

Definition at line 219 of file RobotiqHandPlugin.h.

robotiq_s_model_articulated_msgs::SModelRobotInput RobotiqHandPlugin::handleState [private]

Robotiq Hand State.

Definition at line 239 of file RobotiqHandPlugin.h.

State RobotiqHandPlugin::handState [private]

Hand state.

Definition at line 248 of file RobotiqHandPlugin.h.

std::vector<std::string> RobotiqHandPlugin::jointNames [private]

Vector containing all the joint names.

Definition at line 278 of file RobotiqHandPlugin.h.

gazebo::physics::Joint_V RobotiqHandPlugin::joints [private]

Vector containing all the joints.

Definition at line 284 of file RobotiqHandPlugin.h.

sensor_msgs::JointState RobotiqHandPlugin::jointStates [private]

ROS joint state message.

Definition at line 263 of file RobotiqHandPlugin.h.

gazebo::common::Time RobotiqHandPlugin::lastControllerUpdateTime [private]

keep track of controller update sim-time.

Definition at line 236 of file RobotiqHandPlugin.h.

robotiq_s_model_articulated_msgs::SModelRobotOutput RobotiqHandPlugin::lastHandleCommand [private]

HandleControl message. Last command received before changing the grasping mode.

Definition at line 223 of file RobotiqHandPlugin.h.

const double RobotiqHandPlugin::MaxVelocity = 0.88 [static, private]

Max. joint speed (rad/s). Finger is 125mm and tip speed is 110mm/s.

Definition at line 187 of file RobotiqHandPlugin.h.

const double RobotiqHandPlugin::MinVelocity = 0.176 [static, private]

Min. joint speed (rad/s). Finger is 125mm and tip speed is 22mm/s.

Definition at line 184 of file RobotiqHandPlugin.h.

gazebo::physics::ModelPtr RobotiqHandPlugin::model [private]

Parent model of the hand.

Definition at line 269 of file RobotiqHandPlugin.h.

const int RobotiqHandPlugin::NumJoints = 5 [static, private]

Number of joints in the hand. The three fingers can do abduction/adduction. Fingers 1 and 2 can do circumduction in one axis.

Definition at line 172 of file RobotiqHandPlugin.h.

PubMultiQueue RobotiqHandPlugin::pmq [private]

Definition at line 211 of file RobotiqHandPlugin.h.

gazebo::common::PID RobotiqHandPlugin::posePID[NumJoints] [private]

PIDs used to control the finger positions.

Definition at line 287 of file RobotiqHandPlugin.h.

const double RobotiqHandPlugin::PoseTolerance = 0.002 [static, private]

Position tolerance. If the difference between target position and current position is within this value we'll conclude that the joint reached its target (rad).

Definition at line 181 of file RobotiqHandPlugin.h.

robotiq_s_model_articulated_msgs::SModelRobotOutput RobotiqHandPlugin::prevCommand [private]

Previous command received. We know if the hand is opening or closing by comparing the current command and the previous one.

Definition at line 227 of file RobotiqHandPlugin.h.

ros::Publisher RobotiqHandPlugin::pubHandleState [private]

ROS publisher for Robotiq Hand state.

Definition at line 251 of file RobotiqHandPlugin.h.

PubQueue<robotiq_s_model_articulated_msgs::SModelRobotInput>::Ptr RobotiqHandPlugin::pubHandleStateQueue [private]

ROS publisher queue for Robotiq Hand state.

Definition at line 254 of file RobotiqHandPlugin.h.

ros::Publisher RobotiqHandPlugin::pubJointStates [private]

Joint state publisher (rviz visualization).

Definition at line 257 of file RobotiqHandPlugin.h.

PubQueue<sensor_msgs::JointState>::Ptr RobotiqHandPlugin::pubJointStatesQueue [private]

ROS publisher queue for joint states.

Definition at line 260 of file RobotiqHandPlugin.h.

boost::scoped_ptr<ros::NodeHandle> RobotiqHandPlugin::rosNode [private]

ROS NodeHandle.

Definition at line 202 of file RobotiqHandPlugin.h.

ros::CallbackQueue RobotiqHandPlugin::rosQueue [private]

ROS callback queue.

Definition at line 205 of file RobotiqHandPlugin.h.

sdf::ElementPtr RobotiqHandPlugin::sdf [private]

Pointer to the SDF of this plugin.

Definition at line 272 of file RobotiqHandPlugin.h.

std::string RobotiqHandPlugin::side [private]

Used to select between 'left' or 'right' hand.

Definition at line 275 of file RobotiqHandPlugin.h.

ros::Subscriber RobotiqHandPlugin::subHandleCommand [private]

ROS control interface.

Definition at line 214 of file RobotiqHandPlugin.h.

gazebo::event::ConnectionPtr RobotiqHandPlugin::updateConnection [private]

gazebo world update connection.

Definition at line 233 of file RobotiqHandPlugin.h.

robotiq_s_model_articulated_msgs::SModelRobotOutput RobotiqHandPlugin::userHandleCommand [private]

Original HandleControl message (published by user and unmodified).

Definition at line 230 of file RobotiqHandPlugin.h.

const double RobotiqHandPlugin::VelTolerance = 0.002 [static, private]

Velocity tolerance. Below this value we assume that the joint is stopped (rad/s).

Definition at line 176 of file RobotiqHandPlugin.h.

gazebo::physics::WorldPtr RobotiqHandPlugin::world [private]

World pointer.

Definition at line 266 of file RobotiqHandPlugin.h.

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

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Static Private Attributes

Friends

Detailed Description

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation