ROSEE::ActionTimed Class Reference

An action composed by other ones that must be executed one after other with some wait time (also 0) in between. More...

#include <ActionTimed.h>

Inheritance diagram for ROSEE::ActionTimed:

Collaboration diagram for ROSEE::ActionTimed:

Public Types
typedef std::shared_ptr< ActionTimed >	Ptr
typedef std::shared_ptr< const ActionTimed >	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
	ActionTimed ()
	Default constructor, used when parsing action from yaml file. More...
	ActionTimed (std::string actionName)
	Costructor. More...
	~ActionTimed ()
	Destructor. More...
JointPos	getJointPos () const override
	Override this function is necessary because it is pure virtual in father class Action. More...
std::vector< ROSEE::JointPos >	getAllJointPos () const override
	Override function from father Action. More...
std::vector< ROSEE::JointsInvolvedCount >	getAllJointCountAction () const
	Get the JointsInvolvedCount maps of all the inner actions, in order. More...
std::vector< std::pair< double, double > >	getAllActionMargins () const
	get all the time margins of all inner action More...
ROSEE::JointPos	getJointPosAction (std::string actionName) const
	get for joint positions More...
std::pair< double, double >	getActionMargins (std::string actionName) const
	get for time margins More...
ROSEE::JointsInvolvedCount	getJointCountAction (std::string actionName) const
	get for JointsInvolvedCount of the inner actions More...
std::vector< std::string >	getInnerActionsNames () const
	getter for action that composed this one More...
void	print () const override
	Print info about this action. More...
void	emitYaml (YAML::Emitter &out) const override
	Emit info in a file with yaml format. More...
bool	fillFromYaml (YAML::const_iterator yamlIt) override
	Fill the internal data with infos taken from yaml file. More...
bool	insertAction (ROSEE::Action::Ptr action, double marginBefore=0.0, double marginAfter=0.0, unsigned int jointPosIndex=0, double percentJointPos=1, std::string newActionName="")
	Insert an action as last one in the time line. More...
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 Attributes
std::map< std::string, std::pair< double, double > >	actionsTimeMarginsMap
std::map< std::string, ROSEE::JointPos >	actionsJointPosMap
std::map< std::string, ROSEE::JointsInvolvedCount >	actionsJointCountMap
ROSEE::JointPos	jointPosFinal
	Here is contained the wanted final position of the timed action. More...
std::vector< std::string >	actionsNamesOrdered
	This vector is used to take count of the order of the actions inserted. More...

Additional Inherited Members
Protected Member Functions inherited from ROSEE::Action
	Action ()
	Action (std::string actionName, Action::Type type)
Protected Attributes inherited from ROSEE::Action
std::string	name
Action::Type	type
std::set< std::string >	fingersInvolved
JointsInvolvedCount	jointsInvolvedCount

Detailed Description

An action composed by other ones that must be executed one after other with some wait time (also 0) in between.

E.G. 0.000000second --—> Grasp --—> 0.1 + 0.1 second --—> OpenLid --—> 0.5second -— and so on This class contains all the joint position of each action (actionsJointPosMap ) and some time margins which indicates wait time before and after the action (actionsTimeMarginsMap). Each action inside is identified by its name, so no two action with same name can exist (see insertAction ) After create the ActionTimed object, we can add actions with insertAction(). As all other Action classes, it implements also functions to emit and parse in a yaml file

Todo:

identify inner actions with integer instead of given name?

Definition at line 39 of file ActionTimed.h.

Member Typedef Documentation

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

Definition at line 43 of file ActionTimed.h.

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

Definition at line 42 of file ActionTimed.h.

Constructor & Destructor Documentation

ROSEE::ActionTimed::ActionTimed

(

)

Default constructor, used when parsing action from yaml file.

Definition at line 19 of file ActionTimed.cpp.

                              {
    type = Action::Type::Timed;
}

ROSEE::ActionTimed::ActionTimed

(

std::string

actionName

)

Costructor.

Parameters

actionName

name of the action that will be created

Definition at line 23 of file ActionTimed.cpp.

                                              : Action(name, Action::Type::Timed) {
    
}

ROSEE::ActionTimed::~ActionTimed ( )

inline

Destructor.

Definition at line 58 of file ActionTimed.h.

{};

Member Function Documentation

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

