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/ActionPinchTight.h>
      20             : 
      21          44 : ROSEE::ActionPinchTight::ActionPinchTight() : 
      22          44 :     ActionPinchGeneric ("pinchTight", 2, 3, ActionPrimitive::Type::PinchTight) { }
      23             : 
      24           0 : ROSEE::ActionPinchTight::ActionPinchTight(unsigned int jointStateSetMaxSize) : 
      25           0 :     ActionPinchGeneric ("pinchTight", 2, jointStateSetMaxSize, ActionPrimitive::Type::PinchTight) { }
      26             : 
      27       14390 : ROSEE::ActionPinchTight::ActionPinchTight (std::pair <std::string, std::string> fingerNamesPair, 
      28       14390 :     JointPos jp, collision_detection::Contact cont) :
      29       14390 :     ActionPinchGeneric ("pinchTight", 2, 3, ActionPrimitive::Type::PinchTight )  {
      30             : 
      31             :     //different from insertState, here we are sure the set is empty (we are in costructor)
      32       14390 :     fingersInvolved.insert(fingerNamesPair.first);
      33       14390 :     fingersInvolved.insert(fingerNamesPair.second);
      34       14390 :     actionStates.insert (std::make_pair (jp, cont) );
      35       14390 : }
      36             : 
      37           0 : ROSEE::ActionPinchTight::ActionPinchTight (std::string finger1, std::string finger2, 
      38           0 :     JointPos jp, collision_detection::Contact cont) :
      39           0 :     ActionPinchGeneric ("pinchTight", 2, 3, ActionPrimitive::Type::PinchTight )  {
      40             : 
      41             :     //different from insertState, here we are sure the set is empty (we are in costructor)
      42           0 :     fingersInvolved.insert(finger1);
      43           0 :     fingersInvolved.insert(finger2);
      44           0 :     actionStates.insert (std::make_pair (jp, cont) );
      45           0 : }
      46             : 
      47           0 : ROSEE::JointPos ROSEE::ActionPinchTight::getJointPos() const {
      48           0 :     return (actionStates.begin()->first);
      49             : }
      50             : 
      51           0 : ROSEE::JointPos ROSEE::ActionPinchTight::getJointPos( unsigned int index) const {
      52           0 :     auto it = actionStates.begin();
      53           0 :     unsigned int i = 1;
      54           0 :     while (i < index ) {
      55           0 :         ++ it;
      56             :     }
      57           0 :     return (it->first);
      58             : }
      59             : 
      60         224 : std::vector < ROSEE::JointPos > ROSEE::ActionPinchTight::getAllJointPos() const{
      61             :     
      62         224 :     std::vector < JointPos > retVect;
      63         224 :     retVect.reserve ( actionStates.size() );
      64             :     
      65         896 :     for (auto it : actionStates ) {
      66         672 :         retVect.push_back(it.first);
      67             :     }
      68             :     
      69         224 :     return retVect;
      70             : }
      71             : 
      72             : 
      73          28 : std::vector < ROSEE::ActionPinchTight::StateWithContact > ROSEE::ActionPinchTight::getActionStates () const {
      74             :     
      75          28 :     std::vector < ROSEE::ActionPinchTight::StateWithContact > retVect;
      76          28 :     retVect.reserve ( actionStates.size() );
      77             :     
      78         112 :     for (auto it : actionStates ) {
      79          84 :         retVect.push_back(it);
      80             :     }
      81             :     
      82          28 :     return retVect;
      83             :     
      84             : }
      85             : 
      86       14322 : bool ROSEE::ActionPinchTight::insertActionState (ROSEE::JointPos jp, collision_detection::Contact cont) {
      87             : 
      88       14322 :     auto pairRet = actionStates.insert ( std::make_pair (jp, cont) ) ;
      89             :     
      90       14322 :     if (! pairRet.second ) {
      91             :         //TODO print error no insertion because depth is equal... very improbable
      92         919 :         return false;
      93             :     }
      94             :     
      95       13403 :     if (actionStates.size() > maxStoredActionStates) { 
      96             :         //max capacity reached, we have to delete the last one
      97       13267 :         auto it = pairRet.first;        
      98             :         
      99       13267 :         if ( (++it) == actionStates.end() ){
     100             :            // the new inserted is the last one and has to be erased
     101       12569 :             actionStates.erase(pairRet.first);
     102       12569 :             return false;
     103             :         }
     104             :         
     105             :         // the new inserted is not the last one that has to be erased
     106         698 :         auto lastElem = actionStates.end();
     107         698 :         --lastElem;
     108         698 :         actionStates.erase(lastElem);
     109             :     }
     110             :     
     111         834 :     return true;
     112             : }
     113             : 
     114             : 
     115           0 : void ROSEE::ActionPinchTight::print () const {
     116             :     
     117           0 :     std::stringstream output;
     118           0 :     output << "ActionName: " << name << std::endl;
     119             :     
     120           0 :     output << "FingersInvolved: [";
     121           0 :     for (auto fingName : fingersInvolved){
     122           0 :         output << fingName << ", " ;
     123             :     }
     124           0 :     output.seekp (-2, output.cur); //to remove the last comma (and space)
     125           0 :     output << "]" << std::endl;
     126             :     
     127           0 :     output << "JointsInvolvedCount: " << std::endl;;
     128           0 :     output << jointsInvolvedCount << std::endl;
     129             :     
     130           0 :     unsigned int nActState = 1;
     131           0 :     for (auto itemSet : actionStates) {  //the element in the set
     132           0 :         output << "Action_State_" << nActState << " :" << std::endl;
     133             : 
     134           0 :         output << "\t" << "JointStates:" << std::endl;
     135           0 :         output << itemSet.first;
     136           0 :         output << "\t" << "MoveitContact:" << std::endl;
     137           0 :         output << "\t\tbody_name_1: " << itemSet.second.body_name_1 << std::endl;
     138           0 :         output << "\t\tbody_name_2: " << itemSet.second.body_name_2 << std::endl;
     139           0 :         output << "\t\tbody_type_1: " << itemSet.second.body_type_1 << std::endl;
     140           0 :         output << "\t\tbody_type_2: " << itemSet.second.body_type_2 << std::endl;
     141           0 :         output << "\t\tdepth: " << itemSet.second.depth << std::endl;
     142           0 :         output << "\t\tnormal: " << "["<< itemSet.second.normal.x() << ", " 
     143           0 :             << itemSet.second.normal.y() << ", " << itemSet.second.normal.z() << "]" << std::endl;
     144           0 :         output << "\t\tpos: " << "["<< itemSet.second.pos.x() << ", " 
     145           0 :             << itemSet.second.pos.y() << ", " << itemSet.second.pos.z() << "]" << std::endl;
     146             :             
     147           0 :         nActState++;
     148             :     }
     149           0 :     output << std::endl;
     150             :     
     151           0 :     std::cout << output.str();
     152             : 
     153           0 : }
     154             : 
     155             : 
     156          68 : void ROSEE::ActionPinchTight::emitYaml ( YAML::Emitter& out ) const {
     157             :     
     158          68 :     out << YAML::Key << YAML::Flow << fingersInvolved;
     159             :     
     160          68 :     unsigned int nCont = 1;
     161          68 :     out << YAML::Value << YAML::BeginMap;
     162          68 :     out << YAML::Key << "PrimitiveType" << YAML::Value << primitiveType;
     163          68 :     out << YAML::Key << "ActionName" << YAML::Value << name;
     164          68 :     out << YAML::Key << "JointsInvolvedCount" << YAML::Value << YAML::BeginMap;
     165         595 :     for (const auto &jointCount : jointsInvolvedCount ) {
     166         527 :         out << YAML::Key << jointCount.first;
     167         527 :         out << YAML::Value << jointCount.second;
     168             :     }
     169          68 :     out << YAML::EndMap;
     170             :     
     171         272 :     for (const auto & actionState : actionStates) { //.second is the set of ActionState
     172             :         
     173         204 :         std::string contSeq = "ActionState_" + std::to_string(nCont);
     174         204 :         out << YAML::Key << contSeq; 
     175             :         
     176         204 :         out << YAML::Value << YAML::BeginMap;
     177             :             //actionState.first, the jointstates map
     178         204 :             out << YAML::Key << "JointPos" << YAML::Value << YAML::BeginMap;
     179        1785 :             for (const auto &joint : actionState.first) {
     180        1581 :                 out << YAML::Key << joint.first;
     181        1581 :                 out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
     182             :             }
     183         204 :             out << YAML::EndMap;
     184             :             
     185             :             //actionState.second, the optional
     186         204 :             out << YAML::Key << "Optional" << YAML::Value;
     187         204 :             emitYamlForContact ( actionState.second, out );
     188             :             
     189         204 :         out << YAML::EndMap;
     190         204 :         nCont++;
     191             :     }
     192          68 :     out << YAML::EndMap;
     193             : 
     194          68 : }
     195             : 
     196             : 
     197          44 : bool ROSEE::ActionPinchTight::fillFromYaml ( YAML::const_iterator yamlIt ) {
     198             :         
     199          88 :     std::vector <std::string> fingInvolvedVect = yamlIt->first.as <std::vector < std::string >> ();
     200         132 :     for (const auto &it : fingInvolvedVect) {
     201          88 :         fingersInvolved.insert(it);
     202             :     }
     203             : 
     204         352 :     for ( YAML::const_iterator keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue) {        
     205             :         
     206         264 :         std::string key = keyValue->first.as<std::string>();
     207         264 :         if (key.compare("JointsInvolvedCount") == 0) {
     208          44 :             jointsInvolvedCount = keyValue->second.as < JointsInvolvedCount > ();
     209             :             
     210         220 :         } else if (key.compare ("ActionName") == 0 ) {
     211          44 :             name = keyValue->second.as <std::string> ();
     212             :         
     213         176 :         } else if (key.compare ("PrimitiveType") == 0) {
     214             :             ROSEE::ActionPrimitive::Type parsedType = static_cast<ROSEE::ActionPrimitive::Type> ( 
     215          44 :                 keyValue->second.as <unsigned int>() );
     216          44 :             if (parsedType != primitiveType ) {
     217           0 :                 std::cerr << "[ERROR ActionPinchTight::" << __func__ << " parsed a type " << parsedType << 
     218           0 :                     " but this object has primitive type " << primitiveType << std::endl; 
     219           0 :                 return false;
     220             :             }
     221             :             
     222         132 :         } else if (key.compare(0, 12, "ActionState_") == 0) { //compare 12 caracters from index 0 of key
     223             : 
     224         132 :             JointPos jointPos;
     225         132 :             collision_detection::Contact contact;
     226         528 :             for(YAML::const_iterator asEl = keyValue->second.begin(); asEl != keyValue->second.end(); ++asEl) {
     227             : 
     228             :                 //asEl can be the map JointPos or the map Optional
     229         264 :                 if (asEl->first.as<std::string>().compare ("JointPos") == 0 ) {
     230         132 :                     jointPos = asEl->second.as < JointPos >(); 
     231         132 :                 } else if (asEl->first.as<std::string>().compare ("Optional") == 0 ) {
     232             :                     
     233         264 :                     YAML::Node cont =  asEl->second["MoveItContact"];
     234         132 :                     contact.body_name_1 = cont["body_name_1"].as < std::string >();
     235         132 :                     contact.body_name_2 = cont["body_name_2"].as < std::string >();
     236         132 :                     switch (cont["body_type_1"].as < int >())
     237             :                     {
     238         132 :                     case 0:
     239         132 :                         contact.body_type_1 = collision_detection::BodyType::ROBOT_LINK;
     240         132 :                         break;
     241           0 :                     case 1:
     242           0 :                         contact.body_type_1 = collision_detection::BodyType::ROBOT_ATTACHED;
     243           0 :                         break;
     244           0 :                     case 2:
     245           0 :                         contact.body_type_1 = collision_detection::BodyType::WORLD_OBJECT;
     246           0 :                         break;
     247           0 :                     default:
     248           0 :                         std::cout << "some error, body_type_1" << cont["body_type_1"].as < int >()
     249           0 :                             << "unknown" << std::endl;
     250           0 :                         contact.body_type_1 = collision_detection::BodyType::WORLD_OBJECT;
     251             :                     }
     252         132 :                     switch (cont["body_type_2"].as < int >())
     253             :                     {
     254         132 :                     case 0:
     255         132 :                         contact.body_type_2 = collision_detection::BodyType::ROBOT_LINK;
     256         132 :                         break;
     257           0 :                     case 1:
     258           0 :                         contact.body_type_2 = collision_detection::BodyType::ROBOT_ATTACHED;
     259           0 :                         break;
     260           0 :                     case 2:
     261           0 :                         contact.body_type_2 = collision_detection::BodyType::WORLD_OBJECT;
     262           0 :                         break;
     263           0 :                     default:
     264           0 :                         std::cout << "some error, body_type_2" << cont["body_type_2"].as < int >()
     265           0 :                             << "unknown" << std::endl;
     266           0 :                         contact.body_type_2 = collision_detection::BodyType::WORLD_OBJECT;
     267             :                     }
     268         132 :                     contact.depth = cont["depth"].as<double>();
     269         264 :                     std::vector < double > normVect = cont["normal"].as < std::vector <double> >();
     270         132 :                     std::vector < double > posVect = cont["pos"].as < std::vector <double> >();
     271         132 :                     contact.normal = Eigen::Vector3d (normVect.data() );
     272         132 :                     contact.pos = Eigen::Vector3d (posVect.data() );
     273             :                     
     274             :                 } else {
     275             :                     //ERRROr, only joinstates and optional at this level
     276             :                     std::cerr << "[ERROR ActionPinchTight::" << __func__ << "not know key " 
     277           0 :                         << asEl->first.as<std::string>() << 
     278           0 :                         " found in the yaml file at this level" << std::endl; 
     279           0 :                     return false;
     280             :                 }
     281             :             }  
     282         132 :             actionStates.insert ( std::make_pair (jointPos, contact));
     283             :             
     284             :         } else {
     285             :             std::cerr << "[ERROR ActionPinchTight::" << __func__ << "not know key " << key << 
     286           0 :                 " found in the yaml file" << std::endl; 
     287           0 :             return false;
     288             :         }
     289             :     }
     290             :     
     291          44 :     return true;
     292             : }
     293             : 
     294         204 : bool ROSEE::ActionPinchTight::emitYamlForContact (collision_detection::Contact moveitContact, YAML::Emitter& out) const {
     295             : 
     296         204 :     out << YAML::BeginMap;
     297         204 :         out << YAML::Key << "MoveItContact" << YAML::Value << YAML::BeginMap;
     298         204 :             out << YAML::Key << "body_name_1";
     299         204 :             out << YAML::Value << moveitContact.body_name_1;
     300         204 :             out << YAML::Key << "body_name_2";
     301         204 :             out << YAML::Value << moveitContact.body_name_2;
     302         204 :             out << YAML::Key << "body_type_1";
     303         204 :             out << YAML::Value << moveitContact.body_type_1;
     304         204 :             out << YAML::Key << "body_type_2";
     305         204 :             out << YAML::Value << moveitContact.body_type_2;
     306         204 :             out << YAML::Key << "depth";
     307         204 :             out << YAML::Value << moveitContact.depth;
     308         204 :             out << YAML::Key << "normal";
     309         408 :             std::vector < double > normal ( moveitContact.normal.data(), moveitContact.normal.data() +  moveitContact.normal.rows());  
     310         204 :             out << YAML::Value << YAML::Flow << normal;
     311         204 :             out << YAML::Key << "pos";
     312         204 :             std::vector < double > pos ( moveitContact.pos.data(), moveitContact.pos.data() +  moveitContact.pos.rows());
     313         204 :             out << YAML::Value << YAML::Flow << pos;
     314         204 :         out << YAML::EndMap;
     315         204 :     out << YAML::EndMap;
     316             :     
     317         408 :     return true;
     318             : }