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/ActionPinchLoose.h>
      20             : 
      21          11 : ROSEE::ActionPinchLoose::ActionPinchLoose() : 
      22          11 :     ActionPinchGeneric ("pinchLoose", 2, 3, ActionPrimitive::Type::PinchLoose) { }
      23             : 
      24           0 : ROSEE::ActionPinchLoose::ActionPinchLoose (unsigned int maxStoredActionStates) : 
      25           0 :     ActionPinchGeneric ("pinchLoose", 2, maxStoredActionStates, ActionPrimitive::Type::PinchLoose) { }
      26             : 
      27         272 : ROSEE::ActionPinchLoose::ActionPinchLoose (std::string tip1, std::string tip2) : 
      28         272 :     ActionPinchGeneric ("pinchLoose", 2, 3, ActionPrimitive::Type::PinchLoose) {
      29         272 :         fingersInvolved.insert (tip1);
      30         272 :         fingersInvolved.insert (tip2);
      31         272 : }
      32             :     
      33           0 : ROSEE::ActionPinchLoose::ActionPinchLoose (std::pair <std::string, std::string> tipNames, 
      34           0 :     JointPos jp, double distance) :
      35           0 :     ActionPinchGeneric ("pinchLoose", 2, 3, ActionPrimitive::Type::PinchLoose )  {
      36             : 
      37           0 :     fingersInvolved.insert (tipNames.first);
      38           0 :     fingersInvolved.insert (tipNames.second);
      39             :     
      40             :     //different from insertState, here we are sure the set is empty (we are in costructor)
      41           0 :     actionStates.insert (std::make_pair (jp, distance) );
      42           0 : }
      43             : 
      44           7 : std::vector < ROSEE::ActionPinchLoose::StateWithDistance > ROSEE::ActionPinchLoose::getActionStates() const {
      45             :     
      46           7 :     std::vector < ROSEE::ActionPinchLoose::StateWithDistance > retVect;
      47           7 :     retVect.reserve ( actionStates.size() );
      48             :     
      49          28 :     for (auto it : actionStates ) {
      50          21 :         retVect.push_back(it);
      51             :     }
      52             :     
      53           7 :     return retVect;
      54             : }
      55             : 
      56           0 : ROSEE::JointPos ROSEE::ActionPinchLoose::getJointPos() const {
      57           0 :     return (actionStates.begin()->first);
      58             : }
      59             : 
      60           0 : ROSEE::JointPos ROSEE::ActionPinchLoose::getJointPos(unsigned int index) const {
      61           0 :     auto it = actionStates.begin();
      62           0 :     unsigned int i = 1;
      63           0 :     while (i < index ) {
      64           0 :         ++ it;
      65           0 :         ++ i;
      66             :     }
      67           0 :     return (it->first);
      68             : }
      69             : 
      70          14 : std::vector < ROSEE::JointPos > ROSEE::ActionPinchLoose::getAllJointPos() const {
      71             :     
      72          14 :     std::vector < JointPos > retVect;
      73          14 :     retVect.reserve(actionStates.size());
      74             :     
      75          56 :     for (auto it : actionStates ) {
      76          42 :         retVect.push_back(it.first);
      77             :     }
      78             :     
      79          14 :     return retVect;
      80             : }
      81             : 
      82       85000 : bool ROSEE::ActionPinchLoose::insertActionState (ROSEE::JointPos jp, double dist) {
      83             : 
      84       85000 :     auto pairRet = actionStates.insert ( std::make_pair (jp, dist) ) ;
      85             :     
      86       85000 :     if (actionStates.size() > maxStoredActionStates) { 
      87             :         //max capacity reached, we have to delete the last one
      88       84949 :         auto it = pairRet.first;        
      89             :         
      90       84949 :         if (++(it) == actionStates.end() ){
      91             :            // the new inserted is the last one and has to be erased
      92       84545 :             actionStates.erase(pairRet.first);
      93       84545 :             return false;
      94             :         }
      95             :         
      96             :         // the new inserted is not the last one that has to be erased
      97         404 :         auto lastElem = actionStates.end();
      98         404 :         --lastElem;
      99         404 :         actionStates.erase(lastElem);
     100             :     }
     101             :     
     102         455 :     return true;
     103             : }
     104             : 
     105             : 
     106           0 : void ROSEE::ActionPinchLoose::print () const {
     107             :     
     108           0 :     std::stringstream output;
     109             :     
     110           0 :     output << "ActionName: " << name << std::endl;
     111             :     
     112           0 :     output << "FingersInvolved: [";
     113           0 :     for (auto fingName : fingersInvolved){
     114           0 :         output << fingName << ", " ;
     115             :     }
     116           0 :     output.seekp (-2, output.cur); //to remove the last comma (and space)
     117           0 :     output << "]" << std::endl;
     118             :     
     119           0 :     output << "JointsInvolvedCount: " << std::endl;;
     120           0 :     output << jointsInvolvedCount << std::endl;
     121             : 
     122           0 :     unsigned int nActState = 1;
     123           0 :     for (auto itemSet : actionStates) {  //the element in the set
     124           0 :         output << "Action_State_" << nActState << " :" << std::endl;
     125             : 
     126           0 :         output << "\t" << "JointStates:" << std::endl;
     127           0 :         output << itemSet.first;
     128           0 :         output << "\t" << "Distance:" << std::endl;
     129           0 :         output << "\t\tdistance " << 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          17 : void ROSEE::ActionPinchLoose::emitYaml ( YAML::Emitter& out ) const {
     141             :     
     142          17 :     out << YAML::Key << YAML::Flow << fingersInvolved;
     143             : 
     144          17 :     unsigned int nCont = 1;
     145          17 :     out << YAML::Value << YAML::BeginMap;
     146          17 :     out << YAML::Key << "PrimitiveType" << YAML::Value << primitiveType;
     147          17 :     out << YAML::Key << "ActionName" << YAML::Value << name;
     148          17 :     out << YAML::Key << "JointsInvolvedCount" << YAML::Value << YAML::BeginMap;
     149          34 :     for (const auto &jointCount : jointsInvolvedCount ) {
     150          17 :         out << YAML::Key << jointCount.first;
     151          17 :         out << YAML::Value << jointCount.second;
     152             :     }
     153          17 :     out << YAML::EndMap;
     154             :     
     155          68 :     for (const auto & actionState : actionStates) { //.second is the set of ActionState
     156             :         
     157          51 :         std::string contSeq = "ActionState_" + std::to_string(nCont);
     158          51 :         out << YAML::Key << contSeq; 
     159             :         
     160          51 :         out << YAML::Value << YAML::BeginMap;
     161             :             //actionState.first, the jointstates map
     162          51 :             out << YAML::Key << "JointStates" << YAML::Value << YAML::BeginMap;
     163         102 :             for (const auto &joint : actionState.first) {
     164          51 :                 out << YAML::Key << joint.first;
     165          51 :                 out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
     166             :             }
     167          51 :             out << YAML::EndMap;
     168             :             
     169             :             //actionState.second, the optional
     170          51 :             out << YAML::Key << "Optional" << YAML::Value << YAML::BeginMap;
     171          51 :                 out << YAML::Key << "distance" << YAML::Value << actionState.second;
     172          51 :             out << YAML::EndMap;
     173             :             
     174          51 :         out << YAML::EndMap;
     175          51 :         nCont++;
     176             :     }
     177          17 :     out << YAML::EndMap;
     178             : 
     179          17 : }
     180             : 
     181             : 
     182          11 : bool ROSEE::ActionPinchLoose::fillFromYaml ( YAML::const_iterator yamlIt ) {
     183             :         
     184          22 :     std::vector <std::string> fingInvolvedVect = yamlIt->first.as <std::vector < std::string >> ();
     185          33 :     for (const auto &it : fingInvolvedVect) {
     186          22 :         fingersInvolved.insert(it);
     187             :     }
     188             : 
     189          88 :     for ( YAML::const_iterator keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue) {        
     190             :         
     191          66 :         std::string key = keyValue->first.as<std::string>();
     192          66 :         if ( key.compare("JointsInvolvedCount") == 0 ) {
     193          11 :             jointsInvolvedCount = keyValue->second.as < JointsInvolvedCount > ();
     194             :             
     195          55 :         } else if (key.compare ("ActionName") == 0 ) {
     196          11 :             name = keyValue->second.as <std::string> ();
     197             :         
     198          44 :         } else if (key.compare ("PrimitiveType") == 0) {
     199             :             ROSEE::ActionPrimitive::Type parsedType = static_cast<ROSEE::ActionPrimitive::Type> ( 
     200          11 :                 keyValue->second.as <unsigned int>() );
     201          11 :             if (parsedType != primitiveType ) {
     202           0 :                 std::cerr << "[ERROR ActionPinchLoose::" << __func__ << " parsed a type " << parsedType << 
     203           0 :                     " but this object has primitive type " << primitiveType << std::endl; 
     204           0 :                 return false;
     205             :             }
     206             :             
     207          33 :         } else if (key.compare(0, 12, "ActionState_") == 0) { //compare 12 caracters from index 0 of key
     208             : 
     209          33 :             JointPos jointPos;
     210             :             double distance;
     211         132 :             for(YAML::const_iterator asEl = keyValue->second.begin(); asEl != keyValue->second.end(); ++asEl) {
     212             : 
     213             :                 //asEl can be the map JointStates or the map Optional
     214          66 :                 if (asEl->first.as<std::string>().compare ("JointStates") == 0 ) {
     215          33 :                     jointPos = asEl->second.as < JointPos >(); 
     216             :                     
     217          33 :                 } else if (asEl->first.as<std::string>().compare ("Optional") == 0 ) {
     218          33 :                     distance = asEl->second["distance"].as < double >();
     219             :                     
     220             :                 } else {
     221             :                     //ERRROr, only joinstates and optional at this level
     222             :                     std::cerr << "[ERROR ActionPinchLoose::" << __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          33 :             actionStates.insert ( std::make_pair (jointPos, distance));
     229             :             
     230             :         } else {
     231             :             std::cerr << "[ERROR ActionPinchLoose::" << __func__ << "not know key " << key << 
     232           0 :                 " found in the yaml file" << std::endl; 
     233           0 :             return false;
     234             :         }
     235             :     }
     236             :     
     237          11 :     return true;
     238             : }
     239             : 
     240             :