overridevirtual

Emit info in a file with yaml format.

Parameters

out	a YAML::Emitter& object to emit the infos

Implements ROSEE::Action.

Definition at line 142 of file ActionTimed.cpp.

                                                      {
    
    out << YAML::BeginMap << YAML::Key << name << YAML::Value << YAML::BeginMap ;
        std::string timeline;
        for ( auto it : actionsNamesOrdered )  {
            timeline += (std::to_string(actionsTimeMarginsMap.at(it).first) + " -----> " + 
                it + " -----> " + std::to_string(actionsTimeMarginsMap.at(it).second) + " + ");
        }
        if (! timeline.empty() ) { 
            timeline.pop_back(); timeline.pop_back(); timeline.pop_back(); //to delete last " + " 
        }
        
        out << YAML::Comment(timeline);
        out << YAML::Key << "Type" << YAML::Value << type;
        out << YAML::Key << "FingersInvolved" << YAML::Value << YAML::Flow << fingersInvolved;
        
        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;
        
        out << YAML::Key << "ActionsJointPosFinal" << YAML::Value << YAML::BeginMap;
        for ( const auto joint : jointPosFinal ) {
            out << YAML::Key << joint.first;
            out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
        }
        out << YAML::EndMap;
        
        out << YAML::Key << "ActionsNamesOrdered" << YAML::Value << YAML::Flow << actionsNamesOrdered;

        out << YAML::Key << "ActionsTimeMargins" << YAML::Value << YAML::BeginMap;
        for (const std::string action : actionsNamesOrdered ){

            out << YAML::Key << action << YAML::Value << YAML::BeginMap;
                
                out << YAML::Key << "marginBefore" << 
                    YAML::Value << actionsTimeMarginsMap.at(action).first;
                    
                out << YAML::Key << "marginAfter" << 
                    YAML::Value << actionsTimeMarginsMap.at(action).second;
            out << YAML::EndMap;
        }
        out << YAML::EndMap;

        out << YAML::Key << "ActionsJointPos" << YAML::Value << YAML::BeginMap;
        for (const std::string action : actionsNamesOrdered ){

            out << YAML::Key << action << YAML::Value << YAML::BeginMap;
                
                for ( const auto joint : actionsJointPosMap.at(action) ) {
                    out << YAML::Key << joint.first;
                    out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
                }
            out << YAML::EndMap;
        }
        out << YAML::EndMap;
        
        
        out << YAML::Key << "ActionsJointCount" << YAML::Value << YAML::BeginMap;
        for (const std::string action : actionsNamesOrdered ){

            out << YAML::Key << action << YAML::Value << YAML::BeginMap;
                
                for ( const auto joint : actionsJointCountMap.at(action) ) {
                    out << YAML::Key << joint.first;
                    out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
                }
            out << YAML::EndMap;
        }
        out << YAML::EndMap;
        


        
        out << YAML::EndMap; // map began at the beginning of the function
    out << YAML::EndMap;// map began at the beginning of the function
}

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

overridevirtual

Fill the internal data with infos taken from yaml file.

Parameters

yamlIt

a yamlt iterator to a node which has loaded the file

Returns

false if some error happened

Implements ROSEE::Action.

