ROSEndEffector
ROS End-Effector package: provides a ROS-based set of standard interfaces to command robotics end-effectors in an agnostic fashion.
Public Types | Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes | Private Member Functions | Private Attributes | List of all members
ROSEE::EEHal Class Referenceabstract

Class representing an end-effector. More...

#include <EEHal.h>

+ Inheritance diagram for ROSEE::EEHal:
+ Collaboration diagram for ROSEE::EEHal:

Public Types

typedef std::shared_ptr< EEHalPtr
 
typedef std::shared_ptr< const EEHalConstPtr
 

Public Member Functions

 EEHal (ros::NodeHandle *nh)
 
virtual ~EEHal ()
 
virtual bool sense ()=0
 
virtual bool move ()=0
 
virtual bool publish_joint_state ()
 
virtual bool checkCommandPub ()
 
virtual bool getFingersNames (std::vector< std::string > &fingers_names)
 
virtual bool getMotorsNames (std::vector< std::string > &motors_names)
 
virtual bool getMotorStiffness (Eigen::VectorXd &motors_stiffness)
 
virtual bool getTipsFrictions (Eigen::VectorXd &tips_friction)
 
virtual bool getMotorTorqueLimits (Eigen::VectorXd &motors_torque_limits)
 
virtual bool getTipJointToTipFrameX (Eigen::VectorXd &tip_joint_to_tip_frame_x)
 
virtual bool getTipJointToTipFrameY (Eigen::VectorXd &tip_joint_to_tip_frame_y)
 
virtual bool getTipsJacobiansNormal (std::unordered_map< std::string, Eigen::MatrixXd > &tips_jacobian_normal)
 
virtual bool getTipsJacobiansFriction (std::unordered_map< std::string, Eigen::MatrixXd > &tips_jacobian_friction)
 
virtual bool getTransmissionMatrix (Eigen::MatrixXd &transmission_matrix)
 
virtual bool getTransmissionSquareMatrices (std::unordered_map< std::string, Eigen::MatrixXd > &transmission_square_matrices)
 
virtual bool getTransmissionAugmentedMatrices (std::unordered_map< std::string, Eigen::MatrixXd > &transmission_augmented_matrices)
 
virtual bool getTipsForcesNormal (Eigen::VectorXd &tips_forces_normal)
 
virtual bool getTipsForcesFriction (Eigen::VectorXd &tips_forces_friction)
 
virtual bool parseHandInfo ()
 
virtual bool isHandInfoPresent ()
 
virtual bool init_hand_info_response ()
 init the service message with info parsed from yaml file, ie info that will not change according to hand configuration A derived HAL can override this if necessary, since this is called by EEHalExecutor More...
 
virtual bool setHandInfoCallback ()
 Here service is advertise and callback set: if derived class wants to use different callback, they must override this and do : _hand_info_server = _nh->advertiseService(_hand_info_service_name, <custom_callback>);. More...
 
bool publish_pressure ()
 
Eigen::VectorXd getMotorReference () const
 
Eigen::VectorXd getJointPosition () const
 
Eigen::VectorXd getJointEffort () const
 
Eigen::MatrixXd getPressure () const
 

Public Attributes

bool _pressure_active
 

Protected Member Functions

bool initPressureSensing ()
 

Protected Attributes

ros::NodeHandle * _nh
 
sensor_msgs::JointState _mr_msg
 The references that must be read in the move() to send to the real/simulated robot TODO put private and create a get ? (no set) More...
 
ros::Subscriber _motor_reference_sub
 
sensor_msgs::JointState _js_msg
 The states that must be filled in the sense(), reading info from real/simulated robot TODO put private and create a set (no get) ? More...
 
ros::Publisher _joint_state_pub
 
rosee_msg::MotorPhalangePressure _pressure_msg
 
ros::Publisher _pressure_pub
 
std::vector< std::string > fingers_names
 
std::vector< std::string > motors_names
 
Eigen::VectorXd tips_frictions
 
Eigen::VectorXd motors_torque_limits
 
Eigen::VectorXd motors_stiffness
 
Eigen::VectorXd tip_joint_to_tip_frame_x
 
Eigen::VectorXd tip_joint_to_tip_frame_y
 
