ROSEndEffector
ROS End-Effector package: provides a ROS-based set of standard interfaces to command robotics end-effectors in an agnostic fashion.
ParserMoveIt.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2020 <copyright holder> <email>
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
20 
21 }
22 
24 
25 }
26 
27 bool ROSEE::ParserMoveIt::init ( std::string robot_description, bool verbose ) {
28 
29  if (robot_model != nullptr ) {
30  std::cerr << "[PARSER::" << __func__ << "]: init() already called by someone " << std::endl;;
31  return false;
32  }
33  // it is a ros param in the launch, take care that also sdrf is read by robot_mo
34  // (param for srd is robot_description_semantic)
35  this->robot_description = robot_description;
36 
37  //false: we dont need kinematic solvers now
38  robot_model_loader::RobotModelLoader robot_model_loader(robot_description, false) ;
39  robot_model = robot_model_loader.getModel() ;
40  if (robot_model == nullptr) {
41  std::cerr << " [PARSER::" << __func__ <<
42  "]: Fail To load robot model " << robot_description <<
43  " are you sure to have put both urdf and srdf files in the parameter server " <<
44  "with names robot_description and robot_description_semantic, respectively?" << std::endl;
45  return false;
46  }
47  std::cout << "[PARSER::" << __func__ << "]: Parsed Model: " << robot_model->getName() << std::endl; ;
48 
49  handName = robot_model->getName();
50 
51  lookForFingertips(verbose);
56 
57  return true;
58 
59 }
60 
61 std::string ROSEE::ParserMoveIt::getHandName() const {
62  return handName;
63 }
64 
65 std::vector<std::string> ROSEE::ParserMoveIt::getFingertipNames() const {
66  return fingertipNames;
67 }
68 
69 std::vector<std::string> ROSEE::ParserMoveIt::getActiveJointNames() const {
70  return activeJointNames;
71 }
72 
73 std::vector<const moveit::core::JointModel*> ROSEE::ParserMoveIt::getActiveJointModels() const {
74  return activeJointModels;
75 }
76 
77 std::vector<std::string> ROSEE::ParserMoveIt::getPassiveJointNames() const {
78  return passiveJointNames;
79 }
80 
81 std::map <std::string, std::vector < const moveit::core::LinkModel* > > ROSEE::ParserMoveIt::getDescendantLinksOfJoint() const {
83 }
84 
85 std::map <std::string, std::vector < const moveit::core::JointModel* > > ROSEE::ParserMoveIt::getDescendantJointsOfJoint() const {
87 }
88 
89 unsigned int ROSEE::ParserMoveIt::getNFingers () const {
90  return nFingers;
91 }
92 
93 const robot_model::RobotModelPtr ROSEE::ParserMoveIt::getRobotModel () const {
94  return robot_model;
95 }
96 
97 std::map<std::string, std::vector<std::string> > ROSEE::ParserMoveIt::getFingertipsOfJointMap() const {
98  return fingertipsOfJointMap;
99 }
100 
101 std::map<std::string, std::vector<std::string> > ROSEE::ParserMoveIt::getJointsOfFingertipMap() const {
102  return jointsOfFingertipMap;
103 }
104 
105 std::map < std::string, std::string> ROSEE::ParserMoveIt::getFingerOfFingertipMap() const {
106  return fingerOfFingertipMap;
107 }
108 
109 std::string ROSEE::ParserMoveIt::getFingerOfFingertip (std::string tipName) const {
110 
111  auto it = fingerOfFingertipMap.find(tipName);
112 
113  if (it != fingerOfFingertipMap.end() ) {
114  return (it->second);
115 
116  } else {
117  return "";
118  }
119 }
120 
121 std::map < std::string, std::string> ROSEE::ParserMoveIt::getFingertipOfFingerMap() const {
122  return fingertipOfFingerMap;
123 }
124 
125 std::string ROSEE::ParserMoveIt::getFingertipOfFinger (std::string fingerName) const {
126 
127  auto it = fingertipOfFingerMap.find(fingerName);
128 
129  if (it != fingertipOfFingerMap.end() ) {
130  return (it->second);
131 
132  } else {
133  return "";
134  }
135 }
136 
137 std::pair<std::string, std::string> ROSEE::ParserMoveIt::getMimicNLFatherOfJoint(std::string mimicNLJointName) const {
138 
139  std::pair<std::string, std::string> retPair;
140 
141  auto it = mimicNLFatherOfJointMap.find(mimicNLJointName);
142 
143  if (it != mimicNLFatherOfJointMap.end()) {
144  retPair = it->second;
145  }
146  return retPair;
147 }
148 
149 std::map<std::string, std::pair<std::string, std::string>> ROSEE::ParserMoveIt::getMimicNLFatherOfJointMap() const {
150 
152 
153 }
154 
155 std::string ROSEE::ParserMoveIt::getMimicNLJointOfFather(std::string mimicNLFatherName, std::string mimicNLJointName) const {
156 
157  auto map = getMimicNLJointsOfFather(mimicNLFatherName);
158 
159  auto it = map.find(mimicNLJointName);
160 
161  if (it != map.end()) {
162  return it->second;
163  }
164 
165  return "";
166 }
167 
168 std::map<std::string, std::string> ROSEE::ParserMoveIt::getMimicNLJointsOfFather(std::string mimicNLFatherName) const {
169 
170  std::map<std::string, std::string> map;
171 
172  auto it = mimicNLJointsOfFatherMap.find(mimicNLFatherName);
173 
174  if (it != mimicNLJointsOfFatherMap.end()) {
175  map = it->second;
176  }
177  return map;
178 }
179 
180 std::map<std::string, std::map<std::string, std::string>> ROSEE::ParserMoveIt::getMimicNLJointsOfFatherMap() const {
181 
183 
184 }
185 
186 
187 robot_model::RobotModelPtr ROSEE::ParserMoveIt::getCopyModel() const {
188  robot_model_loader::RobotModelLoader robot_model_loader(robot_description);
189  return robot_model_loader.getModel();
190 }
191 
192 std::vector < std::string > ROSEE::ParserMoveIt::getGroupOfLink ( std::string linkName ) {
193 
194  std::vector < std::string > groupsReturn;
195 
196  if (robot_model == nullptr) {
197  std::cerr << " [PARSER::" << __func__ <<
198  "]: robot_model is null. Have you called init() before?" << std::endl;
199  return groupsReturn;
200  }
201 
202  for (auto group : robot_model->getJointModelGroups() ) {
203 
204  if ( group->hasLinkModel(linkName) ) {
205 
206  groupsReturn.push_back ( group->getName() ) ;
207  }
208  }
209  return groupsReturn;
210 }
211 
212 bool ROSEE::ParserMoveIt::groupIsChain(const std::string groupName) const {
213 
214  if (robot_model == nullptr) {
215  std::cerr << " [PARSER::" << __func__ <<
216  "]: robot_model is null. Have you called init() before?" << std::endl;
217  return false;
218  }
219 
220  if (! robot_model->hasJointModelGroup(groupName) ) {
221  std::cerr << " [PARSER::" << __func__ <<
222  "]: " << groupName << " is not a group " << std::endl;
223  return false;
224  }
225  return groupIsChain(robot_model->getJointModelGroup(groupName));
226 }
227 
228 bool ROSEE::ParserMoveIt::groupIsChain(const moveit::core::JointModelGroup* group) const {
229 
230  std::stringstream log;
231  log << "Checking if " << group->getName() << " is a chain ..." << std::endl;
232  for (auto link : group->getLinkModels() ){
233  if (link->getChildJointModels().size() > 1 ) {
234  log << "... no because " << link->getName() << " has " << link->getChildJointModels().size() << " children " << std::endl;
235  // std::cout << log.str() << std::endl;
236  return false;
237  }
238  }
239  return true;
240 }
241 
242 
243 
244 bool ROSEE::ParserMoveIt::checkIfContinuosJoint ( std::string jointName) const {
245  if (robot_model == nullptr) {
246  std::cerr << " [PARSER::" << __func__ <<
247  "]: robot_model is null. Have you called init() before?" << std::endl;
248  return false;
249  }
250  return (ROSEE::ParserMoveIt::checkIfContinuosJoint(robot_model->getJointModel(jointName)));
251 }
252 
253 bool ROSEE::ParserMoveIt::checkIfContinuosJoint ( const moveit::core::JointModel* joint ) const {
254  if (robot_model == nullptr) {
255  std::cerr << " [PARSER::" << __func__ <<
256  "]: robot_model is null. Have you called init() before?" << std::endl;
257  return false;
258  }
259 
260  // for moveit, a continuos joint is a revolute joint
261  if (joint->getType() != moveit::core::JointModel::REVOLUTE ) {
262  return false;
263  }
264 
265  //if revolute, only one limit (at.(0))
266  moveit::core::JointModel::Bounds limits = joint->getVariableBounds();
267  if ( limits.at(0).max_position_ - limits.at(0).min_position_ >= (2*EIGEN_PI) ) {
268  return true;
269  }
270 
271  return false;
272 }
273 
274 std::vector <double> ROSEE::ParserMoveIt::getBiggerBoundFromZero ( std::string jointName ) const {
275  if (robot_model == nullptr) {
276  std::cerr << " [PARSER::" << __func__ <<
277  "]: robot_model is null. Have you called init() before?" << std::endl;
278  return std::vector<double>();
279  }
280  return ( ROSEE::ParserMoveIt::getBiggerBoundFromZero (robot_model->getJointModel(jointName) ) );
281 
282 }
283 
284 std::vector <double> ROSEE::ParserMoveIt::getBiggerBoundFromZero ( const moveit::core::JointModel* joint ) const {
285  if (robot_model == nullptr) {
286  std::cerr << " [PARSER::" << __func__ <<
287  "]: robot_model is null. Have you called init() before?" << std::endl;
288  return std::vector<double>();
289  }
290 
291  moveit::core::JointModel::Bounds limits = joint->getVariableBounds();
292 
293  std::vector <double> maxPos;
294  for ( auto limit : limits ) {
295  if ( std::abs(limit.max_position_) > std::abs(limit.min_position_)) {
296  maxPos.push_back ( limit.max_position_ ) ;
297  } else {
298  maxPos.push_back ( limit.min_position_ ) ;
299  }
300  }
301  return maxPos;
302 }
303 
304 std::vector <double> ROSEE::ParserMoveIt::getSmallerBoundFromZero ( std::string jointName ) const {
305  if (robot_model == nullptr) {
306  std::cerr << " [PARSER::" << __func__ <<
307  "]: robot_model is null. Have you called init() before?" << std::endl;
308  return std::vector<double>();
309  }
310  return ( ROSEE::ParserMoveIt::getSmallerBoundFromZero (robot_model->getJointModel(jointName) ) );
311 
312 }
313 
314 std::vector <double> ROSEE::ParserMoveIt::getSmallerBoundFromZero ( const moveit::core::JointModel* joint ) const {
315  if (robot_model == nullptr) {
316  std::cerr << " [PARSER::" << __func__ <<
317  "]: robot_model is null. Have you called init() before?" << std::endl;
318  return std::vector<double>();
319  }
320 
321  moveit::core::JointModel::Bounds limits = joint->getVariableBounds();
322 
323  std::vector <double> minPos;
324  for ( auto limit : limits ) {
325  if ( std::abs(limit.max_position_) < std::abs(limit.min_position_)) {
326  minPos.push_back ( limit.max_position_ ) ;
327  } else {
328  minPos.push_back ( limit.min_position_ ) ;
329  }
330  }
331  return minPos;
332 }
333 
334 unsigned int ROSEE::ParserMoveIt::getNExclusiveJointsOfTip ( std::string tipName, bool continuosIncluded ) const {
335 
336  if (jointsOfFingertipMap.size() == 0) {
337  std::cerr << " [PARSER::" << __func__ <<
338  "]: jointsOfFingertipMap empty. Have you called init() before? Also check your URDF and SRDF files"
339  << std::endl;
340  return 0;
341  }
342 
343  if (fingertipsOfJointMap.size() == 0) {
344  std::cerr << " [PARSER::" << __func__ <<
345  "]: jointsOfFingertipMap empty. Have you called init() before? Also check your URDF and SRDF files"
346  << std::endl;
347  return 0;
348  }
349 
350  unsigned int nExclusiveJoints = 0;
351 
352  for (auto jointOfTip : jointsOfFingertipMap.find(tipName)->second ) {
353 
354  if ( !continuosIncluded && checkIfContinuosJoint(jointOfTip) == true ) {
355  continue; //we dont want to count a continuos joint
356  }
357 
358  //check if the joints of the tip move only that tip
359  if ( fingertipsOfJointMap.find(jointOfTip)->second.size() == 1 ) {
360 
361  if (fingertipsOfJointMap.find(jointOfTip)->second.at(0).compare (tipName) != 0) {
362  // this condition is false if jointsOfFingertipMap and fingertipsOfJointMap are built well
363  std::cerr << " [PARSER::" << __func__ <<
364  "]: Strange error in fingertipsOfJointMap and jointsOfFingertipMap: they are not consistent: "
365  << "The unique tip present in the map for the key " << jointOfTip
366  << " is " << fingertipsOfJointMap.find(jointOfTip)->second.at(0)
367  << " and not " << tipName << " as it should be"
368  << std::endl;
369  return 0;
370  }
371 
372  nExclusiveJoints++;
373  }
374  }
375  return nExclusiveJoints;
376 }
377 
378 
379 std::string ROSEE::ParserMoveIt::getFirstActuatedParentJoint ( std::string linkName, bool includeContinuos ) const {
380 
381  const moveit::core::LinkModel* linkModel = robot_model->getLinkModel ( linkName ) ;
382 
383  while ( linkModel->getParentJointModel()->getMimic() != NULL ||
384  linkModel->parentJointIsFixed() ||
385  linkModel->getParentJointModel()->isPassive() ||
386  (!includeContinuos && checkIfContinuosJoint(linkModel->getParentJointModel())) ) {
387 
388  //an active joint is not any of these condition.
389  //passive is an attribute of the joint in the srdf, so it may be not setted
390  // (default is not passive), so we need also the getMimic == NULL
391  // (ie: an actuated joint dont mimic anything)
392  //WARNING these 4 conditions should be enough I think
393 
394  linkModel = linkModel->getParentLinkModel();
395 
396  if (linkModel == NULL ) {
397 
398  std::cerr << " [PARSER::" << __func__ <<
399  "]: Strange error: fingertipsOfJointMap, jointsOfFingertipMap, and/or other structures " <<
400  "may have been built badly" << std::endl ;
401  return "";
402  }
403  }
404 
405  return (linkModel->getParentJointModel()->getName());
406 }
407 
408 
409 std::string ROSEE::ParserMoveIt::getFirstActuatedJointInFinger (std::string linkName) const {
410  const moveit::core::LinkModel* linkModel = robot_model->getLinkModel(linkName);
411  const moveit::core::JointModel* joint;
412 
413  // we stop when the link has more than 1 joint: so linkModel will be the parent of the first
414  // link of the finger group and in joint we have stored the actuated (not continuos)
415  // child joint most near to linkModel
416  while ( (linkModel != NULL) && (linkModel->getChildJointModels().size() < 2) ) {
417 
418  //if the parent joint is an actuated (not cont) joint, store it (or overwrite the previous stored)
419  if ( linkModel->getParentJointModel()->getMimic() == NULL &&
420  (!linkModel->parentJointIsFixed()) &&
421  (!linkModel->getParentJointModel()->isPassive()) &&
422  (!checkIfContinuosJoint(linkModel->getParentJointModel() )) ) {
423 
424  joint = linkModel->getParentJointModel();
425  }
426 
427  linkModel = linkModel->getParentLinkModel();
428  }
429 
430  return joint->getName();
431 }
432 
433 
434 
435 /*********************************** PRIVATE FUNCTIONS **********************************************************/
437  for (auto it: robot_model->getJointModelGroups()) {
438 
439  std::string logGroupInfo;
440  logGroupInfo = "[PARSER::" + std::string(__func__) + "] Found Group '" + it->getName() + "', " ;
441 
442  if (it->getSubgroupNames().size() != 0 ) {
443  logGroupInfo.append("but it has some subgroups \n");
444 
445  } else if (! groupIsChain ( it ) ) {
446  logGroupInfo.append("but it is not a chain \n");
447 
448  } else if (it->getLinkModels().size() == 0) {
449  logGroupInfo.append("but it has 0 links \n");
450 
451  } else {
452 
453  logGroupInfo.append("with links: \n");
454 
455  std::string theTip = ""; //the last link with a visual geometry
456  for (auto itt : it->getLinkModels()) {
457 
458  logGroupInfo.append("\t'" + itt->getName() + "' ");
459 
460  if (itt->getChildJointModels().size() != 0) {
461  logGroupInfo.append("(not a leaf link) ");
462  } else {
463  logGroupInfo.append("(a leaf link) ");
464  }
465 
466  if (itt->getShapes().size() == 0 ) {
467  logGroupInfo.append("(no visual geometry) ");
468 
469  } else {
470  theTip = itt->getName();
471  }
472  logGroupInfo.append("\n");
473 
474  }
475 
476  if (theTip.compare("") == 0) {
477  logGroupInfo.append("Warning: No link has a mesh in this group\n");
478 
479  } else {
480  fingertipNames.push_back(theTip);
481  fingerOfFingertipMap.insert( std::make_pair(theTip, it->getName()));
482  fingertipOfFingerMap.insert( std::make_pair(it->getName(), theTip));
483  }
484  }
485 
486  if (verbose) {
487  std::cout << logGroupInfo << std::endl;
488  }
489  }
490  nFingers = fingertipNames.size();
491 }
492 
494 
495  for (auto joint : robot_model->getActiveJointModels() ) {
496  // robot_model->getActiveJointModels() returns not fixed not mimic but CAN return PASSIVE joints
497  if (! joint->isPassive() ) {
498  activeJointNames.push_back(joint->getName());
499  activeJointModels.push_back(joint);
500  }
501  }
502 }
503 
505 
506  for (auto joint : robot_model->getJointModels() ) {
507  if ( joint->isPassive() ) {
508  passiveJointNames.push_back(joint->getName());
509  }
510  }
511 }
512 
513 
515 
516  //initialize the map with all tips and with empty vectors of its joints
517  for (const auto &it: fingertipNames) {
518  jointsOfFingertipMap.insert ( std::make_pair (it, std::vector<std::string>() ) );
519  }
520 
521  //initialize the map with all the actuated joints and an empty vector for the tips that the joint move
522  for (const auto &it: activeJointNames) {
523  fingertipsOfJointMap.insert ( std::make_pair (it, std::vector<std::string>() ) );
524  }
525 
526  for ( const auto &jointLink: descendantLinksOfJoint){ //for each actuated joint
527 
528  for (const auto &link : jointLink.second) { //for each descendant link
529 
530  //if link is a tip...
531  if (std::find(fingertipNames.begin(), fingertipNames.end(), link->getName()) != fingertipNames.end()){
532  jointsOfFingertipMap.at ( link->getName() ) .push_back( jointLink.first);
533  fingertipsOfJointMap.at ( jointLink.first ) .push_back( link->getName() );
534  }
535  }
536  }
537 
538 }
539 
541 
542  for (auto actJoint : activeJointModels) {
543 
544  std::vector < const moveit::core::LinkModel* > linksVector;
545  std::vector < const moveit::core::JointModel* > jointsVector;
546 
547  getRealDescendantLinkModelsRecursive ( actJoint->getChildLinkModel(), linksVector, actJoint, jointsVector );
548 
549  //now we have to look among the mimic joints, but the mimic of only joint passed as argument, not also the mimic of children joints
550  for (auto mimicJ : actJoint->getMimicRequests()) {
551  // but we do not look on mimic joints that are children of the this joint in the tree,
552  // because we have already explored them with recursion. Here we look only for mimic that are "cousins" of this joint
553  if (std::find (jointsVector.begin(), jointsVector.end(), mimicJ) == jointsVector.end() ) {
554  getRealDescendantLinkModelsRecursive (mimicJ->getChildLinkModel(), linksVector, mimicJ, jointsVector );
555  }
556  }
557 
558  descendantLinksOfJoint.insert (std::make_pair ( actJoint->getName(), linksVector ) );
559  descendantJointsOfJoint.insert (std::make_pair ( actJoint->getName(), jointsVector ) );
560  }
561 }
562 
564  const moveit::core::LinkModel* link, std::vector< const moveit::core::LinkModel* > & linksVect,
565  const moveit::core::JointModel* joint, std::vector< const moveit::core::JointModel* > & jointsVect ) const {
566 
567  linksVect.push_back (link) ;
568  jointsVect.push_back (joint);
569  auto childJoints = link->getChildJointModels();
570  if ( childJoints.size() == 0 ) {
571  return; //recursive base
572  }
573 
574  for (auto cj : childJoints) {
575  //recursion
576  getRealDescendantLinkModelsRecursive( cj->getChildLinkModel(), linksVect, cj, jointsVect );
577  }
578 
579 }
580 
581 
583 
584 
585  //we do not make urdf verification here, it is already done before by robot model loader of moveit,
586  //and also by Parser with parseUrdf
587  TiXmlDocument tiDoc;
588  tiDoc.Parse(xml.c_str());
589  //std::cout << robot_description << std::endl;
590  TiXmlElement* jointEl = tiDoc.FirstChildElement("robot")->FirstChildElement("joint") ;
591 
592  while (jointEl) {
593 
594  std::string jointName = jointEl->Attribute("name");
595  auto mimicEl = jointEl->FirstChildElement("mimic");
596  if (mimicEl) {
597  auto nlAttr = mimicEl->Attribute("nlFunPos");
598  if (nlAttr) {
599  //std::cout << jointName << std::endl;
600  //std::cout << nlAttr << std::endl;
601  //std::cout << mimicEl->Attribute("joint") << std::endl;
602  std::string fatherName = mimicEl->Attribute("joint");
603  mimicNLFatherOfJointMap.insert ( std::make_pair( jointName,
604  std::make_pair(fatherName, nlAttr)) );
605 
606  mimicNLJointsOfFatherMap[fatherName].insert(std::make_pair(jointName, nlAttr)) ;
607  }
608  }
609 
610  jointEl = jointEl->NextSiblingElement("joint");
611  }
612 
613 }
std::string getMimicNLJointOfFather(std::string mimicNLFatherName, std::string mimicNLJointName) const
std::string getFirstActuatedJointInFinger(std::string linkName) const
Given the linkName, this function returns the actuated joint that is a parent of this link and it is ...
std::string getFingerOfFingertip(std::string tipName) const
This function returns the name of the finger which the passed tipName belongs to. ...
std::vector< std::string > activeJointNames
Definition: ParserMoveIt.h:253
std::vector< std::string > fingertipNames
Definition: ParserMoveIt.h:252
std::vector< std::string > getGroupOfLink(std::string linkName)
This function explores all groups of srdf and says to which ones the linkName belongs to...
unsigned int nFingers
Definition: ParserMoveIt.h:258
std::map< std::string, std::string > getMimicNLJointsOfFather(std::string mimicNLFatherName) const
const robot_model::RobotModelPtr getRobotModel() const
the robot model can&#39;t be modified, if you want it to modify, use getCopyModel to get a copy...
std::vector< double > getSmallerBoundFromZero(std::string jointName) const
For each DOF of a joint, find the limit which is nearer from 0 position.
std::vector< std::string > getActiveJointNames() const
getter for all active (actuated) joints&#39; names.
std::map< std::string, std::vector< const moveit::core::LinkModel * > > getDescendantLinksOfJoint() const
getter for descendandsLinksOfJoint.
void lookForPassiveJoints()
This function looks for all passive joints, defined so in the srdf file.
bool checkIfContinuosJoint(const std::string jointName) const
check if the passed joint is continuos (i.e.
std::vector< double > getBiggerBoundFromZero(std::string jointName) const
For each DOF of a joint, find the limit which is farther from 0 position.
std::vector< std::string > getPassiveJointNames() const
getter for all the passive joints (defined in this way in the srdf file) Not all the not-actuated joi...
std::vector< const moveit::core::JointModel * > getActiveJointModels() const
getter for all active (actuated) joints.
std::map< std::string, std::string > fingerOfFingertipMap
The map with as key the name of the finger (defined in srdf file) and as value the fingertip (the las...
Definition: ParserMoveIt.h:274
std::map< std::string, std::string > fingertipOfFingerMap
The map with as key the name of the fingertip (the last (not virtual) link of a finger) and as value ...
Definition: ParserMoveIt.h:278
std::vector< const moveit::core::JointModel * > activeJointModels
Definition: ParserMoveIt.h:256
std::map< std::string, std::vector< const moveit::core::JointModel * > > getDescendantJointsOfJoint() const
getter for descendandsJointsOfJoint.
unsigned int getNExclusiveJointsOfTip(std::string tipName, bool continuosIncluded) const
Given a fingertip link, this function return the number of the joint that affect only the position of...
robot_model::RobotModelPtr robot_model
Definition: ParserMoveIt.h:251
void lookJointsTipsCorrelation()
Here, we find for each tip, which are all the joints (active) that can modifies its position It is ea...
std::map< std::string, std::pair< std::string, std::string > > mimicNLFatherOfJointMap
This map contain as key the name of the mimic joint which position follows a non linear relationship ...
Definition: ParserMoveIt.h:288
std::map< std::string, std::map< std::string, std::string > > getMimicNLJointsOfFatherMap() const
std::string getFirstActuatedParentJoint(std::string linkName, bool includeContinuos) const
starting from the given link, we explore the parents joint, until we found the first actuated...
void lookForDescendants()
Function to explore the kinematic tree from each actuated joint.
std::map< std::string, std::map< std::string, std::string > > mimicNLJointsOfFatherMap
inverse map of previous, even if the function is replicated, this is anyway useful, at the cost of having 2 copy of a string type.
Definition: ParserMoveIt.h:296
std::map< std::string, std::pair< std::string, std::string > > getMimicNLFatherOfJointMap() const
std::map< std::string, std::vector< std::string > > fingertipsOfJointMap
The map with as key the name of the actuated joint and as value all the fingertips which pose can be ...
Definition: ParserMoveIt.h:270
std::vector< std::string > passiveJointNames
Definition: ParserMoveIt.h:254
bool init(std::string robot_description, bool verbose=true)
Init the parser, fill the structures.
std::map< std::string, std::string > getFingerOfFingertipMap() const
void lookForActiveJoints()
This function look for all active joints in the model (i.e.
bool groupIsChain(const std::string groupName) const
check if a group (defined in srdf file) is a chain.
robot_model::RobotModelPtr getCopyModel() const
This function reload another model, same as the one loaded in init but this one can be modified exter...
std::string getFingertipOfFinger(std::string fingerName) const
This function returns the name of the fingertip that belongs to the passed fingerName.
std::pair< std::string, std::string > getMimicNLFatherOfJoint(std::string mimicNLJointName) const
gets for the maps of non linear mimic joints
std::map< std::string, std::vector< const moveit::core::JointModel * > > descendantJointsOfJoint
Map containing info about descendants joints of a joint see lookForDescendants function for more info...
Definition: ParserMoveIt.h:264
std::map< std::string, std::vector< std::string > > getJointsOfFingertipMap() const
unsigned int getNFingers() const
std::map< std::string, std::vector< const moveit::core::LinkModel * > > descendantLinksOfJoint
Map containing info about descendants links of a joint see lookForDescendants function for more info...
Definition: ParserMoveIt.h:261
void parseNonLinearMimicRelations(std::string xml)
std::string getHandName() const
void lookForFingertips(bool verbose=true)
This function explore the robot_model (which was built from urdf and srdf files), and fills the finge...
std::map< std::string, std::vector< std::string > > jointsOfFingertipMap
The map with as key the name of the fingertip and as value all the joints (actuated) that can modify ...
Definition: ParserMoveIt.h:267
std::map< std::string, std::vector< std::string > > getFingertipsOfJointMap() const
void getRealDescendantLinkModelsRecursive(const moveit::core::LinkModel *link, std::vector< const moveit::core::LinkModel * > &linksVect, const moveit::core::JointModel *joint, std::vector< const moveit::core::JointModel * > &jointsVect) const
Recursive function, support for lookForDescendants, to explore the urdf tree.
std::map< std::string, std::string > getFingertipOfFingerMap() const
std::string robot_description
Definition: ParserMoveIt.h:257
std::vector< std::string > getFingertipNames() const
std::string handName
Definition: ParserMoveIt.h:250