Definition at line 222 of file ActionTimed.cpp.

                                                            {
    
    name = yamlIt->first.as<std::string>();
    type = Action::Type::Timed;
    
    for (auto keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue ) {

        std::string key = keyValue->first.as<std::string>();
        
        if ( key.compare ("FingersInvolved") == 0 ) { 
            auto tempVect = keyValue->second.as <std::vector <std::string> > ();
            fingersInvolved.insert ( tempVect.begin(), tempVect.end() );
            
        } else if ( key.compare ("Type") == 0 ) {
            if (ROSEE::Action::Type::Timed != static_cast<ROSEE::Action::Type> ( keyValue->second.as <unsigned int>() )) {
                std::cout << "[Timed ACTION::" << __func__ << "] Error, found type  " << keyValue->second.as <unsigned int>()
                << "instead of Timed type (" << ROSEE::Action::Type::Timed << ")" << std::endl;
                return false;
            }
            type = ROSEE::Action::Type::Timed;
            
        } else if ( key.compare ("ActionsNamesOrdered") == 0 ) {
            actionsNamesOrdered = keyValue->second.as < std::vector <std::string> > ();
            
        } else if ( key.compare ("JointsInvolvedCount") == 0 ) {
            jointsInvolvedCount = keyValue->second.as < JointsInvolvedCount >(); 
            
        } else if ( key.compare("ActionsTimeMargins") == 0 ) {
            
            for (auto tMargins = keyValue->second.begin(); tMargins != keyValue->second.end(); ++tMargins ) {

                std::string actNAme = tMargins->first.as<std::string>();
                double before = tMargins->second["marginBefore"].as<double>();
                double after = tMargins->second["marginAfter"].as<double>();
                actionsTimeMarginsMap.insert (std::make_pair (actNAme, std::make_pair(before, after)  ) ) ;
            }
            
        } else if ( key.compare("ActionsJointPos") == 0) {
            
            for (auto jPos = keyValue->second.begin(); jPos != keyValue->second.end(); ++jPos ) {
                
                std::string actName = jPos->first.as<std::string>();
                JointPos jp = jPos->second.as<ROSEE::JointPos>();
                actionsJointPosMap.insert (std::make_pair (actName, jp) );
            }

        } else if ( key.compare("ActionsJointCount") == 0) {
            
            for (auto jCount = keyValue->second.begin(); jCount != keyValue->second.end(); ++jCount ) {
                
                std::string actName = jCount->first.as<std::string>();
                JointsInvolvedCount jc = jCount->second.as<ROSEE::JointsInvolvedCount>();
                actionsJointCountMap.insert (std::make_pair (actName, jc) );
            }

            
        } else if ( key.compare("ActionsJointPosFinal") == 0) {
            
            jointPosFinal =  keyValue->second.as < JointPos >();
        
        } else {
            std::cerr << "[TIMEDACTION::" << __func__ << "] Error, not known key " << key << std::endl;
            return false;
        }
    }
    
    return true;
}

std::pair< double, double > ROSEE::ActionTimed::getActionMargins

(

std::string

actionName

)

const

get for time margins

Parameters

actionName

the name of the inner action of which we want to know the time margins

Returns

a pair of positive double. The first is the time needed BEFORE the action, the secont the time AFTER return -1 -1 if the actionName was not present in this ActionTimed.

Definition at line 88 of file ActionTimed.cpp.

                                                                                         {
 
    auto it = actionsTimeMarginsMap.find(actionName);
    
    if ( it != actionsTimeMarginsMap.end() ) {
        return ( it->second );
        
    } else {
        std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: action " << actionName << " not present in this composed timed action" << std::endl;
        return std::make_pair(-1, -1);
    }
}

std::vector< std::pair< double, double > > ROSEE::ActionTimed::getAllActionMargins

(

)

const

get all the time margins of all inner action

Returns

vector of pair : the first element of pair is the time to wait before executing the action, the second element the time to wait after.

Definition at line 51 of file ActionTimed.cpp.

                                                                                {
    
    std::vector <std::pair <double,double> > timeVect;
    timeVect.reserve (actionsNamesOrdered.size());
    for (auto actName : actionsNamesOrdered) {
        timeVect.push_back( actionsTimeMarginsMap.at (actName) );
    }
    return timeVect;
}

std::vector< ROSEE::JointsInvolvedCount > ROSEE::ActionTimed::getAllJointCountAction

(

)

const

Get the JointsInvolvedCount maps of all the inner actions, in order.

Returns

vector of JointsInvolvedCount map of inner actions

Definition at line 41 of file ActionTimed.cpp.

                                                                                   {
    
    std::vector<ROSEE::JointsInvolvedCount> jcVect;
    jcVect.reserve (actionsNamesOrdered.size());
    for (auto actName : actionsNamesOrdered) {
        jcVect.push_back( actionsJointCountMap.at (actName) );
    }
    return jcVect;
}

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

overridevirtual

Override function from father Action.

For this class, it returns the jointPos of all the inner actions that are inside this ActionTimed.

Returns

vector of JointPos of all the inner actions, in order of execution

Implements ROSEE::Action.

Definition at line 31 of file ActionTimed.cpp.

                                                                {
    
    std::vector<ROSEE::JointPos> jpVect;
    jpVect.reserve (actionsNamesOrdered.size());
    for (auto actName : actionsNamesOrdered) {
        jpVect.push_back( actionsJointPosMap.at (actName) );
    }
    return jpVect;
}

