class to parse urdf and srdf with moveit classes and to give information about the model parsed More...

#include <ParserMoveIt.h>

Collaboration diagram for ROSEE::ParserMoveIt:

Public Types
typedef std::shared_ptr< ParserMoveIt >	Ptr

typedef std::shared_ptr< const ParserMoveIt >	ConstPtr

Public Member Functions
	ParserMoveIt ()

	~ParserMoveIt ()

bool	init (std::string robot_description, bool verbose=true)
	Init the parser, fill the structures. More...

std::string	getHandName () const

unsigned int	getNFingers () const

std::vector< std::string >	getFingertipNames () const

std::vector< std::string >	getActiveJointNames () const
	getter for all active (actuated) joints' names. More...

std::vector< const moveit::core::JointModel * >	getActiveJointModels () const
	getter for all active (actuated) joints. More...

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 joint are passive (a mimic joint can also be not defined as passive in srdf) More...

std::map< std::string, std::vector< const moveit::core::LinkModel * > >	getDescendantLinksOfJoint () const
	getter for descendandsLinksOfJoint. More...

std::map< std::string, std::vector< const moveit::core::JointModel * > >	getDescendantJointsOfJoint () const
	getter for descendandsJointsOfJoint. More...

const robot_model::RobotModelPtr	getRobotModel () const
	the robot model can't be modified, if you want it to modify, use getCopyModel to get a copy. More...

std::map< std::string, std::vector< std::string > >	getFingertipsOfJointMap () const

std::map< std::string, std::vector< std::string > >	getJointsOfFingertipMap () const

std::map< std::string, std::string >	getFingerOfFingertipMap () const

std::map< std::string, std::string >	getFingertipOfFingerMap () const

std::string	getFingerOfFingertip (std::string tipName) const
	This function returns the name of the finger which the passed `tipName` belongs to. More...

std::string	getFingertipOfFinger (std::string fingerName) const
	This function returns the name of the fingertip that belongs to the passed `fingerName`. More...

robot_model::RobotModelPtr	getCopyModel () const
	This function reload another model, same as the one loaded in init but this one can be modified externally, because it will not affect the internal structures of this class. More...

std::vector< std::string >	getGroupOfLink (std::string linkName)
	This function explores all groups of srdf and says to which ones the linkName belongs to. More...

bool	groupIsChain (const std::string groupName) const
	check if a group (defined in srdf file) is a chain. More...

bool	groupIsChain (const moveit::core::JointModelGroup *group) const
	check if a group (defined in srdf file) is a chain. More...

