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/ActionGeneric.h>
      18             : 
      19             : 
      20          21 : ROSEE::ActionGeneric::ActionGeneric () {}
      21             : 
      22          24 : ROSEE::ActionGeneric::ActionGeneric(std::string actionName) : Action (actionName, Action::Type::Generic) {}
      23             : 
      24           0 : ROSEE::ActionGeneric::ActionGeneric(std::string actionName, ROSEE::JointPos jointPos) : Action(actionName, Action::Type::Generic) {
      25             :     
      26             :     //HACK TODO now consider the position 0 as not used joint
      27           0 :     for (auto jp : jointPos) {
      28           0 :         bool zeros = std::all_of(jp.second.begin(), jp.second.end(), [](double i) { return i==0.0; });
      29           0 :         if (zeros) {
      30           0 :             jointsInvolvedCount.insert (std::make_pair (jp.first, 0) );
      31             :         } else {
      32           0 :             jointsInvolvedCount.insert (std::make_pair (jp.first, 1) );
      33             : 
      34             :         }
      35             :     } 
      36             :     
      37           0 :     this->jointPos = jointPos;
      38             : 
      39           0 : }
      40             : 
      41          12 : ROSEE::ActionGeneric::ActionGeneric(std::string actionName, ROSEE::JointPos jointPos, JointsInvolvedCount jic) : 
      42          12 :                             Action(actionName, Action::Type::Generic) {
      43             :                       
      44          12 :     if (jic.empty()) {
      45             :         //HACK TODO now consider the position 0 as not used joint
      46           0 :         for (auto jp : jointPos) {
      47           0 :             bool zeros = std::all_of(jp.second.begin(), jp.second.end(), [](double i) { return i==0.0; });
      48           0 :             if (zeros) {
      49           0 :                 jointsInvolvedCount.insert (std::make_pair (jp.first, 0) );
      50             :             } else {
      51           0 :                 jointsInvolvedCount.insert (std::make_pair (jp.first, 1) );
      52             : 
      53             :             }
      54             :         }
      55             :         
      56             :     } else {
      57          12 :         if ( ! ROSEE::Utils::keys_equal(jointPos, jic) ) {
      58           0 :             throw ROSEE::Utils::DifferentKeysException<ROSEE::JointPos, ROSEE::JointsInvolvedCount>(&jointPos, &jic);
      59             :         }     
      60          12 :         this->jointsInvolvedCount = jic;
      61             :     }
      62             :     
      63          12 :     this->jointPos = jointPos;
      64             : 
      65          12 : }
      66             : 
      67             : 
      68           0 : ROSEE::ActionGeneric::ActionGeneric(std::string actionName, ROSEE::JointPos jointPos, JointsInvolvedCount jic, 
      69           0 :                                   std::set<std::string> fingersInvolved) : Action(actionName, Action::Type::Generic) {
      70             :     
      71           0 :     if (jic.empty()) {
      72             :         //HACK TODO now consider the position 0 as not used joint
      73           0 :         for (auto jp : jointPos) {
      74           0 :             bool zeros = std::all_of(jp.second.begin(), jp.second.end(), [](double i) { return i==0.0; });
      75           0 :             if (zeros) {
      76           0 :                 jointsInvolvedCount.insert (std::make_pair (jp.first, 0) );
      77             :             } else {
      78           0 :                 jointsInvolvedCount.insert (std::make_pair (jp.first, 1) );
      79             : 
      80             :             }
      81             :         } 
      82             :         
      83             :     } else {
      84             :         
      85           0 :         if ( ! ROSEE::Utils::keys_equal(jointPos, jic) ) {
      86           0 :             throw ROSEE::Utils::DifferentKeysException<ROSEE::JointPos, ROSEE::JointsInvolvedCount>(&jointPos, &jic);
      87             :         }
      88             :         
      89           0 :         this->jointsInvolvedCount = jic;
      90             :     }
      91             :     
      92           0 :     this->jointPos = jointPos;
      93             : 
      94           0 :     this->fingersInvolved = fingersInvolved;
      95           0 : }
      96             : 
      97          41 : ROSEE::JointPos ROSEE::ActionGeneric::getJointPos() const {
      98          41 :     return jointPos;
      99             : }
     100             : 
     101          18 : std::vector<ROSEE::JointPos> ROSEE::ActionGeneric::getAllJointPos() const {
     102          18 :     std::vector < JointPos> vect;
     103          18 :     vect.push_back ( jointPos ) ;
     104          18 :     return vect;
     105             : }
     106             : 
     107          15 : void ROSEE::ActionGeneric::emitYaml(YAML::Emitter& out) const {
     108             :     
     109          15 :     out << YAML::BeginMap << YAML::Key << name << YAML::Value << YAML::BeginMap ;
     110          15 :         out << YAML::Key << "Type" << YAML::Value << type;
     111          15 :         out << YAML::Key << "FingersInvolved" << YAML::Value << YAML::Flow << fingersInvolved;
     112          15 :         out << YAML::Key << "JointsInvolvedCount" << YAML::Value << YAML::BeginMap;
     113          94 :         for (const auto &jointCount : jointsInvolvedCount ) {
     114          79 :             out << YAML::Key << jointCount.first;
     115          79 :             out << YAML::Value << jointCount.second;
     116             :         } 
     117          15 :         out << YAML::EndMap;
     118             : 
     119          15 :         out << YAML::Key << "JointPos" << YAML::Value << YAML::BeginMap;
     120          94 :             for (const auto &joint : jointPos) {
     121          79 :                 out << YAML::Key << joint.first;
     122          79 :                 out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
     123             :             }
     124          15 :         out << YAML::EndMap;
     125          15 :     out << YAML::EndMap;
     126          15 :     out << YAML::EndMap;
     127          15 : }
     128             : 
     129             : 
     130           0 : bool ROSEE::ActionGeneric::fillFromYaml(YAML::const_iterator yamlIt) {
     131             :     
     132           0 :     name = yamlIt->first.as<std::string>();
     133           0 :     type = ROSEE::Action::Type::Generic;
     134             :             
     135           0 :     for (auto keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue ) {
     136             : 
     137           0 :         std::string key = keyValue->first.as<std::string>();
     138             : 
     139           0 :         if ( key.compare ("FingersInvolved") == 0 ) { 
     140             :             // if <not_inserted> tempVect is a empty vector
     141           0 :             auto tempVect = keyValue->second.as <std::vector <std::string> > ();
     142           0 :             fingersInvolved.insert ( tempVect.begin(), tempVect.end() );
     143             :             
     144           0 :         } else if ( key.compare ("Type") == 0 ) {
     145           0 :             if (ROSEE::Action::Type::Generic != static_cast<ROSEE::Action::Type> ( keyValue->second.as <unsigned int>() )) {
     146           0 :                 std::cout << "[GENERIC ACTION::" << __func__ << "] Error, found type  " << keyValue->second.as <unsigned int>()
     147           0 :                 << "instead of generic type (" << ROSEE::Action::Type::Generic << ")" << std::endl;
     148           0 :                 return false;
     149             :             }
     150           0 :             type = ROSEE::Action::Type::Generic;
     151             :             
     152           0 :         } else if ( key.compare ("JointsInvolvedCount") == 0 ) {
     153           0 :             jointsInvolvedCount = keyValue->second.as < JointsInvolvedCount >(); 
     154             :             
     155           0 :         } else if ( key.compare ("JointPos") == 0 ) {
     156           0 :             jointPos = keyValue->second.as < JointPos >();
     157             :             
     158             :         } else {
     159           0 :             std::cout << "[GENERIC ACTION::" << __func__ << "] Error, not known key " << key << std::endl;
     160           0 :             return false;
     161             :         }
     162             :     } 
     163             :     
     164           0 :     if ( ! ROSEE::Utils::keys_equal(jointPos, jointsInvolvedCount) ) {
     165           0 :         throw ROSEE::Utils::DifferentKeysException<ROSEE::JointPos, ROSEE::JointsInvolvedCount>(&jointPos, &jointsInvolvedCount);
     166             :     }
     167             :     
     168           0 :     return true;
     169             : }
     170             : 
     171             : 
     172             : 
     173             : 
     174             :