std::vector< std::string > ROSEE::ActionTimed::getInnerActionsNames

(

)

const

getter for action that composed this one

Returns

vector of string of inner actions, ordered by time execution

Definition at line 101 of file ActionTimed.cpp.

                                                                  {
    return actionsNamesOrdered;
}

ROSEE::JointsInvolvedCount ROSEE::ActionTimed::getJointCountAction

(

std::string

actionName

)

const

get for JointsInvolvedCount of the inner actions

Parameters

actionName

the name of the inner action of which we want to know the JointCount

Returns

JointsInvolvedCount of joints of actionName Return empty JointsInvolvedCount if actionName is not present in this ActionTimed

Definition at line 74 of file ActionTimed.cpp.

                                                                                         {
    
    auto it = actionsJointCountMap.find(actionName);
    
    if ( it != actionsJointCountMap.end() ) {
        return (it->second);
        
    } else {
        std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: action " << actionName << " not present in this composed timed action" << std::endl;
        return ROSEE::JointsInvolvedCount();
    } 
}

ROSEE::JointPos ROSEE::ActionTimed::getJointPos ( ) const

overridevirtual

Override this function is necessary because it is pure virtual in father class Action.

Returns

JointPos the joints positions of the last inserted action (the last one in the time line)

Implements ROSEE::Action.

Definition at line 27 of file ActionTimed.cpp.

                                                  {
    return jointPosFinal;
}

ROSEE::JointPos ROSEE::ActionTimed::getJointPosAction

(

std::string

actionName

)

const

get for joint positions

Parameters

actionName

the name of the inner action of which we want to know the JointPos

Returns

JointPos position of joints of actionName Return empty JointPos if actionName is not present in this ActionTimed

Definition at line 61 of file ActionTimed.cpp.

                                                                             {
    
    auto it = actionsJointPosMap.find(actionName);
    
    if ( it != actionsJointPosMap.end() ) {
        return (it->second);
        
    } else {
        std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: action " << actionName << " not present in this composed timed action" << std::endl;
        return ROSEE::JointPos();
    }
}

bool ROSEE::ActionTimed::insertAction	(	ROSEE::Action::Ptr	action,
		double	marginBefore = 0.0,
		double	marginAfter = 0.0,
		unsigned int	jointPosIndex = 0,
		double	percentJointPos = 1,
		std::string	newActionName = ""
	)

Insert an action as last one in the time line.

Parameters

action	pointer to the action to be inserted
marginBefore	the time margin to wait before executing the action
marginAfter	the time margin to wait after executing the action
jointPosIndex	(default == 0) the wanted jointPos of action to insert. Error the index is greater than the number of joint pos in the action. First element has index 0.
percentJointPos	(default == 1) OPTIONAL argument to scale all the joint position of the action that is being inserted
newActionName	(default == "") OPTIONAL argument if we want to store the action with a different name

Returns

False if some error happened

Warning

We can't have inned actions with same name. So, if action name (or newActionName) is already present, the action is not inserted and the function returns false. Being Action names not changeable, to solve this we can pass the newActionName argument to this function. If it will be inserted, it will be referenced with this new name

Note

We take only necessary infos from action and store them in the members of ActionTimed. There is not way to go back to the original inserted action from an ActionTimed