bool	checkIfContinuosJoint (const std::string jointName) const
	check if the passed joint is continuos (i.e. More...

bool	checkIfContinuosJoint (const moveit::core::JointModel *joint) const
	check if the passed joint is continuos (i.e. More...

std::vector< double >	getBiggerBoundFromZero (std::string jointName) const
	For each DOF of a joint, find the limit which is farther from 0 position. More...

std::vector< double >	getBiggerBoundFromZero (const moveit::core::JointModel *joint) const
	For each DOF of a joint, find the limit which is farther from 0 position. More...

std::vector< double >	getSmallerBoundFromZero (std::string jointName) const
	For each DOF of a joint, find the limit which is nearer from 0 position. More...

std::vector< double >	getSmallerBoundFromZero (const moveit::core::JointModel *joint) const
	For each DOF of a joint, find the limit which is nearer from 0 position. More...

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 this fingertip and not any other fingertips (obviously the joints can affect different other links that are not fingertips (e.g. More...

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

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 the first joint of the chain which the link belongs to. More...

void	parseNonLinearMimicRelations (std::string xml)

std::pair< std::string, std::string >	getMimicNLFatherOfJoint (std::string mimicNLJointName) const
	gets for the maps of non linear mimic joints More...

std::map< std::string, std::pair< std::string, std::string > >	getMimicNLFatherOfJointMap () const

std::string	getMimicNLJointOfFather (std::string mimicNLFatherName, std::string mimicNLJointName) const

std::map< std::string, std::string >	getMimicNLJointsOfFather (std::string mimicNLFatherName) const

std::map< std::string, std::map< std::string, std::string > >	getMimicNLJointsOfFatherMap () const

Private Member Functions
void	lookForFingertips (bool verbose=true)
	This function explore the robot_model (which was built from urdf and srdf files), and fills the fingerTipNames vector. More...

void	lookForActiveJoints ()
	This function look for all active joints in the model (i.e. More...

void	lookForPassiveJoints ()
	This function looks for all passive joints, defined so in the srdf file. More...

void	lookJointsTipsCorrelation ()
	Here, we find for each tip, which are all the joints (active) that can modifies its position It is easier to start from each joint and see which tips has as its descendands, because there is the getDescendantLinkModels() function in moveit that gives ALL the child links. More...

void	lookForDescendants ()
	Function to explore the kinematic tree from each actuated joint. More...

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

Private Attributes
std::string	handName

robot_model::RobotModelPtr	robot_model

std::vector< std::string >	fingertipNames

std::vector< std::string >	activeJointNames

std::vector< std::string >	passiveJointNames

std::vector< const moveit::core::JointModel * >	activeJointModels

std::string	robot_description

unsigned int	nFingers

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

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

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

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 modified by the joint. More...

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 last (not virtual) link of the joint) More...

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 the finger name (defined in the srdf) More...

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 with a father joint. More...

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

Detailed Description

class to parse urdf and srdf with moveit classes and to give information about the model parsed

Todo:: merge this with Parser class?

Definition at line 36 of file ParserMoveIt.h.

Member Typedef Documentation

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

Definition at line 41 of file ParserMoveIt.h.

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

Definition at line 40 of file ParserMoveIt.h.

Constructor & Destructor Documentation

ROSEE::ParserMoveIt::ParserMoveIt ( )

Definition at line 19 of file ParserMoveIt.cpp.

                                 {
 
 }

ROSEE::ParserMoveIt::~ParserMoveIt ( )

Definition at line 23 of file ParserMoveIt.cpp.

                                  {
 
 }

Member Function Documentation

bool ROSEE::ParserMoveIt::checkIfContinuosJoint ( const std::string jointName ) const

check if the passed joint is continuos (i.e.

a revolute one with sum of bounds greater than 2*PI)

Parameters

jointName the name of the joint

Returns: bool true if joint is continuos

Definition at line 244 of file ParserMoveIt.cpp.

                                                                          {
     if (robot_model == nullptr) {
         std::cerr << " [PARSER::" << __func__ << 
             "]: robot_model is null. Have you called init() before?"  << std::endl;
         return false;
     }
     return (ROSEE::ParserMoveIt::checkIfContinuosJoint(robot_model->getJointModel(jointName)));     
 }

bool ROSEE::ParserMoveIt::checkIfContinuosJoint ( const moveit::core::JointModel * joint ) const

check if the passed joint is continuos (i.e.

a revolute one with sum of bounds greater than 2*PI)

Parameters

joint pointer to the joint model

Returns: bool true if joint is continuos

Definition at line 253 of file ParserMoveIt.cpp.

                                                                                         {
     if (robot_model == nullptr) {
         std::cerr <<  " [PARSER::" << __func__ << 
             "]: robot_model is null. Have you called init() before?" << std::endl;
         return false;
     }
     
     // for moveit, a continuos joint is a revolute joint
     if (joint->getType() != moveit::core::JointModel::REVOLUTE ) {
         return false;
     }
     
     //if revolute, only one limit (at.(0))
     moveit::core::JointModel::Bounds limits = joint->getVariableBounds();
     if ( limits.at(0).max_position_ - limits.at(0).min_position_ >= (2*EIGEN_PI) ) {
         return true;
     }
     
     return false;
 }

std::vector< const moveit::core::JointModel * > ROSEE::ParserMoveIt::getActiveJointModels ( ) const

getter for all active (actuated) joints.

The analogous moveit function returns also the "passive" ones (defined in srdf), this one exclude the passive

Returns: vector of pointer of all joints that are actuated

Definition at line 73 of file ParserMoveIt.cpp.

                                                                                      {
     return activeJointModels;
 }

std::vector< std::string > ROSEE::ParserMoveIt::getActiveJointNames ( ) const

getter for all active (actuated) joints' names.

The analogous moveit function returns also the "passive" ones (defined in srdf)

Returns: vector of name of all joints that are actuated

Definition at line 69 of file ParserMoveIt.cpp.

                                                                   {
     return activeJointNames;
 }

std::vector< double > ROSEE::ParserMoveIt::getBiggerBoundFromZero ( std::string jointName ) const

For each DOF of a joint, find the limit which is farther from 0 position.

Parameters

jointName the name of the joint

Returns: std::vector<double> a vector (because joint can have more dofs) containing the values of the limits that are farther from 0

Definition at line 274 of file ParserMoveIt.cpp.

                                                                                          {
     if (robot_model == nullptr) {
         std::cerr << " [PARSER::" << __func__ << 
             "]: robot_model is null. Have you called init() before?" << std::endl;
         return std::vector<double>();
     }
     return ( ROSEE::ParserMoveIt::getBiggerBoundFromZero (robot_model->getJointModel(jointName) ) );
 
 }

std::vector< double > ROSEE::ParserMoveIt::getBiggerBoundFromZero ( const moveit::core::JointModel * joint ) const

For each DOF of a joint, find the limit which is farther from 0 position.

Parameters

joint pointer to the joint model

Returns: std::vector<double> a vector (because joint can have more dofs) containing the values of the limits that are farther from 0

Definition at line 284 of file ParserMoveIt.cpp.

                                                                                                        {
     if (robot_model == nullptr) {
         std::cerr << " [PARSER::" << __func__ << 
             "]: robot_model is null. Have you called init() before?" << std::endl;
         return std::vector<double>();
     }
     
     moveit::core::JointModel::Bounds limits = joint->getVariableBounds();
 
     std::vector <double> maxPos;
     for ( auto limit : limits ) {
         if ( std::abs(limit.max_position_) > std::abs(limit.min_position_)) {
             maxPos.push_back ( limit.max_position_ ) ;
         } else {
             maxPos.push_back ( limit.min_position_ ) ;
         }
     }
     return maxPos;
 }

robot_model::RobotModelPtr ROSEE::ParserMoveIt::getCopyModel ( ) const

This function reload another model, same as the one loaded in init but this one can be modified externally, because it will not affect the internal structures of this class.

Returns: robot_model::RobotModelPtr a pointer to a new robot model created

Definition at line 187 of file ParserMoveIt.cpp.

                                                                {
     robot_model_loader::RobotModelLoader robot_model_loader(robot_description); 
     return robot_model_loader.getModel();
 }

std::map< std::string, std::vector< const moveit::core::JointModel * > > ROSEE::ParserMoveIt::getDescendantJointsOfJoint ( ) const

getter for descendandsJointsOfJoint.

"Descendants" is intended in a slightly different way respect to moveit (see lookForDescendants doc)

Returns: map with keys the joint name and values a vector of pointer of all descendants joints

Definition at line 85 of file ParserMoveIt.cpp.

                                                                                                                     {
     return descendantJointsOfJoint;
 }

std::map< std::string, std::vector< const moveit::core::LinkModel * > > ROSEE::ParserMoveIt::getDescendantLinksOfJoint ( ) const

getter for descendandsLinksOfJoint.

"Descendants" is intended in a slightly different way respect to moveit (see lookForDescendants doc)

Returns: map with keys the joint name and values a vector of pointer of all descendants links

Definition at line 81 of file ParserMoveIt.cpp.

                                                                                                                   {
     return descendantLinksOfJoint;
 }

std::string ROSEE::ParserMoveIt::getFingerOfFingertip ( std::string tipName ) const

This function returns the name of the finger which the passed tipName belongs to.

Parameters

tipName the name of the tip

Returns: the name of the finger which the tip belongs to, empty string if the tipName is not in the map

Note: use getFingerOfFingertipMap to get the full map

Definition at line 109 of file ParserMoveIt.cpp.

                                                                           {
     
     auto it = fingerOfFingertipMap.find(tipName);
     
     if (it != fingerOfFingertipMap.end() ) {
         return (it->second);
         
     } else {
         return "";
     }
 }

std::map< std::string, std::string > ROSEE::ParserMoveIt::getFingerOfFingertipMap ( ) const

Definition at line 105 of file ParserMoveIt.cpp.

                                                                                 {
     return fingerOfFingertipMap;
 }

std::vector< std::string > ROSEE::ParserMoveIt::getFingertipNames ( ) const

Definition at line 65 of file ParserMoveIt.cpp.

                                                                 {
     return fingertipNames;
 }

std::string ROSEE::ParserMoveIt::getFingertipOfFinger ( std::string fingerName ) const

This function returns the name of the fingertip that belongs to the passed fingerName.

Parameters

fingerName the name of the tip

Returns: the name of the fingertip that belongs to the finger, empty string if the fingerName is not in the map

Note: use getFingertipOfFingerMap to get the full map

Definition at line 125 of file ParserMoveIt.cpp.

                                                                              {
     
     auto it = fingertipOfFingerMap.find(fingerName);
     
     if (it != fingertipOfFingerMap.end() ) {
         return (it->second);
         
     } else {
         return "";
     }
 }

std::map< std::string, std::string > ROSEE::ParserMoveIt::getFingertipOfFingerMap ( ) const

Definition at line 121 of file ParserMoveIt.cpp.

                                                                                 {
     return fingertipOfFingerMap;
 }

std::map< std::string, std::vector< std::string > > ROSEE::ParserMoveIt::getFingertipsOfJointMap ( ) const

Definition at line 97 of file ParserMoveIt.cpp.

                                                                                           {
     return fingertipsOfJointMap;
 }

std::string ROSEE::ParserMoveIt::getFirstActuatedJointInFinger ( std::string linkName ) const

Given the linkName, this function returns the actuated joint that is a parent of this link and it is the first joint of the chain which the link belongs to.

E.G. given a fingertip, it returns the first actuated joint of the finger (e.g. the joint that moves the firsts phalanges, if present) From the linkName given, this function explores the parent links until we found a link with more than 1 child joint. This means that we have found the "base" of the kinematic chain, so we can go forward to find the first actuated joint

Parameters

linkName the name of the link which is part of the chain where we have to look in

Returns: std::string the name of the wanted joint

Definition at line 409 of file ParserMoveIt.cpp.

                                                                                     {
     const moveit::core::LinkModel* linkModel = robot_model->getLinkModel(linkName);
     const moveit::core::JointModel* joint;
 
     // we stop when the link has more than 1 joint: so linkModel will be the parent of the first 
     // link of the finger group and in joint we have stored the actuated (not continuos) 
     // child joint most near to linkModel
     while ( (linkModel != NULL) && (linkModel->getChildJointModels().size() < 2) ) {
         
         //if the parent joint is an actuated (not cont) joint, store it (or overwrite the previous stored)
         if ( linkModel->getParentJointModel()->getMimic() == NULL && 
             (!linkModel->parentJointIsFixed()) &&
             (!linkModel->getParentJointModel()->isPassive()) &&
             (!checkIfContinuosJoint(linkModel->getParentJointModel() ))  ) {
             
             joint = linkModel->getParentJointModel();
         }
         
         linkModel = linkModel->getParentLinkModel();
     }
     
     return joint->getName();
 }

std::string ROSEE::ParserMoveIt::getFirstActuatedParentJoint	(	std::string	linkName,
		bool	includeContinuos
	)		const

starting from the given link, we explore the parents joint, until we found the first actuated.

This ones will be the interesting joint

Parameters

linkName	the name of the link for which look for the parent joint
includeContinuos	a bool set to true if we want to include continuos joint in the search

Returns: std::string the name of the first actuated parent joint

Definition at line 379 of file ParserMoveIt.cpp.

                                                                                                            {
 
     const moveit::core::LinkModel* linkModel = robot_model->getLinkModel ( linkName ) ;
 
     while ( linkModel->getParentJointModel()->getMimic() != NULL || 
             linkModel->parentJointIsFixed() ||
             linkModel->getParentJointModel()->isPassive() || 
             (!includeContinuos && checkIfContinuosJoint(linkModel->getParentJointModel())) ) {
         
         //an active joint is not any of these condition.
         //passive is an attribute of the joint in the srdf, so it may be not setted 
         // (default is not passive), so we need also the getMimic == NULL 
         // (ie: an actuated joint dont mimic anything)
         //WARNING these 4 conditions should be enough I think
         
         linkModel = linkModel->getParentLinkModel();
         
         if (linkModel == NULL ) {
         
             std::cerr << " [PARSER::" << __func__ << 
                 "]: Strange error: fingertipsOfJointMap, jointsOfFingertipMap, and/or other structures " <<
                 "may have been built badly"  << std::endl ;
             return "";
         }
     }
 
     return (linkModel->getParentJointModel()->getName());
 }

std::vector< std::string > ROSEE::ParserMoveIt::getGroupOfLink ( std::string linkName )

This function explores all groups of srdf and says to which ones the linkName belongs to.

Returns a vector because a link can be part of more group.

Parameters

linkName the name on the link for which look the group

Returns: std::vector < std::string > a vector containing the name of all the groups which contain the linkName

Definition at line 192 of file ParserMoveIt.cpp.

                                                                                { 
     
     std::vector < std::string > groupsReturn;
 
     if (robot_model == nullptr) {
         std::cerr << " [PARSER::" << __func__ << 
             "]: robot_model is null. Have you called init() before?"  << std::endl;
         return groupsReturn;
     }
     
     for (auto group : robot_model->getJointModelGroups() ) {
         
         if ( group->hasLinkModel(linkName) ) {
                 
             groupsReturn.push_back ( group->getName() ) ;
         }
     }
     return groupsReturn;
 }

std::string ROSEE::ParserMoveIt::getHandName ( ) const

Definition at line 61 of file ParserMoveIt.cpp.

                                                {
     return handName;
 }

std::map< std::string, std::vector< std::string > > ROSEE::ParserMoveIt::getJointsOfFingertipMap ( ) const

Definition at line 101 of file ParserMoveIt.cpp.

                                                                                           {
     return jointsOfFingertipMap;
 }

std::pair< std::string, std::string > ROSEE::ParserMoveIt::getMimicNLFatherOfJoint ( std::string mimicNLJointName ) const

gets for the maps of non linear mimic joints

Definition at line 137 of file ParserMoveIt.cpp.

                                                                                                          {
     
     std::pair<std::string, std::string> retPair;
     
     auto it = mimicNLFatherOfJointMap.find(mimicNLJointName);
     
     if (it != mimicNLFatherOfJointMap.end()) {
         retPair = it->second;
     }
     return retPair;
 }

std::map< std::string, std::pair< std::string, std::string > > ROSEE::ParserMoveIt::getMimicNLFatherOfJointMap ( ) const

Definition at line 149 of file ParserMoveIt.cpp.

                                                                                                      {
     
     return mimicNLFatherOfJointMap;
     
 }

std::string ROSEE::ParserMoveIt::getMimicNLJointOfFather	(	std::string	mimicNLFatherName,
		std::string	mimicNLJointName
	)		const

Definition at line 155 of file ParserMoveIt.cpp.

                                                                                                                   {
     
     auto map = getMimicNLJointsOfFather(mimicNLFatherName);
     
     auto it = map.find(mimicNLJointName);
     
     if (it != map.end()) {
         return it->second;
     }
     
     return "";
 }

std::map< std::string, std::string > ROSEE::ParserMoveIt::getMimicNLJointsOfFather ( std::string mimicNLFatherName ) const

Definition at line 168 of file ParserMoveIt.cpp.

                                                                                                           {
     
     std::map<std::string, std::string> map;
     
     auto it = mimicNLJointsOfFatherMap.find(mimicNLFatherName);
     
     if (it != mimicNLJointsOfFatherMap.end()) {
         map = it->second;
     }
     return map;
 }

std::map< std::string, std::map< std::string, std::string > > ROSEE::ParserMoveIt::getMimicNLJointsOfFatherMap ( ) const

Definition at line 180 of file ParserMoveIt.cpp.

                                                                                                      {
     
     return mimicNLJointsOfFatherMap;
     
 }

unsigned int ROSEE::ParserMoveIt::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 this fingertip and not any other fingertips (obviously the joints can affect different other links that are not fingertips (e.g.

middles phalanges) )

Parameters

tipName	the name of the fingertip link
continuosIncluded	a bool set to true if we want to count also the possible present continuos joints

Returns: unsigned int the number of the exclusive joints

Definition at line 334 of file ParserMoveIt.cpp.

                                                                                                            {
     
     if (jointsOfFingertipMap.size() == 0) {
         std::cerr << " [PARSER::" << __func__ << 
             "]: jointsOfFingertipMap empty. Have you called init() before? Also check your URDF and SRDF files" 
             << std::endl;
         return 0;
     }
     
     if (fingertipsOfJointMap.size() == 0) {
         std::cerr << " [PARSER::" << __func__ << 
             "]: jointsOfFingertipMap empty. Have you called init() before? Also check your URDF and SRDF files" 
             << std::endl;
         return 0;
     }
     
     unsigned int nExclusiveJoints = 0;
 
     for (auto jointOfTip : jointsOfFingertipMap.find(tipName)->second ) { 
 
         if ( !continuosIncluded && checkIfContinuosJoint(jointOfTip) == true ) {
             continue; //we dont want to count a continuos joint
         }
     
         //check if the joints of the tip move only that tip
         if ( fingertipsOfJointMap.find(jointOfTip)->second.size() == 1 ) {
 
             if (fingertipsOfJointMap.find(jointOfTip)->second.at(0).compare (tipName) != 0) {
                 //  this condition is false if jointsOfFingertipMap and fingertipsOfJointMap are built well
                 std::cerr << " [PARSER::" << __func__ << 
                 "]: Strange error in fingertipsOfJointMap and jointsOfFingertipMap: they are not consistent: " 
                 << "The unique tip present in the map for the key " << jointOfTip 
                 << " is " << fingertipsOfJointMap.find(jointOfTip)->second.at(0) 
                 << " and not " << tipName << " as it should be" 
                 << std::endl;
                 return 0;  
             }
             
             nExclusiveJoints++;
         }
     }
     return nExclusiveJoints;  
 }

unsigned int ROSEE::ParserMoveIt::getNFingers ( ) const

Definition at line 89 of file ParserMoveIt.cpp.

                                                    {
     return nFingers;
 }

std::vector< std::string > ROSEE::ParserMoveIt::getPassiveJointNames ( ) const

getter for all the passive joints (defined in this way in the srdf file) Not all the not-actuated joint are passive (a mimic joint can also be not defined as passive in srdf)

Returns: std::vector <std::string> string with names of all passive joints

Definition at line 77 of file ParserMoveIt.cpp.

                                                                    {
     return passiveJointNames;
 }

void ROSEE::ParserMoveIt::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

private

Recursive function, support for lookForDescendants, to explore the urdf tree.

Parameters

link	pointer to the actual link that is being explored
linksVect	[out] vector of explored links are stored here at each iteration
joint	pointer to the actual joint thaqt is being explored (father of `link`, each joint has always one and only one (direct) child link)
jointsVect	[out] vector of explored joints are stored here at each iteration

Definition at line 563 of file ParserMoveIt.cpp.

                                                                                                       {
     
     linksVect.push_back (link) ;
     jointsVect.push_back (joint);
     auto childJoints = link->getChildJointModels();
     if ( childJoints.size() == 0 ) { 
         return; //recursive base
     }
     
     for (auto cj : childJoints) {
         //recursion
         getRealDescendantLinkModelsRecursive( cj->getChildLinkModel(), linksVect, cj, jointsVect );
     }
     
 }

const robot_model::RobotModelPtr ROSEE::ParserMoveIt::getRobotModel ( ) const

the robot model can't be modified, if you want it to modify, use getCopyModel to get a copy.

Returns: const robot_model::RobotModelPtr a shared pointer to the robot model

Definition at line 93 of file ParserMoveIt.cpp.

                                                                        {
     return robot_model;
 }

std::vector< double > ROSEE::ParserMoveIt::getSmallerBoundFromZero ( std::string jointName ) const

For each DOF of a joint, find the limit which is nearer from 0 position.

Parameters

jointName the name of the joint

Returns: std::vector<double> a vector (because joint can have more dofs) containing the values of the limits that are nearer from 0

Definition at line 304 of file ParserMoveIt.cpp.

                                                                                           {
     if (robot_model == nullptr) {
         std::cerr << " [PARSER::" << __func__ << 
             "]: robot_model is null. Have you called init() before?" << std::endl;
         return std::vector<double>();
     }
     return ( ROSEE::ParserMoveIt::getSmallerBoundFromZero (robot_model->getJointModel(jointName) ) );
 
 }

std::vector< double > ROSEE::ParserMoveIt::getSmallerBoundFromZero ( const moveit::core::JointModel * joint ) const

For each DOF of a joint, find the limit which is nearer from 0 position.

Parameters

joint pointer to the joint model

Returns: std::vector<double> a vector (because joint can have more dofs) containing the values of the limits that are nearer from 0

Definition at line 314 of file ParserMoveIt.cpp.

                                                                                                         {
     if (robot_model == nullptr) {
         std::cerr << " [PARSER::" << __func__ << 
             "]: robot_model is null. Have you called init() before?" << std::endl;
         return std::vector<double>();
     }
     
     moveit::core::JointModel::Bounds limits = joint->getVariableBounds();
 
     std::vector <double> minPos;
     for ( auto limit : limits ) {
         if ( std::abs(limit.max_position_) < std::abs(limit.min_position_)) {
             minPos.push_back ( limit.max_position_ ) ;
         } else {
             minPos.push_back ( limit.min_position_ ) ;
         }
     }
     return minPos;
 }

bool ROSEE::ParserMoveIt::groupIsChain ( const std::string groupName ) const

check if a group (defined in srdf file) is a chain.

See groupIsChain ( moveit::core::JointModelGroup* group )

Parameters

groupName the name of the group

Returns: bool : true is group is chain, false othwerwise (also false if some errors happened)

Definition at line 212 of file ParserMoveIt.cpp.

                                                                     {
     
     if (robot_model == nullptr) {
         std::cerr << " [PARSER::" << __func__ << 
             "]: robot_model is null. Have you called init() before?"  << std::endl;
         return false;
     }
     
     if (! robot_model->hasJointModelGroup(groupName) ) {
         std::cerr << " [PARSER::" << __func__ << 
             "]: " << groupName << " is not a group "  << std::endl;
         return false;
     }
     return groupIsChain(robot_model->getJointModelGroup(groupName));
 }

bool ROSEE::ParserMoveIt::groupIsChain ( const moveit::core::JointModelGroup * group ) const

check if a group (defined in srdf file) is a chain.

This is done simply exploring all the links of the group: if no links have more than 1 children, the group is a chain. Moveit has a function isChain() but I don't understand how it works, in fact also if there is a link with more children joint sometimes the group is considered a chain anyway. I can't find where the moveit _is_chain member is set.

Parameters

group pointer to moveit::core::JointModelGroup object

Returns: bool : true is group is chain, false othwerwise (also false if some errors happened)

Definition at line 228 of file ParserMoveIt.cpp.

                                                                                  {
     
     std::stringstream log;
     log << "Checking if " << group->getName() << " is a chain ..." << std::endl;
     for (auto link : group->getLinkModels() ){
         if (link->getChildJointModels().size() > 1 ) {
             log << "... no because " << link->getName() << " has " << link->getChildJointModels().size() << " children " << std::endl;
            // std::cout << log.str() << std::endl;
             return false;
         }
     }
     return true;
 }

bool ROSEE::ParserMoveIt::init	(	std::string	robot_description,
		bool	verbose = `true`
	)

Init the parser, fill the structures.

Parameters

robot_description	a string containing the name given to the param set in ROS server for the urdf file. The srdf file must have the same param name but with a trailing "_semantic"
verbose	pass false to reduce the quantity of informative prints

Definition at line 27 of file ParserMoveIt.cpp.

                                                                          {
     
     if (robot_model != nullptr ) {
         std::cerr << "[PARSER::"  << __func__ << "]: init() already called by someone " << std::endl;;
         return false;
     }
     // it is a ros param in the launch, take care that also sdrf is read by robot_mo
     // (param for srd is robot_description_semantic)
     this->robot_description = robot_description;
     
     //false: we dont need kinematic solvers now
     robot_model_loader::RobotModelLoader robot_model_loader(robot_description, false) ; 
     robot_model = robot_model_loader.getModel() ;
     if (robot_model == nullptr) {
         std::cerr << " [PARSER::" << __func__ << 
             "]: Fail To load robot model " << robot_description <<
             " are you sure to have put both urdf and srdf files in the parameter server " <<
             "with names robot_description and robot_description_semantic, respectively?" << std::endl; 
         return false;
     }
     std::cout << "[PARSER::" << __func__ << "]: Parsed Model: " << robot_model->getName() << std::endl; ;
     
     handName = robot_model->getName();
     
     lookForFingertips(verbose);
     lookForActiveJoints();
     lookForPassiveJoints();
     lookForDescendants();
     lookJointsTipsCorrelation();
     
     return true;
     
 }

void ROSEE::ParserMoveIt::lookForActiveJoints ( )

private

This function look for all active joints in the model (i.e.

not mimic, not fixed, not passive) There exist a moveit function getActiveJointModels() which return all not mimic and not fixed, but it can return also passive joints (a info that is stored in srdf file). So this function also check if the joint is not passive It stores both the joint names and pointer to joint models in two private vector (activeJointNames and activeJointModels)

Definition at line 493 of file ParserMoveIt.cpp.

                                             { 
     
     for (auto joint : robot_model->getActiveJointModels() ) { 
         // robot_model->getActiveJointModels() returns not fixed not mimic but CAN return PASSIVE joints
         if (! joint->isPassive() ) {
             activeJointNames.push_back(joint->getName());
             activeJointModels.push_back(joint);
         }
     }
 }

void ROSEE::ParserMoveIt::lookForDescendants ( )

private

Function to explore the kinematic tree from each actuated joint.

It stores each descendants links and joints in two maps (descendantLinksOfJoint and descendantJointsOfJoint) where the key is the name of the joint and the value a vector of descendandts. The tree is explored recursively thanks to getRealDescendantLinkModelsRecursive support function

Note: Moveit has its own getDescendantLinkModels and getDescendandtsJoint model but it not suitable for us: those one store also the sons of joints that mimic the sons joint of the initial one (the map's key). So in this way we include also some links that may not move when we move the joint. These "errors" happens with schunk, softhand and robotiq for example; The descendants are found only for the actuated joints (not fixed, not mimic, not passive).

Definition at line 540 of file ParserMoveIt.cpp.

                                             {
     
     for (auto actJoint : activeJointModels) {
         
         std::vector < const moveit::core::LinkModel* >  linksVector;      
         std::vector < const moveit::core::JointModel* >  jointsVector;   
         
         getRealDescendantLinkModelsRecursive ( actJoint->getChildLinkModel(), linksVector, actJoint, jointsVector );
 
         //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
         for (auto mimicJ : actJoint->getMimicRequests()) {
         // but we do not look on mimic joints that are children of the this joint in the tree,
         //  because we have already explored them with recursion. Here we look only for mimic that are "cousins" of this joint
             if (std::find (jointsVector.begin(), jointsVector.end(), mimicJ) == jointsVector.end() ) {
                 getRealDescendantLinkModelsRecursive (mimicJ->getChildLinkModel(), linksVector, mimicJ, jointsVector );  
             }
         }
         
         descendantLinksOfJoint.insert (std::make_pair ( actJoint->getName(), linksVector ) );
         descendantJointsOfJoint.insert (std::make_pair ( actJoint->getName(), jointsVector ) );
     }
 }

void ROSEE::ParserMoveIt::lookForFingertips ( bool verbose = true )

private

This function explore the robot_model (which was built from urdf and srdf files), and fills the fingerTipNames vector.

In particular, the function explores only the groups specified in the srdf, and prints infos about each link it finds (eg. not a fingertin, not a chain group, and so on). A fingertip is a link with the following conditions:

It is part of a group (defined in the srdf)
The group which the tip is part of is a chain (not a tree)
It is the last link of the group AND has a mesh or some visual geometry (if not, it is probably a virtual link). If the second condition is not valid, it is taken the last link of the group that has a mesh (so it will not be a "leaf")
Parameters

verbose set it to false to not print explored hand info

Warning
Only link belonging to a group are explored (and printed), so other links (if present) are not considered

Definition at line 436 of file ParserMoveIt.cpp.

                                                       {
      for (auto it: robot_model->getJointModelGroups()) {
         
         std::string logGroupInfo;
         logGroupInfo = "[PARSER::" + std::string(__func__) + "] Found Group '" + it->getName() + "', " ;
         
         if (it->getSubgroupNames().size() != 0 ) {
             logGroupInfo.append("but it has some subgroups \n");
             
         } else if (! groupIsChain ( it ) ) { 
             logGroupInfo.append("but it is not a chain \n");
         
         } else if (it->getLinkModels().size() == 0) { 
             logGroupInfo.append("but it has 0 links \n");
 
         } else {
 
             logGroupInfo.append("with links: \n");
             
             std::string theTip = ""; //the last link with a visual geometry
             for (auto itt : it->getLinkModels()) {
                 
                 logGroupInfo.append("\t'" + itt->getName() + "' ");
                 
                 if (itt->getChildJointModels().size() != 0) {
                     logGroupInfo.append("(not a leaf link) ");
                 } else {
                     logGroupInfo.append("(a leaf link) ");
                 }
                 
                 if (itt->getShapes().size() == 0 ) {
                     logGroupInfo.append("(no visual geometry) ");
                     
                 } else {
                     theTip = itt->getName();
                 }
                 logGroupInfo.append("\n");
 
             }
             
             if (theTip.compare("") == 0) {
                 logGroupInfo.append("Warning: No link has a mesh in this group\n");
                 
             } else {
                 fingertipNames.push_back(theTip);
                 fingerOfFingertipMap.insert( std::make_pair(theTip, it->getName()));
                 fingertipOfFingerMap.insert( std::make_pair(it->getName(), theTip));
             }
         }
         
         if (verbose) {
             std::cout << logGroupInfo << std::endl;
         }
     }
     nFingers = fingertipNames.size();
 }

void ROSEE::ParserMoveIt::lookForPassiveJoints ( )

private

This function looks for all passive joints, defined so in the srdf file.

Definition at line 504 of file ParserMoveIt.cpp.

                                              { 
     
     for (auto joint : robot_model->getJointModels() ) { 
         if ( joint->isPassive() ) {
             passiveJointNames.push_back(joint->getName());
         }
     }
 }

void ROSEE::ParserMoveIt::lookJointsTipsCorrelation ( )

private

Here, we find for each tip, which are all the joints (active) that can modifies its position It is easier to start from each joint and see which tips has as its descendands, because there is the getDescendantLinkModels() function in moveit that gives ALL the child links.

There is not a function like getNonFixedParentJointModels from the tip, there is only the one to take the FIRST parent joint (getParentJointModel()) Meanwhile, we find also, for each joint, all the tips that are influenced by the joint movement: fingertipsOfJointMap

Definition at line 514 of file ParserMoveIt.cpp.

                                                   {
     
     //initialize the map with all tips and with empty vectors of its joints
     for (const auto &it: fingertipNames) { 
         jointsOfFingertipMap.insert ( std::make_pair (it, std::vector<std::string>() ) );
     }
     
     //initialize the map with all the actuated joints and an empty vector for the tips that the joint move
     for (const auto &it: activeJointNames) { 
         fingertipsOfJointMap.insert ( std::make_pair (it, std::vector<std::string>() ) );
     }
     
     for ( const auto &jointLink: descendantLinksOfJoint){ //for each actuated joint   
 
         for (const auto &link : jointLink.second) { //for each descendant link
 
             //if link is a tip...
             if (std::find(fingertipNames.begin(), fingertipNames.end(), link->getName()) != fingertipNames.end()){
                 jointsOfFingertipMap.at ( link->getName() ) .push_back( jointLink.first);
                 fingertipsOfJointMap.at ( jointLink.first ) .push_back( link->getName() );
             }
         }
     }
 
 }

void ROSEE::ParserMoveIt::parseNonLinearMimicRelations ( std::string xml )

Todo:: make docs as convention, in the equation there must exist only the variable x, that is, the position of the father joint

Definition at line 582 of file ParserMoveIt.cpp.

                                                                    {
         
         
     //we do not make urdf verification here, it is already done before by robot model loader of moveit,
     //and also by Parser with parseUrdf
     TiXmlDocument tiDoc;
     tiDoc.Parse(xml.c_str());
     //std::cout << robot_description << std::endl;
     TiXmlElement* jointEl = tiDoc.FirstChildElement("robot")->FirstChildElement("joint") ;
             
     while (jointEl) {
         
         std::string jointName = jointEl->Attribute("name");
         auto mimicEl = jointEl->FirstChildElement("mimic");
         if (mimicEl) {
             auto nlAttr = mimicEl->Attribute("nlFunPos");
             if (nlAttr) {
                 //std::cout << jointName << std::endl;
                 //std::cout << nlAttr << std::endl;
                 //std::cout << mimicEl->Attribute("joint") << std::endl;
                 std::string fatherName = mimicEl->Attribute("joint");
                 mimicNLFatherOfJointMap.insert ( std::make_pair( jointName,
                                                         std::make_pair(fatherName, nlAttr)) );
                 
                 mimicNLJointsOfFatherMap[fatherName].insert(std::make_pair(jointName, nlAttr)) ;
             }
         }
         
         jointEl = jointEl->NextSiblingElement("joint");
     }                
     
 }

Member Data Documentation

std::vector<const moveit::core::JointModel*> ROSEE::ParserMoveIt::activeJointModels

private

Definition at line 256 of file ParserMoveIt.h.

std::vector<std::string> ROSEE::ParserMoveIt::activeJointNames

private

Definition at line 253 of file ParserMoveIt.h.

std::map<std::string, std::vector < const moveit::core::JointModel* > > ROSEE::ParserMoveIt::descendantJointsOfJoint

private

Map containing info about descendants joints of a joint see lookForDescendants function for more info.

Definition at line 264 of file ParserMoveIt.h.

std::map<std::string, std::vector < const moveit::core::LinkModel* > > ROSEE::ParserMoveIt::descendantLinksOfJoint

private

Map containing info about descendants links of a joint see lookForDescendants function for more info.

Definition at line 261 of file ParserMoveIt.h.

std::map<std::string, std::string> ROSEE::ParserMoveIt::fingerOfFingertipMap

private

The map with as key the name of the finger (defined in srdf file) and as value the fingertip (the last (not virtual) link of the joint)

Definition at line 274 of file ParserMoveIt.h.

std::vector<std::string> ROSEE::ParserMoveIt::fingertipNames

private

Definition at line 252 of file ParserMoveIt.h.

std::map<std::string, std::string> ROSEE::ParserMoveIt::fingertipOfFingerMap

private

The map with as key the name of the fingertip (the last (not virtual) link of a finger) and as value the finger name (defined in the srdf)

Definition at line 278 of file ParserMoveIt.h.

std::map<std::string, std::vector<std::string> > ROSEE::ParserMoveIt::fingertipsOfJointMap

private

The map with as key the name of the actuated joint and as value all the fingertips which pose can be modified by the joint.

Definition at line 270 of file ParserMoveIt.h.

std::string ROSEE::ParserMoveIt::handName

private

Definition at line 250 of file ParserMoveIt.h.

std::map<std::string, std::vector<std::string> > ROSEE::ParserMoveIt::jointsOfFingertipMap

private

The map with as key the name of the fingertip and as value all the joints (actuated) that can modify its pose.

Definition at line 267 of file ParserMoveIt.h.

std::map<std::string, std::pair<std::string, std::string> > ROSEE::ParserMoveIt::mimicNLFatherOfJointMap

private

This map contain as key the name of the mimic joint which position follows a non linear relationship with a father joint.

As value there is a pair: first element is the name of the father joint, second element is the non linear equation

as convention, in the equation there must exist only the variable x, that is, the position of the father joint

Definition at line 288 of file ParserMoveIt.h.

std::map<std::string, std::map<std::string, std::string> > ROSEE::ParserMoveIt::mimicNLJointsOfFatherMap

private

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.

the key is the name of the father, the value is a map because more than one child can mimic the father (In the other map, there is a pair)

Definition at line 296 of file ParserMoveIt.h.

unsigned int ROSEE::ParserMoveIt::nFingers

private

Definition at line 258 of file ParserMoveIt.h.

std::vector<std::string> ROSEE::ParserMoveIt::passiveJointNames

private

Definition at line 254 of file ParserMoveIt.h.

std::string ROSEE::ParserMoveIt::robot_description

private

Definition at line 257 of file ParserMoveIt.h.

robot_model::RobotModelPtr ROSEE::ParserMoveIt::robot_model

private

Definition at line 251 of file ParserMoveIt.h.

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

/home/travis/build/ADVRHumanoids/ROSEndEffector/include/end_effector/ParserMoveIt.h
/home/travis/build/ADVRHumanoids/ROSEndEffector/src/ParserMoveIt.cpp

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation