FindActions.h

Go to the documentation of this file.

#ifndef __ROSEE_FIND_ACTIONS_
#define __ROSEE_FIND_ACTIONS_

#include <moveit/planning_scene/planning_scene.h>

#include <end_effector/YamlWorker.h>
#include <end_effector/ParserMoveIt.h>
#include <end_effector/GraspingActions/Action.h>
#include <end_effector/GraspingActions/ActionPinchTight.h>
#include <end_effector/GraspingActions/ActionPinchLoose.h>
#include <end_effector/GraspingActions/ActionTrig.h>
#include <end_effector/GraspingActions/ActionSingleJointMultipleTips.h>
#include <end_effector/GraspingActions/ActionMultiplePinchTight.h>

#include <muParser.h>


#define N_EXP_COLLISION 5000 //5000 is ok
#define N_EXP_DISTANCES 5000 //? is ok
#define N_EXP_COLLISION_MULTPINCH 3000
#define DEFAULT_JOINT_POS 0.0

namespace ROSEE
{
    
class FindActions
{
public:
    FindActions ( std::shared_ptr < ROSEE::ParserMoveIt > parserMoveit ) ;
    
    std::pair <  std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchTight >, 
                 std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose >  > 
                 findPinch ( std::string path2saveYaml );
            
    std::map < std::set <std::string>, ROSEE::ActionMultiplePinchTight > findMultiplePinch (
        unsigned int nFinger, std::string path2saveYaml, bool strict = true );
                 
    std::map <std::string, ROSEE::ActionTrig> findTrig (  ROSEE::ActionPrimitive::Type actionType,
        std::string path2saveYaml );
    
    std::map < std::string, ROSEE::ActionSingleJointMultipleTips> findSingleJointMultipleTips ( unsigned int nFinger, std::string path2saveYaml );
    
private:
    
    std::shared_ptr < ROSEE::ParserMoveIt > parserMoveIt;
    
    //lets store this, we access at each setRandomPos
    std::map<std::string, std::pair<std::string, std::string>> mimicNLRelMap;
    
    std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchTight >  checkCollisions();
    
    void checkDistances (std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose >* mapOfLoosePinches);
    
    void checkWhichTipsCollideWithoutBounds (
        std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose >* mapOfLoosePinches);
    
    std::map <std::set<std::string>, ROSEE::ActionMultiplePinchTight>
        checkCollisionsForMultiplePinch(unsigned int nFinger, bool strict);

    void removeBoundsOfNotCollidingTips ( const std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose >* mapOfLoosePinches, 
                                          robot_model::RobotModelPtr kinematic_model_noBound);
    
    void fillNotCollidingTips ( std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose >* mapOfLoosePinches,
        const std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchTight >* mapOfPinches );
    
    void changeFingertipsToFingerNames ( 
        std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchTight >* mapOfPinches, 
        std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose >* mapOfLoosePinches) ;

    
    std::map <std::string, ActionTrig> trig();
    
    std::map <std::string, ROSEE::ActionTrig> tipFlex();
    
    std::map <std::string, ROSEE::ActionTrig> fingFlex();

    
    bool insertJointPosForTrigInMap ( std::map <std::string, ActionTrig>& trigMap, 
        ROSEE::ActionTrig action, std::string jointName, double trigValue);  


    
    ROSEE::JointsInvolvedCount setOnlyDependentJoints(std::pair < std::string, std::string > tipsNames, JointPos *jPos);
    
    
    ROSEE::JointsInvolvedCount setOnlyDependentJoints( std::set< std::string > tipsNames, 
                                                       JointPos *jPos);
    
    JointPos getConvertedJointPos(const robot_state::RobotState* kinematic_state);
    
    
    void setToDefaultPositionPassiveJoints(moveit::core::RobotState * kinematic_state);
    
    std::pair < std::string, std::string > getFingersPair (std::pair <std::string, std::string> tipsPair) const;
    
    std::set <std::string> getFingersSet (std::set <std::string> tipsSet) const;
    
    std::pair < std::string, std::string > getFingertipsPair (std::pair <std::string, std::string> fingersPair) const;

    void setToRandomPositions(robot_state::RobotState* kinematic_state);


};
    
}


#endif //__ROSEE_FIND_ACTIONS_