ROSEE::ActionPinchTight Class Reference

The action of pinch with two tips. More...

#include <ActionPinchTight.h>

Inheritance diagram for ROSEE::ActionPinchTight:

Collaboration diagram for ROSEE::ActionPinchTight:

Classes
struct	depthComp
	struct to put in order the actionStates. More...

Public Types
typedef std::map< std::pair< std::string, std::string >, ActionPinchTight >	Map
typedef std::pair< JointPos, collision_detection::Contact >	StateWithContact
	A pair to "link" the JointPos with infos about the collision among the two tips. More...
Public Types inherited from ROSEE::ActionPrimitive
enum	Type {   PinchTight,   PinchLoose,   MultiplePinchTight,   Trig,   TipFlex,   FingFlex,   SingleJointMultipleTips,   None }
	Enum useful to discriminate each primitive action when, for example, we want to parse a file if you change this enum, change also the ROSEEControl.msg accordingly. More...
typedef std::shared_ptr< ActionPrimitive >	Ptr
typedef std::shared_ptr< const ActionPrimitive >	ConstPtr
Public Types inherited from ROSEE::Action
enum	Type {   Primitive,   Generic,   Composed,   Timed,   None }
	Enum useful to discriminate each action when, for example, we want to parse a file if you change this enum, change also the ROSEEControl.msg accordingly. More...
typedef std::shared_ptr< Action >	Ptr
typedef std::shared_ptr< const Action >	ConstPtr

Public Member Functions
	ActionPinchTight ()
	ActionPinchTight (unsigned int maxStoredActionStates)
	ActionPinchTight (std::pair< std::string, std::string >, JointPos, collision_detection::Contact)
	ActionPinchTight (std::string finger1, std::string finger2, JointPos, collision_detection::Contact)
JointPos	getJointPos () const override
	Get the position related to this action. More...
JointPos	getJointPos (unsigned int index) const
std::vector< ROSEE::JointPos >	getAllJointPos () const override
	Return all the joint position stored. More...
std::vector< ROSEE::ActionPinchTight::StateWithContact >	getActionStates () const
	Specific get for the ActionPinchTight to return the state with contact info. More...
bool	insertActionState (JointPos, collision_detection::Contact)
	function to insert a single action in the actionStates set of possible action. More...
void	print () const override
	For the pinch, we override these function to print, emit and parse the optional info Contact, which is specific of the pinch. More...
void	emitYaml (YAML::Emitter &) const override
	Function to fill the argument passed with info about the action. More...
bool	fillFromYaml (YAML::const_iterator yamlIt) override
	function to fill members of the Action with infos taken from yaml files More...
Public Member Functions inherited from ROSEE::ActionPinchGeneric
	ActionPinchGeneric (std::string name, ActionPrimitive::Type type)
	KNOW the distance at a certain percentage? so se volgio prendere un oggetto largo 3 cm il programma sa quale è la percentage per avere una distanza di 3cm More...
	ActionPinchGeneric (std::string name, unsigned int maxStoredActionStates, ActionPrimitive::Type type)
	ActionPinchGeneric (std::string name, unsigned int nFingerInvolved, unsigned int maxStoredActionStates, ActionPrimitive::Type type)
std::set< std::string >	getKeyElements () const override
	Necessary method to know the key used by the maps which store all the Actions of one type. More...
Public Member Functions inherited from ROSEE::ActionPrimitive
virtual	~ActionPrimitive ()
Type	getPrimitiveType () const
unsigned int	getMaxStoredActionStates () const
unsigned int	getnFingersInvolved () const
void	setJointsInvolvedCount (ROSEE::JointsInvolvedCount jointsInvolvedCount)
Public Member Functions inherited from ROSEE::Action
virtual	~Action ()
std::string	getName () const
	Get the name of the action. More...
Type	getType () const
std::set< std::string >	getFingersInvolved () const
	Get for fingersInvolved. More...
JointsInvolvedCount	getJointsInvolvedCount () const
	Get for jointsInvolvedCount. More...

Private Member Functions
bool	emitYamlForContact (collision_detection::Contact, YAML::Emitter &) const
	private function to be called by the emitYaml More...

Private Attributes
std::set< StateWithContact, depthComp >	actionStates
	For each pair, we want a set of action because we want to store (in general) more possible way to do that action. More...

