ParserMoveIt.h

Go to the documentation of this file.

/*
 * Copyright 2020 <copyright holder> <email>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 
#ifndef __ROSEE_PARSER_MOVEIT_H
#define __ROSEE_PARSER_MOVEIT_H
 
#include <string>
#include <vector>
#include <map>
#include <iostream>
 
#include <moveit/robot_model_loader/robot_model_loader.h>
 
//parse customized urdf with non linear mimic
#include <tinyxml.h>
 
 
namespace ROSEE {
class ParserMoveIt {
    
public:
    
    typedef std::shared_ptr<ParserMoveIt> Ptr;
    typedef std::shared_ptr<const ParserMoveIt> ConstPtr;
 
    ParserMoveIt();
    ~ParserMoveIt();
    
    bool init (std::string robot_description, bool verbose = true) ;
    std::string getHandName () const;
    unsigned int getNFingers () const ;
    std::vector <std::string> getFingertipNames () const; 
    
    std::vector <std::string> getActiveJointNames () const; 
    
    std::vector <const moveit::core::JointModel*> getActiveJointModels () const;
    
    std::vector <std::string> getPassiveJointNames () const;
    
    std::map <std::string, std::vector < const moveit::core::LinkModel* > > getDescendantLinksOfJoint() const ;
    
    std::map <std::string, std::vector < const moveit::core::JointModel* > > getDescendantJointsOfJoint() const ; 
 
    const robot_model::RobotModelPtr getRobotModel () const ;
    
    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;
    
    std::string getFingertipOfFinger (std::string fingerName) const;
 
    
    robot_model::RobotModelPtr getCopyModel ( ) const;
    
    std::vector < std::string > getGroupOfLink ( std::string linkName );
    
    bool groupIsChain ( const std::string groupName ) const;
    
    bool groupIsChain ( const moveit::core::JointModelGroup* group ) const;
    
    bool checkIfContinuosJoint ( const std::string jointName) const;
    bool checkIfContinuosJoint ( const moveit::core::JointModel* joint ) const;
    
    std::vector<double> getBiggerBoundFromZero ( std::string jointName ) const;
    std::vector<double> getBiggerBoundFromZero ( const moveit::core::JointModel* joint ) const;
    
    std::vector<double> getSmallerBoundFromZero ( std::string jointName ) const;
    std::vector<double> getSmallerBoundFromZero ( const moveit::core::JointModel* joint ) const;
    
    unsigned int getNExclusiveJointsOfTip (std::string tipName, bool continuosIncluded) const;
    
    std::string getFirstActuatedParentJoint ( std::string linkName, bool includeContinuos ) const;
    
    std::string getFirstActuatedJointInFinger (std::string linkName) const ;
    
    void parseNonLinearMimicRelations(std::string xml);
    
    std::pair<std::string, std::string> getMimicNLFatherOfJoint(std::string mimicNLJointName) const;
    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:
    
    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;
    
    std::map <std::string, std::vector < const moveit::core::JointModel* > > descendantJointsOfJoint;
    
    std::map<std::string, std::vector<std::string>> jointsOfFingertipMap;
    
    std::map<std::string, std::vector<std::string>> fingertipsOfJointMap;
    
    std::map<std::string, std::string> fingerOfFingertipMap;
    
    std::map<std::string, std::string> fingertipOfFingerMap;
    
    std::map<std::string, std::pair<std::string, std::string>>  mimicNLFatherOfJointMap;
    
    std::map<std::string, std::map<std::string, std::string>>  mimicNLJointsOfFatherMap;
 
    
    void lookForFingertips(bool verbose = true);
    
    
    void lookForActiveJoints();
    
    
    void lookForPassiveJoints();
    
    
    void lookJointsTipsCorrelation();
    
    void lookForDescendants();
    
    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;
    
                                                
 
        
 
 
};
 
} //namespace
 
#endif // __ROSEE_PARSER_MOVEIT_H