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/ActionSingleJointMultipleTips.h>
      18             : 
      19           0 : ROSEE::ActionSingleJointMultipleTips::ActionSingleJointMultipleTips() : 
      20           0 :     ActionPrimitive ( "singleJointMultipleTips", 1, ROSEE::ActionPrimitive::Type::SingleJointMultipleTips ) {}
      21             : 
      22           0 : ROSEE::ActionSingleJointMultipleTips::ActionSingleJointMultipleTips(std::string actionName, unsigned int nFingers ) : 
      23           0 :     ActionPrimitive ( actionName, nFingers, 1, ROSEE::ActionPrimitive::Type::SingleJointMultipleTips ) {}
      24             : 
      25           1 : ROSEE::ActionSingleJointMultipleTips::ActionSingleJointMultipleTips (std::string actionName, std::vector<std::string> fingers, std::string jointName, 
      26           1 :                                        JointPos jpFurther, JointPos jpNearer) : 
      27           1 :     ActionPrimitive ( actionName, fingers.size(), 1, ROSEE::ActionPrimitive::Type::SingleJointMultipleTips ) {
      28             :         
      29           1 :     this->jointInvolved = jointName;
      30           1 :     fingersInvolved.insert (fingers.begin(), fingers.end());
      31             :     
      32           1 :     this->jointPosFurther = jpFurther;
      33           1 :     this->jointPosNearer = jpNearer;
      34             :     
      35             :     // still need to do this, that can be done in costructor because we know that only one joint will be used, per definition of this action
      36           2 :     for (auto it : jpFurther ) {
      37           1 :         jointsInvolvedCount.insert (std::make_pair (it.first, 0) );
      38             :     }
      39           1 :     jointsInvolvedCount.at (jointName) = 1;
      40             :         
      41           1 : }
      42             : 
      43           0 : std::vector<ROSEE::JointPos> ROSEE::ActionSingleJointMultipleTips::getAllJointPos() const {
      44             :     
      45           0 :     std::vector<JointPos> vect;
      46           0 :     vect.push_back (jointPosFurther); 
      47           0 :     vect.push_back (jointPosNearer);
      48           0 :     return vect;
      49             : }
      50             : 
      51           0 : ROSEE::JointPos ROSEE::ActionSingleJointMultipleTips::getJointPos() const {
      52           0 :     return jointPosFurther;
      53             : }
      54             : 
      55           0 : ROSEE::JointPos ROSEE::ActionSingleJointMultipleTips::getJointPosFurther() const {
      56           0 :     return jointPosFurther;
      57             : }
      58             : 
      59           0 : ROSEE::JointPos ROSEE::ActionSingleJointMultipleTips::getJointPosNearer() const {
      60           0 :     return jointPosNearer;
      61             : }
      62             : 
      63           0 : std::string ROSEE::ActionSingleJointMultipleTips::getJointName() const {
      64           0 :     return jointInvolved;
      65             : }
      66             : 
      67           1 : std::set<std::string> ROSEE::ActionSingleJointMultipleTips::getKeyElements() const {
      68           1 :     std::set <std::string> set;
      69           1 :     set.insert(jointInvolved);
      70           1 :     return set;
      71             : }
      72             : 
      73           0 : void ROSEE::ActionSingleJointMultipleTips::print() const {
      74             :     
      75           0 :     std::stringstream output;
      76           0 :     output << "ActionName: " << name << std::endl;
      77             :     
      78           0 :     output << "FingersInvolved: [";
      79           0 :     for (auto fingName : fingersInvolved){
      80           0 :         output << fingName << ", " ;
      81             :     }
      82           0 :     output.seekp (-2, output.cur); //to remove the last comma (and space)
      83           0 :     output << "]" << std::endl;
      84             :     
      85           0 :     output << "Joint which moves the tips: " << jointInvolved << std::endl;
      86             :     
      87           0 :     output << "JointPos Further from 0:" << std::endl;
      88           0 :     output << jointPosFurther;
      89             :     
      90           0 :     output << "JointPos Nearer to 0:" << std::endl;
      91           0 :     output << jointPosNearer;
      92             : 
      93           0 :     output << std::endl;
      94             : 
      95           0 :     std::cout << output.str();
      96             :     
      97           0 : }
      98             : 
      99           1 : void ROSEE::ActionSingleJointMultipleTips::emitYaml(YAML::Emitter& out) const {
     100             :     
     101           1 :     out << YAML::Key << jointInvolved;
     102           1 :     out << YAML::Value << YAML::BeginMap;
     103             :     
     104           1 :         out << YAML::Key << "PrimitiveType" << YAML::Value << primitiveType;
     105           1 :         out << YAML::Key << "ActionName" << YAML::Value << name;
     106           1 :         out << YAML::Key << "FingersInvolved" << YAML::Value << YAML::Flow << fingersInvolved;
     107             :         
     108           1 :         out << YAML::Key << "JointPosFurther" << YAML::Value << YAML::BeginMap;
     109           2 :         for (const auto &joint : jointPosFurther) {
     110           1 :             out << YAML::Key << joint.first;
     111           1 :             out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
     112             :         }
     113           1 :         out << YAML::EndMap;
     114             :         
     115           1 :         out << YAML::Key << "JointPosNearer" << YAML::Value << YAML::BeginMap;
     116           2 :         for (const auto &joint : jointPosNearer) {
     117           1 :             out << YAML::Key << joint.first;
     118           1 :             out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
     119             :         }
     120           1 :         out << YAML::EndMap;
     121             :     
     122           1 :     out << YAML::EndMap;
     123             :     
     124           1 : }
     125             : 
     126             : 
     127             : 
     128           0 : bool ROSEE::ActionSingleJointMultipleTips::fillFromYaml(YAML::const_iterator yamlIt) {
     129             :     
     130           0 :     jointInvolved = yamlIt->first.as < std::string > ();
     131             :     
     132           0 :     for ( YAML::const_iterator keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue) {
     133           0 :         std::string key = keyValue->first.as<std::string>();
     134             :         
     135           0 :         if ( key.compare ("ActionName") == 0 ){
     136           0 :             name = keyValue->second.as < std::string > ();
     137             :             
     138           0 :         } else if (key.compare("FingersInvolved") == 0) {
     139           0 :             std::vector <std::string> fingInvolvedVect = keyValue->second.as <std::vector < std::string >> ();
     140           0 :             for (const auto &it : fingInvolvedVect) {
     141           0 :                 fingersInvolved.insert(it);
     142             :             }
     143             :             
     144           0 :         } else if (key.compare ("JointPosNearer") == 0) {
     145           0 :             jointPosNearer = keyValue->second.as <JointPos>();
     146             :             
     147           0 :         } else if (key.compare ("JointPosFurther") == 0) {
     148           0 :             jointPosFurther = keyValue->second.as <JointPos>();
     149             :         
     150           0 :         } else if (key.compare ("PrimitiveType") == 0) {
     151             :             ROSEE::ActionPrimitive::Type parsedType = static_cast<ROSEE::ActionPrimitive::Type> ( 
     152           0 :                 keyValue->second.as <unsigned int>() );
     153           0 :             if (parsedType != primitiveType ) {
     154           0 :                 std::cerr << "[ERROR ActionSingleJointMultipleTips::" << __func__ << " parsed a type " << parsedType << 
     155           0 :                     " but this object has primitive type " << primitiveType << std::endl; 
     156           0 :                 return false;
     157             :             }
     158             :             
     159             :         } else {
     160             :             std::cerr << "[ERROR ActionSingleJointMultipleTips::" << __func__ << "not know key " << key << 
     161           0 :                 " found in the yaml file" << std::endl; 
     162           0 :             return false;
     163             :         }
     164             :     }
     165             :     
     166             :     // we have also to fill this structure, that is not present on yaml file because redundant 
     167           0 :     for (auto it : jointPosFurther ) {
     168           0 :         jointsInvolvedCount.insert (std::make_pair (it.first, 0) );
     169             :     }
     170           0 :     jointsInvolvedCount.at (jointInvolved) = 1;
     171             :     
     172           0 :     nFingersInvolved = fingersInvolved.size();
     173             : 
     174           0 :     return true;
     175             :     
     176             : }
     177             : 
     178             : 
     179             : 
     180             :