Additional Inherited Members
Protected Member Functions inherited from ROSEE::ActionPrimitive
	ActionPrimitive (std::string name, unsigned int maxStoredActionStates, Type type)
	ActionPrimitive (std::string name, unsigned int nFingersInvolved, unsigned int maxStoredActionStates, Type type)
	Protected costructor: object creable only by derived classes. More...
Protected Member Functions inherited from ROSEE::Action
	Action ()
	Action (std::string actionName, Action::Type type)
Protected Attributes inherited from ROSEE::ActionPrimitive
unsigned int	nFingersInvolved
const unsigned int	maxStoredActionStates
const Type	primitiveType
Protected Attributes inherited from ROSEE::Action
std::string	name
Action::Type	type
std::set< std::string >	fingersInvolved
JointsInvolvedCount	jointsInvolvedCount

Detailed Description

The action of pinch with two tips.

The two tips must collide for some hand configuration to mark this configuration as a pinchTight. All the non involved fingers are set in the default state. A pinchTight is defined by:

• 2 tips ( that are inside fingersInvolved ), so nFingersInvolved == 2 ( members of base class ActionPrimitive )
• JointStates position: where the collision happens (inside actionStates )
• Optional info (inside actionStates ): the contact of moveit. Now only the member depth is used. It is used to order the actions in the actionStates set, to have a "quality" measure (make sense if maxStoredActionStates > 1 ): the more the depth of compenetration is, the more we say the pinchTight is good

Definition at line 40 of file ActionPinchTight.h.

Member Typedef Documentation

typedef std::map< std::pair<std::string, std::string>, ActionPinchTight > ROSEE::ActionPinchTight::Map

Definition at line 45 of file ActionPinchTight.h.

typedef std::pair<JointPos, collision_detection::Contact> ROSEE::ActionPinchTight::StateWithContact

A pair to "link" the JointPos with infos about the collision among the two tips.

Definition at line 49 of file ActionPinchTight.h.

Constructor & Destructor Documentation

ROSEE::ActionPinchTight::ActionPinchTight

(

)

Definition at line 21 of file ActionPinchTight.cpp.

                                        : 
    ActionPinchGeneric ("pinchTight", 2, 3, ActionPrimitive::Type::PinchTight) { }

ROSEE::ActionPinchTight::ActionPinchTight

(

unsigned int

maxStoredActionStates

)

Definition at line 24 of file ActionPinchTight.cpp.

                                                                         : 
    ActionPinchGeneric ("pinchTight", 2, jointStateSetMaxSize, ActionPrimitive::Type::PinchTight) { }

ROSEE::ActionPinchTight::ActionPinchTight	(	std::pair< std::string, std::string >	fingerNamesPair,
		JointPos	jp,
		collision_detection::Contact	cont
	)

Definition at line 27 of file ActionPinchTight.cpp.

                                                  :
    ActionPinchGeneric ("pinchTight", 2, 3, ActionPrimitive::Type::PinchTight )  {

    //different from insertState, here we are sure the set is empty (we are in costructor)
    fingersInvolved.insert(fingerNamesPair.first);
    fingersInvolved.insert(fingerNamesPair.second);
    actionStates.insert (std::make_pair (jp, cont) );
}

ROSEE::ActionPinchTight::ActionPinchTight	(	std::string	finger1,
		std::string	finger2,
		JointPos	jp,
		collision_detection::Contact	cont
	)

Definition at line 37 of file ActionPinchTight.cpp.

                                                  :
    ActionPinchGeneric ("pinchTight", 2, 3, ActionPrimitive::Type::PinchTight )  {

    //different from insertState, here we are sure the set is empty (we are in costructor)
    fingersInvolved.insert(finger1);
    fingersInvolved.insert(finger2);
    actionStates.insert (std::make_pair (jp, cont) );
}

Member Function Documentation

void ROSEE::ActionPinchTight::emitYaml ( YAML::Emitter &  out ) const

overridevirtual

Function to fill the argument passed with info about the action.

Pure virtual because each derived class has different infos and stored differently. check YamlWorker to correctly emit and parse the file

Parameters

out	the yaml-cpp emitter which store infos about the action

Note

this function does not print in a file, but simply fill a YAML::Emitter.

Reimplemented from ROSEE::ActionPrimitive.