Definition at line 291 of file ActionTimed.cpp.

                                                                                                                   {
    
    std::string usedName = (newActionName.empty()) ? action->getName() : newActionName;

    unsigned int count = actionsJointPosMap.count( usedName );

    if (count > 0) {
        std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: " << usedName << " already present. Failed Insertion" << std::endl;
        return false;
    }

    if (marginAfter < 0 || marginBefore < 0) {
        std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: Please pass positive time margins" << std::endl;
        return false;
    }
    
    if ( jointPosIndex > action->getAllJointPos().size()-1 ) {
        std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: you pass index "<< jointPosIndex <<
            " but there are only " << action->getAllJointPos().size() << " JointPos in the action passed as argument" << std::endl;
        return false;
    }
    
    if (percentJointPos < 0 || percentJointPos > 1) {
        std::cerr << "[ACTIONTIMED:: " << __func__ << "] Please insert percentage for jointpos between 0 and 1. Passed: " 
            << percentJointPos << std::endl;
        return false;
    }
    
    if ( actionsNamesOrdered.size() > 0 && 
        (! ROSEE::Utils::keys_equal(action->getAllJointPos().at(jointPosIndex), actionsJointPosMap.begin()->second)) ) {
        //we need only to compare the first jointPos (.begin())
        std::cerr << "[ACTIONTIMED:: " << __func__ << "] The action passed as argument has different keys in jointPosmap" 
                  << " respect to the others inserted in this timed action " << std::endl;
        return false;
    }

    ROSEE::JointPos insertingJP = (percentJointPos)*(action->getAllJointPos().at( jointPosIndex )) ;
    actionsJointPosMap.insert (std::make_pair ( usedName, insertingJP) );
    actionsTimeMarginsMap.insert ( std::make_pair( usedName, std::make_pair(marginBefore, marginAfter)));
    actionsNamesOrdered.push_back ( usedName );
    actionsJointCountMap.insert (std::make_pair (usedName, action->getJointsInvolvedCount()));

    //father member
    for ( auto it: action->getFingersInvolved() ) {
         fingersInvolved.insert ( it );
    }

    if (actionsNamesOrdered.size() == 1 ) { //We are inserting first action, we have to init the JointsInvolvedCount map 
        
        jointsInvolvedCount = action->getJointsInvolvedCount();
        jointPosFinal = insertingJP;
        
    } else { 
        // add the action.jointInvolvedCount to the timed jointCount
        // and update jointPosFinal 
        for (auto jic : action->getJointsInvolvedCount() ) {
            jointsInvolvedCount.at(jic.first) += jic.second;
            
            if (jic.second > 0) {
                //if so, we must overwrite the pos of this joint in jointPosFina
                jointPosFinal.at(jic.first) = insertingJP.at(jic.first);
            }
        } 
    }

    return true;
}

void ROSEE::ActionTimed::print ( ) const

overridevirtual

Print info about this action.

Reimplemented from ROSEE::Action.

Definition at line 106 of file ActionTimed.cpp.

                                   {
    
    std::stringstream output;
    
    output << "Timed Action '" << name << "'" << std::endl;
    
    output << "\tNice TimeLine:" << std::endl << "\t\t";
    for ( auto it : actionsNamesOrdered )  {
        output << actionsTimeMarginsMap.at(it).first << " -----> " << it << " -----> " << actionsTimeMarginsMap.at(it).second << " + ";
    }
    output.seekp (-3, output.cur); //to remove the last  --+--
    output << std::endl;
    
    output << "\tJointPos of each actions, in order of execution:\n";
    for ( auto actionName : actionsNamesOrdered )  {
        output << "\t" << actionName << std::endl;
        output << actionsJointPosMap.at(actionName) << std::endl;
    }
    
    output << "\tJointPos final, the sum of all joint pos of each inner action:\n";
    output << jointPosFinal << std::endl;
    
    output << "\tFingers involved: [" ;
    for (auto it : fingersInvolved) {
        output << it << ", ";
    }
    output.seekp (-2, output.cur); //to remove the last comma (and space)
    output << "]" << std::endl;
    
    output << "\tEach joint influenced by x inner action:" << std::endl;
    output << jointsInvolvedCount << std::endl;
    
    std::cout << output.str();
    
}

Member Data Documentation

std::map<std::string, ROSEE::JointsInvolvedCount> ROSEE::ActionTimed::actionsJointCountMap

private

Definition at line 156 of file ActionTimed.h.

std::map<std::string, ROSEE::JointPos> ROSEE::ActionTimed::actionsJointPosMap

private

Definition at line 155 of file ActionTimed.h.

std::vector< std::string > ROSEE::ActionTimed::actionsNamesOrdered

private

This vector is used to take count of the order of the actions inserted.

Definition at line 167 of file ActionTimed.h.

std::map<std::string, std::pair<double, double> > ROSEE::ActionTimed::actionsTimeMarginsMap

private

Definition at line 154 of file ActionTimed.h.

ROSEE::JointPos ROSEE::ActionTimed::jointPosFinal

private

Here is contained the wanted final position of the timed action.

So, it is the sum of all the wanted joint position of all the inner actions

Definition at line 162 of file ActionTimed.h.

---

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

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