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/ActionComposed.h>
      20             : 
      21          21 : ROSEE::ActionComposed::ActionComposed() : ActionGeneric() {
      22          21 :     independent = true;
      23          21 :     nInnerActions = 0;
      24          21 :     type = Action::Type::Composed;
      25          21 : }
      26             : 
      27           8 : ROSEE::ActionComposed::ActionComposed(std::string name) : ActionGeneric(name) {
      28           8 :     independent = true;
      29           8 :     nInnerActions = 0;
      30           8 :     type = Action::Type::Composed;
      31           8 : }
      32             : 
      33          16 : ROSEE::ActionComposed::ActionComposed(std::string name, bool independent) : ActionGeneric(name) {
      34          16 :     this->independent = independent;
      35          16 :     nInnerActions = 0;
      36          16 :     type = Action::Type::Composed;
      37          16 : }
      38             : 
      39          26 : unsigned int ROSEE::ActionComposed::numberOfInnerActions() const {
      40          26 :     return nInnerActions;
      41             : }
      42             : 
      43          10 : bool ROSEE::ActionComposed::isIndependent() const {
      44          10 :     return independent;
      45             : }
      46             : 
      47           8 : std::vector<std::string> ROSEE::ActionComposed::getInnerActionsNames() const {
      48           8 :     return innerActionsNames;
      49             : }
      50             : 
      51           0 : bool ROSEE::ActionComposed::empty() {
      52           0 :     return (nInnerActions == 0);
      53             : }
      54             : 
      55             : 
      56          60 : bool ROSEE::ActionComposed::sumAction ( ROSEE::Action::Ptr action, double jointPosScaleFactor, unsigned int jointPosIndex )
      57             : {
      58             :     
      59          60 :     if ( ! checkIndependency(action) ) {
      60           0 :         return false; //cant add this primitive
      61             :     }
      62             :     
      63          60 :     if ( jointPosIndex > action->getAllJointPos().size()-1 ) {
      64           0 :         std::cerr << "[ACTIONCOMPOSED:: " << __func__ << "] The given jointPosindex " << jointPosIndex  
      65           0 :             << " exceed the number  " << action->getAllJointPos().size() << " of jointpos of passed action" << std::endl;
      66           0 :         return false;
      67             :     }
      68             :     
      69         102 :     if ( nInnerActions > 0 && 
      70         102 :         (! ROSEE::Utils::keys_equal(action->getAllJointPos().at(jointPosIndex), jointPos)) ) {
      71             :         std::cerr << "[ACTIONCOMPOSED:: " << __func__ << "] The action passed as argument has different keys in jointPosmap" 
      72           0 :                   << " respect to the others inserted in this composed action " << std::endl;
      73           0 :         return false;
      74             :     }
      75             :     
      76          60 :     if (jointPosScaleFactor < 0) {
      77             :         std::cerr << "[ACTIONCOMPOSED:: " << __func__ << "] You can not scale the joint position of the action to be inserted by a " 
      78           0 :                   <<  "value less than 0; jointPosScaleFactor passed is: " << jointPosScaleFactor << std::endl;
      79           0 :         return false;
      80             :     } 
      81             :     
      82          60 :     if (jointPosScaleFactor > 1) {
      83             :         std::wcerr << "[ACTIONCOMPOSED:: " << __func__ << "] WARNING, You are scaling with a value greater than 1 " 
      84           0 :                    << " this could cause to command position over the joint limits;  jointPosScaleFactor passed is: " << jointPosScaleFactor << std::endl;
      85             :     } 
      86             : 
      87         180 :     JointPos actionJP = action->getAllJointPos().at(jointPosIndex) * jointPosScaleFactor;
      88          60 :     JointsInvolvedCount actionJIC = action->getJointsInvolvedCount();
      89             :         
      90          60 :     if (nInnerActions == 0) { //first primitive inserted
      91             :         
      92         132 :         for (auto joint : actionJP ){
      93         114 :             jointsInvolvedCount.insert ( std::make_pair (joint.first, actionJIC.at(joint.first) ) );
      94         114 :             jointPos.insert ( std::make_pair (joint.first, joint.second) );
      95             :         }
      96             :         
      97             :     } else {
      98             :         
      99          42 :         if (independent) {
     100             : 
     101             :             //if here, action is independent, we can add the joints states
     102         195 :             for ( auto joint : actionJP ){ 
     103             :                 
     104         170 :                 if ( actionJIC.at (joint.first ) > 0 ) {
     105             :                     
     106          60 :                     jointPos.at(joint.first) = joint.second;
     107          60 :                     jointsInvolvedCount.at(joint.first) += actionJIC.at (joint.first );
     108             :                     //+= or = is the same for the checks done before 
     109             :                 } 
     110             :             }
     111             :             
     112             :         } else {
     113             :             // for each joint, add the state's value to the mean 
     114             :             // (number of element in the previous mean is given by jointsInvolvedCount.at(x))
     115             : 
     116         135 :             for ( auto joint : actionJP ) { 
     117         118 :                 if ( actionJIC.at( joint.first ) == 0 ) {
     118          76 :                     continue; //if the action that is being added has this joint not setted, not consider it
     119             :                 }
     120             :                 
     121             :                 //update the count 
     122          42 :                 jointsInvolvedCount.at(joint.first) += actionJIC.at (joint.first ); 
     123             :                 
     124             :                 //iterate all dofs of jointPos
     125          84 :                 for (unsigned int dof = 0; dof < joint.second.size(); dof++ ) {
     126             :                     
     127          42 :                     double mean = jointPos.at( joint.first ).at(dof) + 
     128          42 :                         ( (joint.second.at(dof) - jointPos.at(joint.first).at(dof)) / 
     129          42 :                             jointsInvolvedCount.at(joint.first) );
     130             :                         
     131          42 :                     jointPos.at(joint.first).at(dof) = mean;     
     132             :                 }
     133             :             }
     134             :         }
     135             :     }
     136             :     
     137          60 :     innerActionsNames.push_back ( action->getName() );
     138         124 :     for ( auto it: action->getFingersInvolved() ) {
     139          64 :          fingersInvolved.insert ( it );
     140             :     }
     141             : 
     142          60 :     nInnerActions ++;
     143             :     
     144          60 :     return true;
     145             : }
     146             : 
     147          60 : bool ROSEE::ActionComposed::checkIndependency ( ROSEE::Action::Ptr action ) {
     148             :     
     149          60 :     if (!independent) {
     150             :         
     151          40 :     } else if (nInnerActions == 0 ) {
     152             :         
     153         110 :         for ( auto jic : action->getJointsInvolvedCount() ){
     154          95 :             if ( jic.second  > 1  ) {
     155             :                 // if action is dependent we check that all its joints are setted only once
     156             :                 // so we can teoretically add a "dipendent" action if all its joints are setted once
     157           0 :                 return false; //cant add this primitive
     158             :             }
     159             :         }
     160             :         
     161             :     } else {   
     162             :     
     163         195 :         for ( auto jic : action->getJointsInvolvedCount() ){
     164         170 :             if ( jic.second + jointsInvolvedCount.at(jic.first) > 1  ) {
     165             :                 // we use the sum so also if action is dependent we check that all its joints are setted once
     166             :                 // so we can teoretically add a "dipendent" action if all its joints are setted once
     167           0 :                 return false; //cant add this primitive
     168             :             }
     169             :         }
     170             :     }
     171             :     
     172          60 :     return true;
     173             : }
     174             : 
     175             : 
     176           0 : void ROSEE::ActionComposed::print () const {
     177             :     
     178           0 :     std::stringstream output;
     179             :     
     180           0 :     output << "Composed Action '" << name;
     181           0 :     independent ? output << "' (independent):" : output << "' (not independent):" ;
     182           0 :     output << std::endl;
     183             :     
     184           0 :     output << "Composed by " << nInnerActions << " inner action: [" ;
     185           0 :     for (auto it : innerActionsNames) {
     186           0 :         output << it << ", ";
     187             :     }
     188           0 :     output.seekp (-2, output.cur); //to remove the last comma (and space)
     189           0 :     output << "]" << std::endl;
     190             :     
     191           0 :     output << "Fingers involved: [" ;
     192           0 :     for (auto it : fingersInvolved) {
     193           0 :         output << it << ", ";
     194             :     }
     195           0 :     output.seekp (-2, output.cur); //to remove the last comma (and space)
     196           0 :     output << "]" << std::endl;
     197             :     
     198           0 :     output << "Each joint influenced by x inner action:" << std::endl;
     199           0 :     output << jointsInvolvedCount;
     200             : 
     201           0 :     output << "JointPos:" << std::endl;
     202           0 :     output << jointPos << std::endl;
     203             :     
     204           0 :     std::cout << output.str();
     205           0 : }
     206             : 
     207             : 
     208           9 : void ROSEE::ActionComposed::emitYaml ( YAML::Emitter& out) const {
     209             :     
     210           9 :     out << YAML::BeginMap << YAML::Key << name << YAML::Value << YAML::BeginMap ;
     211           9 :         out << YAML::Key << "Type" << YAML::Value << type;
     212           9 :         out << YAML::Key << "Independent" << YAML::Value << independent;
     213           9 :         out << YAML::Key << "NInnerActions" << YAML::Value << nInnerActions;
     214           9 :         out << YAML::Key << "InnerActionsNames" << YAML::Value << YAML::Flow << innerActionsNames;
     215           9 :         out << YAML::Key << "FingersInvolved" << YAML::Value << YAML::Flow << fingersInvolved;
     216           9 :         out << YAML::Key << "JointsInvolvedCount" << YAML::Value << YAML::BeginMap;
     217          66 :         for (const auto &jointCount : jointsInvolvedCount ) {
     218          57 :             out << YAML::Key << jointCount.first;
     219          57 :             out << YAML::Value << jointCount.second;
     220             :         } 
     221           9 :         out << YAML::EndMap;
     222             : 
     223           9 :         out << YAML::Key << "JointPos" << YAML::Value << YAML::BeginMap;
     224          66 :             for (const auto &joint : jointPos) {
     225          57 :                 out << YAML::Key << joint.first;
     226          57 :                 out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
     227             :             }
     228           9 :         out << YAML::EndMap;
     229           9 :     out << YAML::EndMap;
     230           9 :     out << YAML::EndMap;
     231           9 : }
     232             : 
     233             : // if parsed, we dont have the link with the primitives... so primitiveObjects is empty
     234           9 : bool ROSEE::ActionComposed::fillFromYaml ( YAML::const_iterator yamlIt ) {
     235             :     
     236           9 :     name = yamlIt->first.as<std::string>();
     237             :             
     238          81 :     for (auto keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue ) {
     239             : 
     240          63 :         std::string key = keyValue->first.as<std::string>();
     241             : 
     242          63 :         if ( key.compare ("Independent") == 0 ) {
     243           9 :             independent = keyValue->second.as<bool>();
     244             :             
     245          54 :         } else if ( key.compare ("NInnerActions") == 0 ) {
     246           9 :             nInnerActions = keyValue->second.as <unsigned int>();
     247             :             
     248          45 :         } else if ( key.compare ("Type") == 0 ) {
     249           9 :             if (ROSEE::Action::Type::Composed != static_cast<ROSEE::Action::Type> ( keyValue->second.as <unsigned int>() )) {
     250           0 :                 std::cout << "[COMPOSED ACTION::" << __func__ << "] Error, found type  " << keyValue->second.as <unsigned int>()
     251           0 :                 << "instead of Composed type (" << ROSEE::Action::Type::Composed << ")" << std::endl;
     252           0 :                 return false;
     253             :             }
     254           9 :             type = ROSEE::Action::Type::Composed;
     255             :             
     256          36 :         } else if ( key.compare ("InnerActionsNames") == 0 ) {
     257           9 :             innerActionsNames = keyValue->second.as <std::vector <std::string> >();
     258             :         
     259          27 :         } else if ( key.compare ("FingersInvolved") == 0 ) { 
     260          18 :             auto tempVect = keyValue->second.as <std::vector <std::string> > ();
     261           9 :             fingersInvolved.insert ( tempVect.begin(), tempVect.end() );
     262             :             
     263          18 :         } else if ( key.compare ("JointsInvolvedCount") == 0 ) {
     264           9 :             jointsInvolvedCount = keyValue->second.as < JointsInvolvedCount >(); 
     265             :             
     266           9 :         } else if ( key.compare ("JointPos") == 0 ) {
     267           9 :             jointPos = keyValue->second.as < JointPos >();
     268             :             
     269             :         } else {
     270           0 :             std::cout << "[COMPOSED ACTION PARSER] Error, not known key " << key << std::endl;
     271           0 :             return false;
     272             :         }
     273             :     } 
     274           9 :     return true;
     275             : }
     276             : 
     277             : 
     278             :