Definition at line 156 of file ActionPinchTight.cpp.

                                                              {
    
    out << YAML::Key << YAML::Flow << fingersInvolved;
    
    unsigned int nCont = 1;
    out << YAML::Value << YAML::BeginMap;
    out << YAML::Key << "PrimitiveType" << YAML::Value << primitiveType;
    out << YAML::Key << "ActionName" << YAML::Value << name;
    out << YAML::Key << "JointsInvolvedCount" << YAML::Value << YAML::BeginMap;
    for (const auto &jointCount : jointsInvolvedCount ) {
        out << YAML::Key << jointCount.first;
        out << YAML::Value << jointCount.second;
    }
    out << YAML::EndMap;
    
    for (const auto & actionState : actionStates) { //.second is the set of ActionState
        
        std::string contSeq = "ActionState_" + std::to_string(nCont);
        out << YAML::Key << contSeq; 
        
        out << YAML::Value << YAML::BeginMap;
            //actionState.first, the jointstates map
            out << YAML::Key << "JointPos" << YAML::Value << YAML::BeginMap;
            for (const auto &joint : actionState.first) {
                out << YAML::Key << joint.first;
                out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
            }
            out << YAML::EndMap;
            
            //actionState.second, the optional
            out << YAML::Key << "Optional" << YAML::Value;
            emitYamlForContact ( actionState.second, out );
            
        out << YAML::EndMap;
        nCont++;
    }
    out << YAML::EndMap;

}

bool ROSEE::ActionPinchTight::emitYamlForContact ( collision_detection::Contact  moveitContact, YAML::Emitter &  out  ) const

private

private function to be called by the emitYaml

Definition at line 294 of file ActionPinchTight.cpp.

                                                                                                                {

    out << YAML::BeginMap;
        out << YAML::Key << "MoveItContact" << YAML::Value << YAML::BeginMap;
            out << YAML::Key << "body_name_1";
            out << YAML::Value << moveitContact.body_name_1;
            out << YAML::Key << "body_name_2";
            out << YAML::Value << moveitContact.body_name_2;
            out << YAML::Key << "body_type_1";
            out << YAML::Value << moveitContact.body_type_1;
            out << YAML::Key << "body_type_2";
            out << YAML::Value << moveitContact.body_type_2;
            out << YAML::Key << "depth";
            out << YAML::Value << moveitContact.depth;
            out << YAML::Key << "normal";
            std::vector < double > normal ( moveitContact.normal.data(), moveitContact.normal.data() +  moveitContact.normal.rows());  
            out << YAML::Value << YAML::Flow << normal;
            out << YAML::Key << "pos";
            std::vector < double > pos ( moveitContact.pos.data(), moveitContact.pos.data() +  moveitContact.pos.rows());
            out << YAML::Value << YAML::Flow << pos;
        out << YAML::EndMap;
    out << YAML::EndMap;
    
    return true;
}

bool ROSEE::ActionPinchTight::fillFromYaml ( YAML::const_iterator  yamlIt )

overridevirtual

function to fill members of the Action with infos taken from yaml files

Parameters

yamlIt

a YAML::const_iterator to the node that is loaded with YAML::LoadFile(dirPath + filename). check YamlWorker to correctly parse and emit the file

Implements ROSEE::Action.

