Line data    Source code

       1             : /*
       2             :  * Copyright (C) 2020 IIT-HHCM
       3             :  * Author: Davide Torielli
       4             :  * email:  davide.torielli@iit.it
       5             :  *
       6             :  * Licensed under the Apache License, Version 2.0 (the "License");
       7             :  * you may not use this file except in compliance with the License.
       8             :  * You may obtain a copy of the License at
       9             :  *
      10             :  *     http://www.apache.org/licenses/LICENSE-2.0
      11             :  *
      12             :  * Unless required by applicable law or agreed to in writing, software
      13             :  * distributed under the License is distributed on an "AS IS" BASIS,
      14             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      15             :  * See the License for the specific language governing permissions and
      16             :  * limitations under the License.
      17             :  */
      18             : 
      19             : #include <end_effector/GraspingActions/ActionMultiplePinchTight.h>
      20             : 
      21           0 : ROSEE::ActionMultiplePinchTight::ActionMultiplePinchTight() : 
      22           0 :     ActionPinchGeneric ("multiplePinchTight", 3, ActionPrimitive::Type::MultiplePinchTight) { }
      23             : 
      24           0 : ROSEE::ActionMultiplePinchTight::ActionMultiplePinchTight(unsigned int maxStoredActionStates) : 
      25           0 :     ActionPinchGeneric ("multiplePinchTight", maxStoredActionStates, ActionPrimitive::Type::MultiplePinchTight) { }
      26             : 
      27           6 : ROSEE::ActionMultiplePinchTight::ActionMultiplePinchTight (
      28             :     std::set <std::string> fingerNamesSet, 
      29           6 :     JointPos jp, double depthSum) :
      30          12 :     ActionPinchGeneric ( ("multiplePinchTight_" + std::to_string(fingerNamesSet.size())),
      31          18 :                         fingerNamesSet.size(), 3, ActionPrimitive::Type::MultiplePinchTight )  {
      32             : 
      33             :     //different from insertState, here we are sure the set is empty (we are in costructor)
      34           6 :     fingersInvolved = fingerNamesSet;
      35           6 :     actionStates.insert (std::make_pair (jp, depthSum) );
      36           6 : }
      37             : 
      38             : 
      39           0 : ROSEE::JointPos ROSEE::ActionMultiplePinchTight::getJointPos() const {
      40           0 :     return (actionStates.begin()->first);
      41             : }
      42             : 
      43           0 : ROSEE::JointPos ROSEE::ActionMultiplePinchTight::getJointPos( unsigned int index) const {
      44           0 :     auto it = actionStates.begin();
      45           0 :     unsigned int i = 1;
      46           0 :     while (i < index ) {
      47           0 :         ++ it;
      48             :     }
      49           0 :     return (it->first);
      50             : }
      51             : 
      52           0 : std::vector < ROSEE::JointPos > ROSEE::ActionMultiplePinchTight::getAllJointPos() const{
      53             :     
      54           0 :     std::vector < JointPos > retVect;
      55           0 :     retVect.reserve ( actionStates.size() );
      56             :     
      57           0 :     for (auto it : actionStates ) {
      58           0 :         retVect.push_back(it.first);
      59             :     }
      60             :     
      61           0 :     return retVect;
      62             : }
      63             : 
      64             : 
      65           0 : std::vector < ROSEE::ActionMultiplePinchTight::StateWithDepth > ROSEE::ActionMultiplePinchTight::getActionStates () const {
      66             :     
      67           0 :     std::vector < ROSEE::ActionMultiplePinchTight::StateWithDepth > retVect;
      68           0 :     retVect.reserve ( actionStates.size() );
      69             :     
      70           0 :     for (auto it : actionStates ) {
      71           0 :         retVect.push_back(it);
      72             :     }
      73             :     
      74           0 :     return retVect;
      75             :     
      76             : }
      77             : 
      78           5 : bool ROSEE::ActionMultiplePinchTight::insertActionState (
      79             :     ROSEE::JointPos jp, double depthSum) {
      80             : 
      81           5 :     auto pairRet = actionStates.insert ( std::make_pair (jp, depthSum) ) ;
      82             :     
      83           5 :     if (! pairRet.second ) {
      84             :         //TODO print error no insertion because depth is equal... very improbable
      85           0 :         return false;
      86             :     }
      87             :     
      88           5 :     if (actionStates.size() > maxStoredActionStates) { 
      89             :         //max capacity reached, we have to delete the last one
      90           3 :         auto it = pairRet.first;        
      91             :         
      92           3 :         if ( (++it) == actionStates.end() ){
      93             :            // the new inserted is the last one and has to be erased
      94           1 :             actionStates.erase(pairRet.first);
      95           1 :             return false;
      96             :         }
      97             :         
      98             :         // the new inserted is not the last one that has to be erased
      99           2 :         auto lastElem = actionStates.end();
     100           2 :         --lastElem;
     101           2 :         actionStates.erase(lastElem);
     102             :     }
     103             :     
     104           4 :     return true;
     105             : }
     106             : 
     107             : 
     108           0 : void ROSEE::ActionMultiplePinchTight::print () const {
     109             :     
     110           0 :     std::stringstream output;
     111           0 :     output << "ActionName: " << name << std::endl;
     112             :     
     113           0 :     output << "FingersInvolved: [";
     114           0 :     for (auto fingName : fingersInvolved){
     115           0 :         output << fingName << ", " ;
     116             :     }
     117           0 :     output.seekp (-2, output.cur); //to remove the last comma (and space)
     118           0 :     output << "]" << std::endl;
     119             :     
     120           0 :     output << "JointsInvolvedCount: " << std::endl;;
     121           0 :     output << jointsInvolvedCount << std::endl;
     122             :     
     123           0 :     unsigned int nActState = 1;
     124           0 :     for (auto itemSet : actionStates) {  //the element in the set
     125           0 :         output << "Action_State_" << nActState << " :" << std::endl;
     126             : 
     127           0 :         output << "\t" << "JointStates:" << std::endl;
     128           0 :         output << itemSet.first;
     129           0 :         output << "\t" << "DepthSum:" <<  itemSet.second <<std::endl;
     130             :             
     131           0 :         nActState++;
     132             :     }
     133           0 :     output << std::endl;
     134             :     
     135           0 :     std::cout << output.str();
     136             : 
     137           0 : }
     138             : 
     139             : 
     140           1 : void ROSEE::ActionMultiplePinchTight::emitYaml ( YAML::Emitter& out ) const {
     141             :     
     142           1 :     out << YAML::Key << YAML::Flow << fingersInvolved;
     143             :     
     144           1 :     unsigned int nCont = 1;
     145           1 :     out << YAML::Value << YAML::BeginMap;
     146           1 :     out << YAML::Key << "PrimitiveType" << YAML::Value << primitiveType;
     147           1 :     out << YAML::Key << "ActionName" << YAML::Value << name;
     148           1 :     out << YAML::Key << "JointsInvolvedCount" << YAML::Value << YAML::BeginMap;
     149          10 :     for (const auto &jointCount : jointsInvolvedCount ) {
     150           9 :         out << YAML::Key << jointCount.first;
     151           9 :         out << YAML::Value << jointCount.second;
     152             :     }
     153           1 :     out << YAML::EndMap;
     154             :     
     155           4 :     for (const auto & actionState : actionStates) { //.second is the set of ActionState
     156             :         
     157           3 :         std::string contSeq = "ActionState_" + std::to_string(nCont);
     158           3 :         out << YAML::Key << contSeq; 
     159             :         
     160           3 :         out << YAML::Value << YAML::BeginMap;
     161             :             //actionState.first, the jointstates map
     162           3 :             out << YAML::Key << "JointPos" << YAML::Value << YAML::BeginMap;
     163          30 :             for (const auto &joint : actionState.first) {
     164          27 :                 out << YAML::Key << joint.first;
     165          27 :                 out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
     166             :             }
     167           3 :             out << YAML::EndMap;
     168             :             
     169             :              //actionState.second, the optional
     170           3 :             out << YAML::Key << "Optional" << YAML::Value << YAML::BeginMap;
     171           3 :                 out << YAML::Key << "DepthSum" << YAML::Value << actionState.second;
     172           3 :             out << YAML::EndMap;
     173             :             
     174           3 :         out << YAML::EndMap;
     175           3 :         nCont++;
     176             :     }
     177           1 :     out << YAML::EndMap;
     178             : 
     179           1 : }
     180             : 
     181             : 
     182           0 : bool ROSEE::ActionMultiplePinchTight::fillFromYaml ( YAML::const_iterator yamlIt ) {
     183             :         
     184           0 :     std::vector <std::string> fingInvolvedVect = yamlIt->first.as <std::vector < std::string >> ();
     185           0 :     for (const auto &it : fingInvolvedVect) {
     186           0 :         fingersInvolved.insert(it);
     187             :     }
     188             : 
     189           0 :     for ( YAML::const_iterator keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue) {        
     190             :         
     191           0 :         std::string key = keyValue->first.as<std::string>();
     192           0 :         if (key.compare("JointsInvolvedCount") == 0) {
     193           0 :             jointsInvolvedCount = keyValue->second.as < JointsInvolvedCount > ();
     194             :             
     195           0 :         } else if (key.compare ("ActionName") == 0 ) {
     196           0 :             name = keyValue->second.as <std::string> ();
     197             :         
     198           0 :         } else if (key.compare ("PrimitiveType") == 0) {
     199             :             ROSEE::ActionPrimitive::Type parsedType = static_cast<ROSEE::ActionPrimitive::Type> ( 
     200           0 :                 keyValue->second.as <unsigned int>() );
     201           0 :             if (parsedType != primitiveType ) {
     202           0 :                 std::cerr << "[ERROR ActionMultPinch::" << __func__ << " parsed a type " << parsedType << 
     203           0 :                     " but this object has primitive type " << primitiveType << std::endl; 
     204           0 :                 return false;
     205             :             }
     206             :             
     207           0 :         } else if (key.compare(0, 12, "ActionState_") == 0) { //compare 12 caracters from index 0 of key
     208             : 
     209           0 :             JointPos jointPos;
     210             :             double depthSum;
     211           0 :             for(YAML::const_iterator asEl = keyValue->second.begin(); asEl != keyValue->second.end(); ++asEl) {
     212             : 
     213             :                 //asEl can be the map JointPos or the map Optional
     214           0 :                 if (asEl->first.as<std::string>().compare ("JointPos") == 0 ) {
     215           0 :                     jointPos = asEl->second.as < JointPos >(); 
     216             :                     
     217           0 :                 } else if (asEl->first.as<std::string>().compare ("Optional") == 0 ) {
     218           0 :                     depthSum = asEl->second["DepthSum"].as < double >();
     219             :                     
     220             :                 } else {
     221             :                     //ERRROr, only JointPos and Optional at this level
     222             :                     std::cerr << "[ERROR ActionMultPinch::" << __func__ << "not know key " 
     223           0 :                         << asEl->first.as<std::string>() << 
     224           0 :                         " found in the yaml file at this level" << std::endl; 
     225           0 :                     return false;
     226             :                 }
     227             :             }  
     228           0 :             actionStates.insert ( std::make_pair (jointPos, depthSum));
     229             :             
     230             :         } else {
     231             :             std::cerr << "[ERROR ActionMultPinch::" << __func__ << "not know key " << key << 
     232           0 :                 " found in the yaml file" << std::endl; 
     233           0 :             return false;
     234             :         }
     235             :     }
     236             :     
     237           0 :     nFingersInvolved = fingersInvolved.size();
     238             :     
     239           0 :     return true;
     240             : }