ros::ServiceServer _hand_info_server
 
rosee_msg::HandInfo::Response _hand_info_response
 
std::string _hand_info_service_name
 

Private Member Functions

bool handInfoEEHalCallback (rosee_msg::HandInfo::Request &request, rosee_msg::HandInfo::Response &response)
 
void motor_reference_clbk (const sensor_msgs::JointState::ConstPtr &msg)
 

Private Attributes

bool _hand_info_present
 

Detailed Description

Class representing an end-effector.

Definition at line 54 of file EEHal.h.

Member Typedef Documentation

typedef std::shared_ptr<const EEHal> ROSEE::EEHal::ConstPtr

Definition at line 59 of file EEHal.h.

typedef std::shared_ptr<EEHal> ROSEE::EEHal::Ptr

Definition at line 58 of file EEHal.h.

Constructor & Destructor Documentation

ROSEE::EEHal::EEHal ( ros::NodeHandle *  nh)

Definition at line 20 of file EEHal.cpp.

20  {
21 
22  _nh = nh;
23 
24  //init sub to receive reference from UniversalROSEEEX
25  //TODO take topic name from roslaunch
26  std::string motor_reference_topic = "/ros_end_effector/motor_reference_pos";
27 
28  _motor_reference_sub = _nh->subscribe(motor_reference_topic, 10,
30 
31  std::string joint_state_topic = "/ros_end_effector/joint_states";
32 
33  _joint_state_pub = _nh->advertise<sensor_msgs::JointState>(joint_state_topic, 10);
34 
36 
37  if (_hand_info_present) {
38  _hand_info_service_name = "hand_info";
39  }
40 
41  _pressure_active = false; // if a derived class want to use this, it must call initPressureSensing()
42 
43 
44 
45 }
ros::Publisher _joint_state_pub
Definition: EEHal.h:137
bool _pressure_active
Definition: EEHal.h:112
void motor_reference_clbk(const sensor_msgs::JointState::ConstPtr &msg)
Definition: EEHal.cpp:54
ros::NodeHandle * _nh
Definition: EEHal.h:123
bool _hand_info_present
Definition: EEHal.h:157
ros::Subscriber _motor_reference_sub
Definition: EEHal.h:130
std::string _hand_info_service_name
Definition: EEHal.h:152
virtual bool parseHandInfo()
Definition: EEHal.cpp:157
virtual ROSEE::EEHal::~EEHal ( )
inlinevirtual

Definition at line 62 of file EEHal.h.

62 {};

Member Function Documentation

bool ROSEE::EEHal::checkCommandPub ( )
virtual

Definition at line 47 of file EEHal.cpp.

47  {
48 
49  return (_motor_reference_sub.getNumPublishers() > 0 && _mr_msg.name.size() != 0);
50 }
sensor_msgs::JointState _mr_msg
The references that must be read in the move() to send to the real/simulated robot TODO put private a...
Definition: EEHal.h:129
ros::Subscriber _motor_reference_sub
Definition: EEHal.h:130
bool ROSEE::EEHal::getFingersNames ( std::vector< std::string > &  fingers_names)
virtual

Definition at line 69 of file EEHal.cpp.

69  {
70 
71  if (this->fingers_names.size() != 0) {
72 
73  fingers_names = this->fingers_names;
74  return true;
75 
76  } else {
77  return false;
78  }
79 
80 }
std::vector< std::string > fingers_names
Definition: EEHal.h:146
Eigen::VectorXd ROSEE::EEHal::getJointEffort ( ) const

Definition at line 298 of file EEHal.cpp.

298  {
299 
300  Eigen::VectorXd jointEffort;
301  jointEffort.resize(_js_msg.name.size());
302  for (int i=0; i<_js_msg.name.size(); i++ ) {
303 
304  jointEffort(i) = _js_msg.effort.at(i);
305  }
306 
307  return jointEffort;
308 }
sensor_msgs::JointState _js_msg
The states that must be filled in the sense(), reading info from real/simulated robot TODO put privat...
Definition: EEHal.h:136
Eigen::VectorXd ROSEE::EEHal::getJointPosition ( ) const

Definition at line 286 of file EEHal.cpp.

286  {
287 
288  Eigen::VectorXd jointPos;
289  jointPos.resize(_js_msg.name.size());
290  for (int i=0; i<_js_msg.name.size(); i++ ) {
291 
292  jointPos(i) = _js_msg.position.at(i);
293  }
294 
295  return jointPos;
296 }
sensor_msgs::JointState _js_msg
The states that must be filled in the sense(), reading info from real/simulated robot TODO put privat...
Definition: EEHal.h:136
Eigen::VectorXd ROSEE::EEHal::getMotorReference ( ) const

Definition at line 274 of file EEHal.cpp.

274  {
275 
276  Eigen::VectorXd motorRef;
277  motorRef.resize(_mr_msg.name.size());
278  for (int i=0; i<_mr_msg.name.size(); i++ ) {
279 
280  motorRef(i) = _mr_msg.position.at(i);
281  }
282 
283  return motorRef;
284 }
sensor_msgs::JointState _mr_msg
The references that must be read in the move() to send to the real/simulated robot TODO put private a...
Definition: EEHal.h:129
bool ROSEE::EEHal::getMotorsNames ( std::vector< std::string > &  motors_names)
virtual

Definition at line 81 of file EEHal.cpp.

81  {
82 
83  if (this->motors_names.size() != 0) {
84 
85  motors_names = this->motors_names;
86  return true;
87 
88  } else {
89  return false;
90  }
91 
92 }
std::vector< std::string > motors_names
Definition: EEHal.h:146
bool ROSEE::EEHal::getMotorStiffness ( Eigen::VectorXd &  motors_stiffness)
virtual

Definition at line 94 of file EEHal.cpp.

94  {
95 
96  if (this->motors_stiffness.size() != 0) {
97 
99  return true;
100 
101  } else {
102  return false;
103  }
104 
105 }
Eigen::VectorXd motors_stiffness
Definition: EEHal.h:147
bool ROSEE::EEHal::getMotorTorqueLimits ( Eigen::VectorXd &  motors_torque_limits)
virtual

Definition at line 120 of file EEHal.cpp.

120  {
121 
122  if (this->motors_torque_limits.size() != 0) {
123 
125  return true;
126 
127  } else {
128  return false;
129  }
130 
131 }
Eigen::VectorXd motors_torque_limits
Definition: EEHal.h:147
Eigen::MatrixXd ROSEE::EEHal::getPressure ( ) const

Definition at line 310 of file EEHal.cpp.

310  {
311 
312  Eigen::MatrixXd pressure;
313  pressure.resize(4, _pressure_msg.pressure_finger1.size()); //message has 4 finger field
314  for (int i=0; i<_pressure_msg.pressure_finger1.size(); i++ ) {
315 
316  pressure(0, i) = _pressure_msg.pressure_finger1.at(i);
317  pressure(1, i) = _pressure_msg.pressure_finger2.at(i);
318  pressure(2, i) = _pressure_msg.pressure_finger3.at(i);
319  pressure(3, i) = _pressure_msg.pressure_thumb.at(i);
320  }
321 
322  return pressure;
323 
324 }
rosee_msg::MotorPhalangePressure _pressure_msg
Definition: EEHal.h:141
bool ROSEE::EEHal::getTipJointToTipFrameX ( Eigen::VectorXd &  tip_joint_to_tip_frame_x)
virtual

Definition at line 133 of file EEHal.cpp.

133  {
134 
135  if (this->tip_joint_to_tip_frame_x.size() != 0) {
136 
138  return true;
139 
140  } else {
141  return false;
142  }
143 }
Eigen::VectorXd tip_joint_to_tip_frame_x
Definition: EEHal.h:147
bool ROSEE::EEHal::getTipJointToTipFrameY ( Eigen::VectorXd &  tip_joint_to_tip_frame_y)
virtual

Definition at line 145 of file EEHal.cpp.

145  {
146 
147  if (this->tip_joint_to_tip_frame_y.size() != 0) {
148 
150  return true;
151 
152  } else {
153  return false;
154  }
155 }
Eigen::VectorXd tip_joint_to_tip_frame_y
Definition: EEHal.h:147
virtual bool ROSEE::EEHal::getTipsForcesFriction ( Eigen::VectorXd &  tips_forces_friction)
inlinevirtual

Definition at line 93 of file EEHal.h.

93 {return false;}
virtual bool ROSEE::EEHal::getTipsForcesNormal ( Eigen::VectorXd &  tips_forces_normal)
inlinevirtual

Definition at line 92 of file EEHal.h.

92 {return false;}
bool ROSEE::EEHal::getTipsFrictions ( Eigen::VectorXd &  tips_friction)
virtual

Definition at line 107 of file EEHal.cpp.

107  {
108 
109  if (this->tips_frictions.size() != 0) {
110 
112  return true;
113 
114  } else {
115  return false;
116  }
117 
118 }
Eigen::VectorXd tips_frictions
Definition: EEHal.h:147
virtual bool ROSEE::EEHal::getTipsJacobiansFriction ( std::unordered_map< std::string, Eigen::MatrixXd > &  tips_jacobian_friction)
inlinevirtual

Definition at line 88 of file EEHal.h.

88 {return false;}
virtual bool ROSEE::EEHal::getTipsJacobiansNormal ( std::unordered_map< std::string, Eigen::MatrixXd > &  tips_jacobian_normal)
inlinevirtual

Definition at line 87 of file EEHal.h.

87 {return false;}
virtual bool ROSEE::EEHal::getTransmissionAugmentedMatrices ( std::unordered_map< std::string, Eigen::MatrixXd > &  transmission_augmented_matrices)
inlinevirtual

Definition at line 91 of file EEHal.h.

91 {return false;}
virtual bool ROSEE::EEHal::getTransmissionMatrix ( Eigen::MatrixXd &  transmission_matrix)
inlinevirtual

Definition at line 89 of file EEHal.h.

89 {return false;}
virtual bool ROSEE::EEHal::getTransmissionSquareMatrices ( std::unordered_map< std::string, Eigen::MatrixXd > &  transmission_square_matrices)
inlinevirtual

Definition at line 90 of file EEHal.h.

90 {return false;}
bool ROSEE::EEHal::handInfoEEHalCallback ( rosee_msg::HandInfo::Request &  request,
rosee_msg::HandInfo::Response &  response 
)
private

Definition at line 242 of file EEHal.cpp.

244  {
245 
246  //generic hal does not read the request
247 
248  response = _hand_info_response;
249 
250  return true;
251 }
rosee_msg::HandInfo::Response _hand_info_response
Definition: EEHal.h:151
bool ROSEE::EEHal::init_hand_info_response ( )
virtual

init the service message with info parsed from yaml file, ie info that will not change according to hand configuration A derived HAL can override this if necessary, since this is called by EEHalExecutor

Definition at line 217 of file EEHal.cpp.

217  {
218 
219  _hand_info_response.fingers_names = fingers_names;
220 
221  _hand_info_response.motors_names = motors_names;
222 
223  _hand_info_response.motors_stiffness =
225 
226  _hand_info_response.tips_frictions =
228 
229  _hand_info_response.motors_torque_limits =
231 
232  return true;
233 }
Eigen::VectorXd motors_stiffness
Definition: EEHal.h:147
Eigen::VectorXd tips_frictions
Definition: EEHal.h:147
std::vector< std::string > fingers_names
Definition: EEHal.h:146
static std::vector< float > eigenVectorToStdVector(Eigen::VectorXd eigenVector)
Definition: UtilsEigen.h:58
std::vector< std::string > motors_names
Definition: EEHal.h:146
Eigen::VectorXd motors_torque_limits
Definition: EEHal.h:147
rosee_msg::HandInfo::Response _hand_info_response
Definition: EEHal.h:151
bool ROSEE::EEHal::initPressureSensing ( )
protected

Definition at line 253 of file EEHal.cpp.

254 {
255 
256  std::string topic_name = "/ros_end_effector/pressure_phalanges";
257 
258  _pressure_pub = _nh->advertise<rosee_msg::MotorPhalangePressure>(topic_name, 10);
259 
260  _pressure_active = true;
261 
262  return true;
263 }
bool _pressure_active
Definition: EEHal.h:112
ros::NodeHandle * _nh
Definition: EEHal.h:123
ros::Publisher _pressure_pub
Definition: EEHal.h:142
bool ROSEE::EEHal::isHandInfoPresent ( )
virtual

Definition at line 52 of file EEHal.cpp.

52 { return _hand_info_present; }
bool _hand_info_present
Definition: EEHal.h:157
void ROSEE::EEHal::motor_reference_clbk ( const sensor_msgs::JointState::ConstPtr &  msg)
private

Definition at line 54 of file EEHal.cpp.

54  {
55 
56  _mr_msg = *msg;
57 
58 }
sensor_msgs::JointState _mr_msg
The references that must be read in the move() to send to the real/simulated robot TODO put private a...
Definition: EEHal.h:129
virtual bool ROSEE::EEHal::move ( )
pure virtual

Implemented in ROSEE::DummyHal, and ROSEE::XBot2Hal.

bool ROSEE::EEHal::parseHandInfo ( )
virtual

Definition at line 157 of file EEHal.cpp.

157  {
158 
159  std::string _rosee_config_path;
160  if (! _nh->getParam ( "/ros_ee_config_path", _rosee_config_path )) {
161  return false;
162  }
163 
164  std::ifstream ifile(_rosee_config_path);
165  if (! ifile) {
166  ROS_WARN_STREAM ("EEHALExecutor: config file " << _rosee_config_path << " not found");
167  return false;
168  }
169 
170  YAML::Node node = YAML::LoadFile(_rosee_config_path);
171 
172  if (! node["hand_info"]) {
173  ROS_WARN_STREAM ("EEHALExecutor: config file " << _rosee_config_path << " does not contain "
174  << "hand_info node. I will not parse hand information");
175 
176  return false;
177  }
178 
179  if(node["hand_info"]["fingers_names"]){
180 
181  fingers_names = node["hand_info"]["fingers_names"].as<std::vector<std::string>>();
182  }
183 
184  if(node["hand_info"]["motors_names"]){
185 
186  motors_names = node["hand_info"]["motors_names"].as<std::vector<std::string>>();
187  }
188 
189  if(node["hand_info"]["motors_stiffness"]){
190 
191  motors_stiffness = ROSEE::Utils::yamlVectorToEigen(node["hand_info"]["motors_stiffness"]);
192  }
193 
194  if(node["hand_info"]["tips_frictions"]){
195 
196  tips_frictions = ROSEE::Utils::yamlVectorToEigen(node["hand_info"]["tips_frictions"]);
197  }
198 
199  if(node["hand_info"]["motors_torque_limits"]){
200 
201  motors_torque_limits = ROSEE::Utils::yamlVectorToEigen(node["hand_info"]["motors_torque_limits"]);
202  }
203 
204  if(node["hand_info"]["tip_joint_to_tip_frame_x"]){
205 
206  tip_joint_to_tip_frame_x = ROSEE::Utils::yamlVectorToEigen(node["hand_info"]["tip_joint_to_tip_frame_x"]);
207  }
208 
209  if(node["hand_info"]["tip_joint_to_tip_frame_y"]){
210 
211  tip_joint_to_tip_frame_y = ROSEE::Utils::yamlVectorToEigen(node["hand_info"]["tip_joint_to_tip_frame_y"]);
212  }
213 
214  return true;
215 }
Eigen::VectorXd motors_stiffness
Definition: EEHal.h:147
Eigen::VectorXd tip_joint_to_tip_frame_y
Definition: EEHal.h:147
Eigen::VectorXd tips_frictions
Definition: EEHal.h:147
std::vector< std::string > fingers_names
Definition: EEHal.h:146
ros::NodeHandle * _nh
Definition: EEHal.h:123
std::vector< std::string > motors_names
Definition: EEHal.h:146
Eigen::VectorXd motors_torque_limits
Definition: EEHal.h:147
static Eigen::VectorXd yamlVectorToEigen(const YAML::Node &vectorNode)
Definition: UtilsYAML.h:37
Eigen::VectorXd tip_joint_to_tip_frame_x
Definition: EEHal.h:147
bool ROSEE::EEHal::publish_joint_state ( )
virtual

Definition at line 60 of file EEHal.cpp.

60  {
61 
62  //NOTE _js_msg must be filled by the derived class
63  _joint_state_pub.publish(_js_msg);
64 
65  return true;
66 
67 }
ros::Publisher _joint_state_pub
Definition: EEHal.h:137
sensor_msgs::JointState _js_msg
The states that must be filled in the sense(), reading info from real/simulated robot TODO put privat...
Definition: EEHal.h:136
bool ROSEE::EEHal::publish_pressure ( )

Definition at line 265 of file EEHal.cpp.

265  {
266 
267  //NOTE _pressure_msg must be filled by the derived class
268  _pressure_pub.publish(_pressure_msg);
269 
270  return true;
271 
272 }
rosee_msg::MotorPhalangePressure _pressure_msg
Definition: EEHal.h:141
ros::Publisher _pressure_pub
Definition: EEHal.h:142
virtual bool ROSEE::EEHal::sense ( )
pure virtual

Implemented in ROSEE::DummyHal, and ROSEE::XBot2Hal.

bool ROSEE::EEHal::setHandInfoCallback ( )
virtual

Here service is advertise and callback set: if derived class wants to use different callback, they must override this and do : _hand_info_server = _nh->advertiseService(_hand_info_service_name, <custom_callback>);.

Definition at line 235 of file EEHal.cpp.

235  {
236 
237  _hand_info_server = _nh->advertiseService(_hand_info_service_name,
239  return true;
240 }
ros::NodeHandle * _nh
Definition: EEHal.h:123
std::string _hand_info_service_name
Definition: EEHal.h:152
bool handInfoEEHalCallback(rosee_msg::HandInfo::Request &request, rosee_msg::HandInfo::Response &response)
Definition: EEHal.cpp:242
ros::ServiceServer _hand_info_server
Definition: EEHal.h:150

Member Data Documentation

bool ROSEE::EEHal::_hand_info_present
private

Definition at line 157 of file EEHal.h.

rosee_msg::HandInfo::Response ROSEE::EEHal::_hand_info_response
protected

Definition at line 151 of file EEHal.h.

ros::ServiceServer ROSEE::EEHal::_hand_info_server
protected

Definition at line 150 of file EEHal.h.

std::string ROSEE::EEHal::_hand_info_service_name
protected

Definition at line 152 of file EEHal.h.

ros::Publisher ROSEE::EEHal::_joint_state_pub
protected

Definition at line 137 of file EEHal.h.

sensor_msgs::JointState ROSEE::EEHal::_js_msg
protected

The states that must be filled in the sense(), reading info from real/simulated robot TODO put private and create a set (no get) ?

Definition at line 136 of file EEHal.h.

ros::Subscriber ROSEE::EEHal::_motor_reference_sub
protected

Definition at line 130 of file EEHal.h.

sensor_msgs::JointState ROSEE::EEHal::_mr_msg
protected

The references that must be read in the move() to send to the real/simulated robot TODO put private and create a get ? (no set)

Definition at line 129 of file EEHal.h.

ros::NodeHandle* ROSEE::EEHal::_nh
protected

Definition at line 123 of file EEHal.h.

bool ROSEE::EEHal::_pressure_active

Definition at line 112 of file EEHal.h.

rosee_msg::MotorPhalangePressure ROSEE::EEHal::_pressure_msg
protected

Definition at line 141 of file EEHal.h.

ros::Publisher ROSEE::EEHal::_pressure_pub
protected

Definition at line 142 of file EEHal.h.

std::vector<std::string> ROSEE::EEHal::fingers_names
protected

Definition at line 146 of file EEHal.h.

std::vector<std::string> ROSEE::EEHal::motors_names
protected

Definition at line 146 of file EEHal.h.

Eigen::VectorXd ROSEE::EEHal::motors_stiffness
protected

Definition at line 147 of file EEHal.h.

Eigen::VectorXd ROSEE::EEHal::motors_torque_limits
protected

Definition at line 147 of file EEHal.h.

Eigen::VectorXd ROSEE::EEHal::tip_joint_to_tip_frame_x
protected

Definition at line 147 of file EEHal.h.

Eigen::VectorXd ROSEE::EEHal::tip_joint_to_tip_frame_y
protected

Definition at line 147 of file EEHal.h.

Eigen::VectorXd ROSEE::EEHal::tips_frictions
protected

Definition at line 147 of file EEHal.h.


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