Definition at line 197 of file ActionPinchTight.cpp.

                                                                     {
        
    std::vector <std::string> fingInvolvedVect = yamlIt->first.as <std::vector < std::string >> ();
    for (const auto &it : fingInvolvedVect) {
        fingersInvolved.insert(it);
    }

    for ( YAML::const_iterator keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue) {        
        
        std::string key = keyValue->first.as<std::string>();
        if (key.compare("JointsInvolvedCount") == 0) {
            jointsInvolvedCount = keyValue->second.as < JointsInvolvedCount > ();
            
        } else if (key.compare ("ActionName") == 0 ) {
            name = keyValue->second.as <std::string> ();
        
        } else if (key.compare ("PrimitiveType") == 0) {
            ROSEE::ActionPrimitive::Type parsedType = static_cast<ROSEE::ActionPrimitive::Type> ( 
                keyValue->second.as <unsigned int>() );
            if (parsedType != primitiveType ) {
                std::cerr << "[ERROR ActionPinchTight::" << __func__ << " parsed a type " << parsedType << 
                    " but this object has primitive type " << primitiveType << std::endl; 
                return false;
            }
            
        } else if (key.compare(0, 12, "ActionState_") == 0) { //compare 12 caracters from index 0 of key

            JointPos jointPos;
            collision_detection::Contact contact;
            for(YAML::const_iterator asEl = keyValue->second.begin(); asEl != keyValue->second.end(); ++asEl) {

                //asEl can be the map JointPos or the map Optional
                if (asEl->first.as<std::string>().compare ("JointPos") == 0 ) {
                    jointPos = asEl->second.as < JointPos >(); 
                } else if (asEl->first.as<std::string>().compare ("Optional") == 0 ) {
                    
                    YAML::Node cont =  asEl->second["MoveItContact"];
                    contact.body_name_1 = cont["body_name_1"].as < std::string >();
                    contact.body_name_2 = cont["body_name_2"].as < std::string >();
                    switch (cont["body_type_1"].as < int >())
                    {
                    case 0:
                        contact.body_type_1 = collision_detection::BodyType::ROBOT_LINK;
                        break;
                    case 1:
                        contact.body_type_1 = collision_detection::BodyType::ROBOT_ATTACHED;
                        break;
                    case 2:
                        contact.body_type_1 = collision_detection::BodyType::WORLD_OBJECT;
                        break;
                    default:
                        std::cout << "some error, body_type_1" << cont["body_type_1"].as < int >()
                            << "unknown" << std::endl;
                        contact.body_type_1 = collision_detection::BodyType::WORLD_OBJECT;
                    }
                    switch (cont["body_type_2"].as < int >())
                    {
                    case 0:
                        contact.body_type_2 = collision_detection::BodyType::ROBOT_LINK;
                        break;
                    case 1:
                        contact.body_type_2 = collision_detection::BodyType::ROBOT_ATTACHED;
                        break;
                    case 2:
                        contact.body_type_2 = collision_detection::BodyType::WORLD_OBJECT;
                        break;
                    default:
                        std::cout << "some error, body_type_2" << cont["body_type_2"].as < int >()
                            << "unknown" << std::endl;
                        contact.body_type_2 = collision_detection::BodyType::WORLD_OBJECT;
                    }
                    contact.depth = cont["depth"].as<double>();
                    std::vector < double > normVect = cont["normal"].as < std::vector <double> >();
                    std::vector < double > posVect = cont["pos"].as < std::vector <double> >();
                    contact.normal = Eigen::Vector3d (normVect.data() );
                    contact.pos = Eigen::Vector3d (posVect.data() );
                    
                } else {
                    //ERRROr, only joinstates and optional at this level
                    std::cerr << "[ERROR ActionPinchTight::" << __func__ << "not know key " 
                        << asEl->first.as<std::string>() << 
                        " found in the yaml file at this level" << std::endl; 
                    return false;
                }
            }  
            actionStates.insert ( std::make_pair (jointPos, contact));
            
        } else {
            std::cerr << "[ERROR ActionPinchTight::" << __func__ << "not know key " << key << 
                " found in the yaml file" << std::endl; 
            return false;
        }
    }
    
    return true;
}

std::vector< ROSEE::ActionPinchTight::StateWithContact > ROSEE::ActionPinchTight::getActionStates

(

)

const

Specific get for the ActionPinchTight to return the state with contact info.

Returns

The vector (of size maxStoredActionStates) containing all the StateWithContact objects

Definition at line 73 of file ActionPinchTight.cpp.

                                                                                                  {
    
    std::vector < ROSEE::ActionPinchTight::StateWithContact > retVect;
    retVect.reserve ( actionStates.size() );
    
    for (auto it : actionStates ) {
        retVect.push_back(it);
    }
    
    return retVect;
    
}

std::vector< ROSEE::JointPos > ROSEE::ActionPinchTight::getAllJointPos ( ) const

overridevirtual

Return all the joint position stored.

If the concrete (derived from Action) has only one joint position info, this function is equal to getJointPos.

Returns

vector containing all the joint pos of the action

Implements ROSEE::Action.

Definition at line 60 of file ActionPinchTight.cpp.

                                                                       {
    
    std::vector < JointPos > retVect;
    retVect.reserve ( actionStates.size() );
    
    for (auto it : actionStates ) {
        retVect.push_back(it.first);
    }
    
    return retVect;
}

ROSEE::JointPos ROSEE::ActionPinchTight::getJointPos ( ) const

overridevirtual

Get the position related to this action.

Pure Virtual function: the derived class store this info differently so they are in charge of providing the read.

Returns

JointsPos the map indicating how the position of the joint

