Line data    Source code

       1             : /*
       2             :  * Copyright 2020 <copyright holder> <email>
       3             :  *
       4             :  * Licensed under the Apache License, Version 2.0 (the "License");
       5             :  * you may not use this file except in compliance with the License.
       6             :  * You may obtain a copy of the License at
       7             :  *
       8             :  *     http://www.apache.org/licenses/LICENSE-2.0
       9             :  *
      10             :  * Unless required by applicable law or agreed to in writing, software
      11             :  * distributed under the License is distributed on an "AS IS" BASIS,
      12             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      13             :  * See the License for the specific language governing permissions and
      14             :  * limitations under the License.
      15             :  */
      16             : 
      17             : #include <end_effector/GraspingActions/ActionTimed.h>
      18             : 
      19           8 : ROSEE::ActionTimed::ActionTimed() {
      20           8 :     type = Action::Type::Timed;
      21           8 : }
      22             : 
      23          12 : ROSEE::ActionTimed::ActionTimed (std::string name ) : Action(name, Action::Type::Timed) {
      24             :     
      25          12 : }
      26             : 
      27          44 : ROSEE::JointPos ROSEE::ActionTimed::getJointPos() const {
      28          44 :     return jointPosFinal;
      29             : }
      30             : 
      31          98 : std::vector<ROSEE::JointPos> ROSEE::ActionTimed::getAllJointPos() const {
      32             :     
      33          98 :     std::vector<ROSEE::JointPos> jpVect;
      34          98 :     jpVect.reserve (actionsNamesOrdered.size());
      35         526 :     for (auto actName : actionsNamesOrdered) {
      36         428 :         jpVect.push_back( actionsJointPosMap.at (actName) );
      37             :     }
      38          98 :     return jpVect;
      39             : }
      40             : 
      41         106 : std::vector<ROSEE::JointsInvolvedCount> ROSEE::ActionTimed::getAllJointCountAction() const {
      42             :     
      43         106 :     std::vector<ROSEE::JointsInvolvedCount> jcVect;
      44         106 :     jcVect.reserve (actionsNamesOrdered.size());
      45         562 :     for (auto actName : actionsNamesOrdered) {
      46         456 :         jcVect.push_back( actionsJointCountMap.at (actName) );
      47             :     }
      48         106 :     return jcVect;
      49             : }
      50             : 
      51          48 : std::vector<std::pair<double, double> > ROSEE::ActionTimed::getAllActionMargins() const {
      52             :     
      53          48 :     std::vector <std::pair <double,double> > timeVect;
      54          48 :     timeVect.reserve (actionsNamesOrdered.size());
      55         234 :     for (auto actName : actionsNamesOrdered) {
      56         186 :         timeVect.push_back( actionsTimeMarginsMap.at (actName) );
      57             :     }
      58          48 :     return timeVect;
      59             : }
      60             : 
      61           0 : ROSEE::JointPos ROSEE::ActionTimed::getJointPosAction (std::string actionName) const {
      62             :     
      63           0 :     auto it = actionsJointPosMap.find(actionName);
      64             :     
      65           0 :     if ( it != actionsJointPosMap.end() ) {
      66           0 :         return (it->second);
      67             :         
      68             :     } else {
      69           0 :         std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: action " << actionName << " not present in this composed timed action" << std::endl;
      70           0 :         return ROSEE::JointPos();
      71             :     }
      72             : }
      73             : 
      74           0 : ROSEE::JointsInvolvedCount ROSEE::ActionTimed::getJointCountAction(std::string actionName) const {
      75             :     
      76           0 :     auto it = actionsJointCountMap.find(actionName);
      77             :     
      78           0 :     if ( it != actionsJointCountMap.end() ) {
      79           0 :         return (it->second);
      80             :         
      81             :     } else {
      82           0 :         std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: action " << actionName << " not present in this composed timed action" << std::endl;
      83           0 :         return ROSEE::JointsInvolvedCount();
      84             :     } 
      85             : }
      86             : 
      87             : 
      88           0 : std::pair <double, double> ROSEE::ActionTimed::getActionMargins ( std::string actionName ) const {
      89             :  
      90           0 :     auto it = actionsTimeMarginsMap.find(actionName);
      91             :     
      92           0 :     if ( it != actionsTimeMarginsMap.end() ) {
      93           0 :         return ( it->second );
      94             :         
      95             :     } else {
      96           0 :         std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: action " << actionName << " not present in this composed timed action" << std::endl;
      97           0 :         return std::make_pair(-1, -1);
      98             :     }
      99             : }
     100             : 
     101          34 : std::vector<std::string> ROSEE::ActionTimed::getInnerActionsNames() const {
     102          34 :     return actionsNamesOrdered;
     103             : }
     104             : 
     105             : 
     106           0 : void ROSEE::ActionTimed::print() const {
     107             :     
     108           0 :     std::stringstream output;
     109             :     
     110           0 :     output << "Timed Action '" << name << "'" << std::endl;
     111             :     
     112           0 :     output << "\tNice TimeLine:" << std::endl << "\t\t";
     113           0 :     for ( auto it : actionsNamesOrdered )  {
     114           0 :         output << actionsTimeMarginsMap.at(it).first << " -----> " << it << " -----> " << actionsTimeMarginsMap.at(it).second << " + ";
     115             :     }
     116           0 :     output.seekp (-3, output.cur); //to remove the last  --+--
     117           0 :     output << std::endl;
     118             :     
     119           0 :     output << "\tJointPos of each actions, in order of execution:\n";
     120           0 :     for ( auto actionName : actionsNamesOrdered )  {
     121           0 :         output << "\t" << actionName << std::endl;
     122           0 :         output << actionsJointPosMap.at(actionName) << std::endl;
     123             :     }
     124             :     
     125           0 :     output << "\tJointPos final, the sum of all joint pos of each inner action:\n";
     126           0 :     output << jointPosFinal << std::endl;
     127             :     
     128           0 :     output << "\tFingers involved: [" ;
     129           0 :     for (auto it : fingersInvolved) {
     130           0 :         output << it << ", ";
     131             :     }
     132           0 :     output.seekp (-2, output.cur); //to remove the last comma (and space)
     133           0 :     output << "]" << std::endl;
     134             :     
     135           0 :     output << "\tEach joint influenced by x inner action:" << std::endl;
     136           0 :     output << jointsInvolvedCount << std::endl;
     137             :     
     138           0 :     std::cout << output.str();
     139             :     
     140           0 : }
     141             : 
     142          12 : void ROSEE::ActionTimed::emitYaml(YAML::Emitter& out) const {
     143             :     
     144          12 :     out << YAML::BeginMap << YAML::Key << name << YAML::Value << YAML::BeginMap ;
     145          24 :         std::string timeline;
     146          49 :         for ( auto it : actionsNamesOrdered )  {
     147          74 :             timeline += (std::to_string(actionsTimeMarginsMap.at(it).first) + " -----> " + 
     148         111 :                 it + " -----> " + std::to_string(actionsTimeMarginsMap.at(it).second) + " + ");
     149             :         }
     150          12 :         if (! timeline.empty() ) { 
     151          12 :             timeline.pop_back(); timeline.pop_back(); timeline.pop_back(); //to delete last " + " 
     152             :         }
     153             :         
     154          12 :         out << YAML::Comment(timeline);
     155          12 :         out << YAML::Key << "Type" << YAML::Value << type;
     156          12 :         out << YAML::Key << "FingersInvolved" << YAML::Value << YAML::Flow << fingersInvolved;
     157             :         
     158          12 :         out << YAML::Key << "JointsInvolvedCount" << YAML::Value << YAML::BeginMap;
     159          72 :         for (const auto &jointCount : jointsInvolvedCount ) {
     160          60 :             out << YAML::Key << jointCount.first;
     161          60 :             out << YAML::Value << jointCount.second;
     162             :         } 
     163          12 :         out << YAML::EndMap;
     164             :         
     165          12 :         out << YAML::Key << "ActionsJointPosFinal" << YAML::Value << YAML::BeginMap;
     166          72 :         for ( const auto joint : jointPosFinal ) {
     167          60 :             out << YAML::Key << joint.first;
     168          60 :             out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
     169             :         }
     170          12 :         out << YAML::EndMap;
     171             :         
     172          12 :         out << YAML::Key << "ActionsNamesOrdered" << YAML::Value << YAML::Flow << actionsNamesOrdered;
     173             : 
     174          12 :         out << YAML::Key << "ActionsTimeMargins" << YAML::Value << YAML::BeginMap;
     175          49 :         for (const std::string action : actionsNamesOrdered ){
     176             : 
     177          37 :             out << YAML::Key << action << YAML::Value << YAML::BeginMap;
     178             :                 
     179             :                 out << YAML::Key << "marginBefore" << 
     180          37 :                     YAML::Value << actionsTimeMarginsMap.at(action).first;
     181             :                     
     182             :                 out << YAML::Key << "marginAfter" << 
     183          37 :                     YAML::Value << actionsTimeMarginsMap.at(action).second;
     184          37 :             out << YAML::EndMap;
     185             :         }
     186          12 :         out << YAML::EndMap;
     187             : 
     188          12 :         out << YAML::Key << "ActionsJointPos" << YAML::Value << YAML::BeginMap;
     189          49 :         for (const std::string action : actionsNamesOrdered ){
     190             : 
     191          37 :             out << YAML::Key << action << YAML::Value << YAML::BeginMap;
     192             :                 
     193         261 :                 for ( const auto joint : actionsJointPosMap.at(action) ) {
     194         224 :                     out << YAML::Key << joint.first;
     195         224 :                     out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
     196             :                 }
     197          37 :             out << YAML::EndMap;
     198             :         }
     199          12 :         out << YAML::EndMap;
     200             :         
     201             :         
     202          12 :         out << YAML::Key << "ActionsJointCount" << YAML::Value << YAML::BeginMap;
     203          49 :         for (const std::string action : actionsNamesOrdered ){
     204             : 
     205          37 :             out << YAML::Key << action << YAML::Value << YAML::BeginMap;
     206             :                 
     207         261 :                 for ( const auto joint : actionsJointCountMap.at(action) ) {
     208         224 :                     out << YAML::Key << joint.first;
     209         224 :                     out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
     210             :                 }
     211          37 :             out << YAML::EndMap;
     212             :         }
     213          12 :         out << YAML::EndMap;
     214             :         
     215             : 
     216             : 
     217             :         
     218          12 :         out << YAML::EndMap; // map began at the beginning of the function
     219          12 :     out << YAML::EndMap;// map began at the beginning of the function
     220          12 : }
     221             : 
     222           8 : bool ROSEE::ActionTimed::fillFromYaml(YAML::const_iterator yamlIt){
     223             :     
     224           8 :     name = yamlIt->first.as<std::string>();
     225           8 :     type = Action::Type::Timed;
     226             :     
     227          80 :     for (auto keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue ) {
     228             : 
     229          64 :         std::string key = keyValue->first.as<std::string>();
     230             :         
     231          64 :         if ( key.compare ("FingersInvolved") == 0 ) { 
     232          16 :             auto tempVect = keyValue->second.as <std::vector <std::string> > ();
     233           8 :             fingersInvolved.insert ( tempVect.begin(), tempVect.end() );
     234             :             
     235          56 :         } else if ( key.compare ("Type") == 0 ) {
     236           8 :             if (ROSEE::Action::Type::Timed != static_cast<ROSEE::Action::Type> ( keyValue->second.as <unsigned int>() )) {
     237           0 :                 std::cout << "[Timed ACTION::" << __func__ << "] Error, found type  " << keyValue->second.as <unsigned int>()
     238           0 :                 << "instead of Timed type (" << ROSEE::Action::Type::Timed << ")" << std::endl;
     239           0 :                 return false;
     240             :             }
     241           8 :             type = ROSEE::Action::Type::Timed;
     242             :             
     243          48 :         } else if ( key.compare ("ActionsNamesOrdered") == 0 ) {
     244           8 :             actionsNamesOrdered = keyValue->second.as < std::vector <std::string> > ();
     245             :             
     246          40 :         } else if ( key.compare ("JointsInvolvedCount") == 0 ) {
     247           8 :             jointsInvolvedCount = keyValue->second.as < JointsInvolvedCount >(); 
     248             :             
     249          32 :         } else if ( key.compare("ActionsTimeMargins") == 0 ) {
     250             :             
     251          44 :             for (auto tMargins = keyValue->second.begin(); tMargins != keyValue->second.end(); ++tMargins ) {
     252             : 
     253          28 :                 std::string actNAme = tMargins->first.as<std::string>();
     254          28 :                 double before = tMargins->second["marginBefore"].as<double>();
     255          28 :                 double after = tMargins->second["marginAfter"].as<double>();
     256          28 :                 actionsTimeMarginsMap.insert (std::make_pair (actNAme, std::make_pair(before, after)  ) ) ;
     257             :             }
     258             :             
     259          24 :         } else if ( key.compare("ActionsJointPos") == 0) {
     260             :             
     261          44 :             for (auto jPos = keyValue->second.begin(); jPos != keyValue->second.end(); ++jPos ) {
     262             :                 
     263          56 :                 std::string actName = jPos->first.as<std::string>();
     264          28 :                 JointPos jp = jPos->second.as<ROSEE::JointPos>();
     265          28 :                 actionsJointPosMap.insert (std::make_pair (actName, jp) );
     266             :             }
     267             : 
     268          16 :         } else if ( key.compare("ActionsJointCount") == 0) {
     269             :             
     270          44 :             for (auto jCount = keyValue->second.begin(); jCount != keyValue->second.end(); ++jCount ) {
     271             :                 
     272          56 :                 std::string actName = jCount->first.as<std::string>();
     273          28 :                 JointsInvolvedCount jc = jCount->second.as<ROSEE::JointsInvolvedCount>();
     274          28 :                 actionsJointCountMap.insert (std::make_pair (actName, jc) );
     275             :             }
     276             : 
     277             :             
     278           8 :         } else if ( key.compare("ActionsJointPosFinal") == 0) {
     279             :             
     280           8 :             jointPosFinal =  keyValue->second.as < JointPos >();
     281             :         
     282             :         } else {
     283           0 :             std::cerr << "[TIMEDACTION::" << __func__ << "] Error, not known key " << key << std::endl;
     284           0 :             return false;
     285             :         }
     286             :     }
     287             :     
     288           8 :     return true;
     289             : }
     290             : 
     291          37 : bool ROSEE::ActionTimed::insertAction(ROSEE::Action::Ptr action, double marginBefore, double marginAfter, 
     292             :                                       unsigned int jointPosIndex, double percentJointPos, std::string newActionName) {
     293             :     
     294          74 :     std::string usedName = (newActionName.empty()) ? action->getName() : newActionName;
     295             : 
     296          37 :     unsigned int count = actionsJointPosMap.count( usedName );
     297             : 
     298          37 :     if (count > 0) {
     299           0 :         std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: " << usedName << " already present. Failed Insertion" << std::endl;
     300           0 :         return false;
     301             :     }
     302             : 
     303          37 :     if (marginAfter < 0 || marginBefore < 0) {
     304           0 :         std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: Please pass positive time margins" << std::endl;
     305           0 :         return false;
     306             :     }
     307             :     
     308          37 :     if ( jointPosIndex > action->getAllJointPos().size()-1 ) {
     309           0 :         std::cerr << "[ACTIONTIMED:: " << __func__ << "] ERROR: you pass index "<< jointPosIndex <<
     310           0 :             " but there are only " << action->getAllJointPos().size() << " JointPos in the action passed as argument" << std::endl;
     311           0 :         return false;
     312             :     }
     313             :     
     314          37 :     if (percentJointPos < 0 || percentJointPos > 1) {
     315           0 :         std::cerr << "[ACTIONTIMED:: " << __func__ << "] Please insert percentage for jointpos between 0 and 1. Passed: " 
     316           0 :             << percentJointPos << std::endl;
     317           0 :         return false;
     318             :     }
     319             :     
     320          62 :     if ( actionsNamesOrdered.size() > 0 && 
     321          62 :         (! ROSEE::Utils::keys_equal(action->getAllJointPos().at(jointPosIndex), actionsJointPosMap.begin()->second)) ) {
     322             :         //we need only to compare the first jointPos (.begin())
     323             :         std::cerr << "[ACTIONTIMED:: " << __func__ << "] The action passed as argument has different keys in jointPosmap" 
     324           0 :                   << " respect to the others inserted in this timed action " << std::endl;
     325           0 :         return false;
     326             :     }
     327             : 
     328          74 :     ROSEE::JointPos insertingJP = (percentJointPos)*(action->getAllJointPos().at( jointPosIndex )) ;
     329          37 :     actionsJointPosMap.insert (std::make_pair ( usedName, insertingJP) );
     330          37 :     actionsTimeMarginsMap.insert ( std::make_pair( usedName, std::make_pair(marginBefore, marginAfter)));
     331          37 :     actionsNamesOrdered.push_back ( usedName );
     332          37 :     actionsJointCountMap.insert (std::make_pair (usedName, action->getJointsInvolvedCount()));
     333             : 
     334             :     //father member
     335          93 :     for ( auto it: action->getFingersInvolved() ) {
     336          56 :          fingersInvolved.insert ( it );
     337             :     }
     338             : 
     339          37 :     if (actionsNamesOrdered.size() == 1 ) { //We are inserting first action, we have to init the JointsInvolvedCount map 
     340             :         
     341          12 :         jointsInvolvedCount = action->getJointsInvolvedCount();
     342          12 :         jointPosFinal = insertingJP;
     343             :         
     344             :     } else { 
     345             :         // add the action.jointInvolvedCount to the timed jointCount
     346             :         // and update jointPosFinal 
     347         189 :         for (auto jic : action->getJointsInvolvedCount() ) {
     348         164 :             jointsInvolvedCount.at(jic.first) += jic.second;
     349             :             
     350         164 :             if (jic.second > 0) {
     351             :                 //if so, we must overwrite the pos of this joint in jointPosFina
     352          95 :                 jointPosFinal.at(jic.first) = insertingJP.at(jic.first);
     353             :             }
     354             :         } 
     355             :     }
     356             : 
     357          37 :     return true;
     358             : }
     359             : 
     360             :