A plugin that implements the Robotiq 3-Finger Adaptative Gripper. The plugin exposes the next parameters via SDF tags: More...

#include <RobotiqHandPlugin.h>

Inheritance diagram for RobotiqHandPlugin:

[legend]

Public Member Functions
void	Load (gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf)

	RobotiqHandPlugin ()
	Constructor. More...

virtual	~RobotiqHandPlugin ()
	Destructor. More...

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

void	GetAndPublishHandleState ()
	Publish Robotiq Hand state. More...

void	GetAndPublishJointState (const gazebo::common::Time &_curTime)
	Publish Robotiq Joint state. More...

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

uint8_t	GetCurrentPosition (const gazebo::physics::JointPtr &_joint)
	Internal helper to get the actual position of the finger. More...

uint8_t	GetObjectDetection (const gazebo::physics::JointPtr &_joint, int _index, uint8_t _rPR, uint8_t _prevrPR)
	Internal helper to get the object detection value. More...

bool	IsHandFullyOpen ()
	Checks if the hand is fully open. return True when all the fingers are fully open or false otherwise. More...

void	ReleaseHand ()
	Fully open the hand at half of the maximum speed. More...

void	RosQueueThread ()
	ROS callback queue thread. More...

void	SetHandleCommand (const robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput::ConstPtr &_msg)
	ROS topic callback to update Robotiq Hand Control Commands. More...

void	StopHand ()
	Stop the fingers. More...

void	UpdatePIDControl (double _dt)
	Update PID Joint controllers. More...

void	UpdateStates ()
	Update the controller. More...

bool	VerifyCommand (const robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput::ConstPtr &_command)
	Verify that all the command fields are within the correct range. More...

bool	VerifyField (const std::string &_label, int _min, int _max, int _v)
	Verify that one command field is within the correct range. More...

Private Attributes
boost::thread	callbackQueueThread
	ROS callback queue thread. More...

boost::mutex	controlMutex
	Controller update mutex. More...

gazebo::physics::Joint_V	fingerJoints
	Vector containing all the actuated finger joints. More...

GraspingMode	graspingMode
	Grasping mode. More...

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

robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInput	handleState
	Robotiq Hand State. More...

State	handState
	Hand state. More...

std::vector< std::string >	jointNames
	Vector containing all the joint names. More...

gazebo::physics::Joint_V	joints
	Vector containing all the joints. More...

sensor_msgs::JointState	jointStates
	ROS joint state message. More...

gazebo::common::Time	lastControllerUpdateTime
	keep track of controller update sim-time. More...

robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput	lastHandleCommand
	HandleControl message. Last command received before changing the grasping mode. More...

gazebo::physics::ModelPtr	model
	Parent model of the hand. More...

PubMultiQueue	pmq

gazebo::common::PID	posePID [NumJoints]
	PIDs used to control the finger positions. More...

robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput	prevCommand
	Previous command received. We know if the hand is opening or closing by comparing the current command and the previous one. More...

ros::Publisher	pubHandleState
	ROS publisher for Robotiq Hand state. More...

PubQueue< robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInput >::Ptr	pubHandleStateQueue
	ROS publisher queue for Robotiq Hand state. More...

ros::Publisher	pubJointStates
	Joint state publisher (rviz visualization). More...

PubQueue< sensor_msgs::JointState >::Ptr	pubJointStatesQueue
	ROS publisher queue for joint states. More...

boost::scoped_ptr< ros::NodeHandle >	rosNode
	ROS NodeHandle. More...

ros::CallbackQueue	rosQueue
	ROS callback queue. More...

sdf::ElementPtr	sdf
	Pointer to the SDF of this plugin. More...

std::string	side
	Used to select between 'left' or 'right' hand. More...

ros::Subscriber	subHandleCommand
	ROS control interface. More...

gazebo::event::ConnectionPtr	updateConnection
	gazebo world update connection. More...

robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput	userHandleCommand
	Original HandleControl message (published by user and unmodified). More...

gazebo::physics::WorldPtr	world
	World pointer. More...

Static Private Attributes
static const std::string	DefaultLeftTopicCommand
	Default topic name for sending control updates to the left hand. More...

static const std::string	DefaultLeftTopicState
	Default topic name for receiving state updates from the left hand. More...

static const std::string	DefaultRightTopicCommand
	Default topic name for sending control updates to the right hand. More...

static const std::string	DefaultRightTopicState
	Default topic name for receiving state updates from the right hand. More...

static constexpr double	MaxVelocity = 0.88
	Max. joint speed (rad/s). Finger is 125mm and tip speed is 110mm/s. More...

static constexpr double	MinVelocity = 0.176
	Min. joint speed (rad/s). Finger is 125mm and tip speed is 22mm/s. More...

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

static constexpr 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). More...

static constexpr double	VelTolerance = 0.002
	Velocity tolerance. Below this value we assume that the joint is stopped (rad/s). More...

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 52 of file RobotiqHandPlugin.cpp.

RobotiqHandPlugin::~RobotiqHandPlugin ( )

virtual

Destructor.

Definition at line 69 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 839 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::GetAndPublishHandleState ( )

private

Publish Robotiq Hand state.

Definition at line 551 of file RobotiqHandPlugin.cpp.

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

private

Publish Robotiq Joint state.

Definition at line 668 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 822 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 522 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 483 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 324 of file RobotiqHandPlugin.cpp.

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

Definition at line 81 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::ReleaseHand ( )

private

Fully open the hand at half of the maximum speed.

Definition at line 301 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::RosQueueThread ( )

private

ROS callback queue thread.

Definition at line 944 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::SetHandleCommand ( const robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput::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 282 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::StopHand ( )

private

Stop the fingers.

Definition at line 315 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 687 of file RobotiqHandPlugin.cpp.

void RobotiqHandPlugin::UpdateStates ( )

private

Update the controller.

Definition at line 352 of file RobotiqHandPlugin.cpp.

bool RobotiqHandPlugin::VerifyCommand ( const robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput::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 258 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 245 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

staticprivate

Initial value:

=

"/left_hand/command"

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

Definition at line 190 of file RobotiqHandPlugin.h.

const std::string RobotiqHandPlugin::DefaultLeftTopicState

staticprivate

Initial value:

=

"/left_hand/state"

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

Definition at line 193 of file RobotiqHandPlugin.h.

const std::string RobotiqHandPlugin::DefaultRightTopicCommand

staticprivate

Initial value:

=

"/right_hand/command"

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

Definition at line 196 of file RobotiqHandPlugin.h.

const std::string RobotiqHandPlugin::DefaultRightTopicState

staticprivate

Initial value:

=

"/right_hand/state"

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_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput 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_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInput 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_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput RobotiqHandPlugin::lastHandleCommand

private

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

Definition at line 223 of file RobotiqHandPlugin.h.

constexpr double RobotiqHandPlugin::MaxVelocity = 0.88

staticprivate

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

Definition at line 187 of file RobotiqHandPlugin.h.

constexpr double RobotiqHandPlugin::MinVelocity = 0.176

staticprivate

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

staticprivate

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.

constexpr double RobotiqHandPlugin::PoseTolerance = 0.002

staticprivate

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_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput 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_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInput>::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_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput RobotiqHandPlugin::userHandleCommand

private

Original HandleControl message (published by user and unmodified).

Definition at line 230 of file RobotiqHandPlugin.h.

constexpr double RobotiqHandPlugin::VelTolerance = 0.002

staticprivate

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