Implements ROSEE::Action.

Definition at line 47 of file ActionPinchTight.cpp.

                                                       {
    return (actionStates.begin()->first);
}

ROSEE::JointPos ROSEE::ActionPinchTight::getJointPos

(

unsigned int

index

)

const

Definition at line 51 of file ActionPinchTight.cpp.

                                                                          {
    auto it = actionStates.begin();
    unsigned int i = 1;
    while (i < index ) {
        ++ it;
    }
    return (it->first);
}

bool ROSEE::ActionPinchTight::insertActionState	(	ROSEE::JointPos	jp,
		collision_detection::Contact	cont
	)

function to insert a single action in the actionStates set of possible action.

If the action is not so good (based on depth now) the action is not inserted and the function return false

Parameters

JointPos	The joints position
collision_detection::Contact	the contact associated with the action

Returns

TRUE if the action is good and is inserted in the set actionStates FALSE if the action given as param was not good as the others in the set actionStates and the set was already full (maxStoredActionStates)

Definition at line 86 of file ActionPinchTight.cpp.

                                                                                                {

    auto pairRet = actionStates.insert ( std::make_pair (jp, cont) ) ;
    
    if (! pairRet.second ) {
        //TODO print error no insertion because depth is equal... very improbable
        return false;
    }
    
    if (actionStates.size() > maxStoredActionStates) { 
        //max capacity reached, we have to delete the last one
        auto it = pairRet.first;        
        
        if ( (++it) == actionStates.end() ){
           // the new inserted is the last one and has to be erased
            actionStates.erase(pairRet.first);
            return false;
        }
        
        // the new inserted is not the last one that has to be erased
        auto lastElem = actionStates.end();
        --lastElem;
        actionStates.erase(lastElem);
    }
    
    return true;
}

void ROSEE::ActionPinchTight::print ( ) const

overridevirtual

For the pinch, we override these function to print, emit and parse the optional info Contact, which is specific of the pinch.

Reimplemented from ROSEE::Action.

Definition at line 115 of file ActionPinchTight.cpp.

                                         {
    
    std::stringstream output;
    output << "ActionName: " << name << std::endl;
    
    output << "FingersInvolved: [";
    for (auto fingName : fingersInvolved){
        output << fingName << ", " ;
    }
    output.seekp (-2, output.cur); //to remove the last comma (and space)
    output << "]" << std::endl;
    
    output << "JointsInvolvedCount: " << std::endl;;
    output << jointsInvolvedCount << std::endl;
    
    unsigned int nActState = 1;
    for (auto itemSet : actionStates) {  //the element in the set
        output << "Action_State_" << nActState << " :" << std::endl;

        output << "\t" << "JointStates:" << std::endl;
        output << itemSet.first;
        output << "\t" << "MoveitContact:" << std::endl;
        output << "\t\tbody_name_1: " << itemSet.second.body_name_1 << std::endl;
        output << "\t\tbody_name_2: " << itemSet.second.body_name_2 << std::endl;
        output << "\t\tbody_type_1: " << itemSet.second.body_type_1 << std::endl;
        output << "\t\tbody_type_2: " << itemSet.second.body_type_2 << std::endl;
        output << "\t\tdepth: " << itemSet.second.depth << std::endl;
        output << "\t\tnormal: " << "["<< itemSet.second.normal.x() << ", " 
            << itemSet.second.normal.y() << ", " << itemSet.second.normal.z() << "]" << std::endl;
        output << "\t\tpos: " << "["<< itemSet.second.pos.x() << ", " 
            << itemSet.second.pos.y() << ", " << itemSet.second.pos.z() << "]" << std::endl;
            
        nActState++;
    }
    output << std::endl;
    
    std::cout << output.str();

}

Member Data Documentation

std::set< StateWithContact, depthComp > ROSEE::ActionPinchTight::actionStates

private

For each pair, we want a set of action because we want to store (in general) more possible way to do that action.

The pinch among two tips can theoretically be done in infinite ways, we store the best ways found (ordering them by the depth of fingertips compenetration)

Definition at line 109 of file ActionPinchTight.h.

---

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

• /home/travis/build/ADVRHumanoids/ROSEndEffector/include/end_effector/GraspingActions/ActionPinchTight.h
• /home/travis/build/ADVRHumanoids/ROSEndEffector/src/GraspingActions/ActionPinchTight.cpp