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/Action.h>
      20             : 
      21             : /******************************** OPERATORS OVERLOAD FOR TYPEDEFS ***********************************/
      22             : 
      23             : /** operator overload for JointPos so it is easier to print */
      24       14390 : std::ostream& ROSEE::operator << (std::ostream& output, const ROSEE::JointPos jp) {
      25      115755 :     for (const auto &jsEl : jp) {
      26      101365 :         output << "\t\t"<<jsEl.first << " : "; //joint name
      27      202730 :         for(const auto &jValue : jsEl.second){
      28      101365 :             output << jValue << ", "; //joint position (vector because can have multiple dof)
      29             :         }
      30      101365 :         output.seekp (-2, output.cur); //to remove the last comma (and space)
      31      101365 :         output << std::endl;       
      32             :     }
      33       14390 :     return output;
      34             : }
      35             : 
      36          37 : ROSEE::JointPos ROSEE::operator * ( double multiplier,  ROSEE::JointPos jp) {
      37             :     
      38          37 :     return jp *= multiplier;
      39             : }
      40             : 
      41          60 : ROSEE::JointPos ROSEE::operator * ( ROSEE::JointPos jp,  double multiplier ) {
      42             :     
      43          60 :     return jp *= multiplier;
      44             : }
      45             : 
      46          97 : ROSEE::JointPos& ROSEE::operator *= ( ROSEE::JointPos& jp, double multiplier) {
      47             :     
      48         723 :     for ( auto &jsEl : jp) {
      49        1252 :         for (int i = 0; i< jsEl.second.size(); i++) {
      50         626 :             jsEl.second.at(i) *= multiplier;
      51             :         }
      52             :     }
      53             :     
      54          97 :     return jp;
      55             : }
      56             : 
      57           0 : ROSEE::JointPos ROSEE::operator + ( ROSEE::JointPos jp1, ROSEE::JointPos jp2) {
      58             :     
      59           0 :     return jp1 += jp2;
      60             : }
      61             : 
      62           0 : ROSEE::JointPos& ROSEE::operator += (ROSEE::JointPos& jp1, ROSEE::JointPos jp2) {
      63             :     
      64           0 :     if ( ! ROSEE::Utils::keys_equal(jp1, jp2) ) {
      65           0 :         throw ROSEE::Utils::DifferentKeysException<ROSEE::JointPos, ROSEE::JointPos>(&jp1, &jp2);
      66             :     }
      67             :     
      68           0 :     for (auto &jsEl : jp1) {
      69           0 :         if (jsEl.second.size() != jp2.at(jsEl.first).size() ) {
      70           0 :             throw "Dofs not same";
      71             :         }
      72             : 
      73           0 :         for (int i = 0; i < jsEl.second.size(); i++) {
      74           0 :             jsEl.second.at(i) += jp2.at(jsEl.first).at(i); 
      75             :         }
      76             :     }
      77             :     
      78           0 :     return jp1;
      79             : }
      80             : 
      81           0 : std::ostream& ROSEE::operator << (std::ostream& output, const ROSEE::JointsInvolvedCount jic) {
      82           0 :     for (const auto &jicEl : jic) {
      83           0 :         output << "\t"<< jicEl.first << " : " << jicEl.second;
      84           0 :         output << std::endl;       
      85             :     }
      86           0 :     return output;
      87             : }
      88             : 
      89           0 : std::ostream& ROSEE::operator<<(std::ostream& out, const ROSEE::Action::Type type){
      90             :     
      91           0 :         const char* s = 0;
      92             : #define PROCESS_VAL(p) case(p): s = #p; break;
      93           0 :     switch(type){
      94           0 :         PROCESS_VAL(ROSEE::Action::Type::Primitive);     
      95           0 :         PROCESS_VAL(ROSEE::Action::Type::Generic);     
      96           0 :         PROCESS_VAL(ROSEE::Action::Type::Composed);
      97           0 :         PROCESS_VAL(ROSEE::Action::Type::Timed);
      98           0 :         PROCESS_VAL(ROSEE::Action::Type::None);
      99             :     }
     100             : #undef PROCESS_VAL
     101             : 
     102           0 :     return out << s;
     103             : }
     104             : 
     105             : /********************************************* CLASS ACTION **************************************/
     106          29 : ROSEE::Action::Action () {
     107             : 
     108          29 : }
     109             : 
     110             : 
     111       15369 : ROSEE::Action::Action ( std::string name, Action::Type type ) {
     112       15369 :     this->name = name;
     113       15369 :     this->type = type;
     114       15369 : }
     115             : 
     116         416 : std::string ROSEE::Action::getName() const {
     117         416 :     return name;
     118             : }
     119             : 
     120         136 : ROSEE::Action::Type ROSEE::Action::getType() const {
     121         136 :     return type;
     122             : }
     123             : 
     124             : 
     125         227 : std::set<std::string> ROSEE::Action::getFingersInvolved() const {
     126         227 :     return fingersInvolved;
     127             : }
     128             : 
     129             : 
     130         294 : ROSEE::JointsInvolvedCount ROSEE::Action::getJointsInvolvedCount() const {
     131         294 :     return jointsInvolvedCount;
     132             : }
     133             : 
     134             : 
     135           0 : void ROSEE::Action::print () const {
     136             :     
     137           0 :     std::stringstream output;
     138           0 :     output << "ActionName: " << name << std::endl;
     139             :     
     140           0 :     if (fingersInvolved.size() > 0 ){
     141           0 :         output << "FingersInvolved: [";
     142           0 :         for (auto fingName : fingersInvolved){
     143           0 :             output << fingName << ", " ;
     144             :         }
     145           0 :         output.seekp (-2, output.cur); //to remove the last comma (and space)
     146           0 :         output << "]" << std::endl;
     147             :         
     148             :     } else {
     149           0 :         output << "FingersInvolved: <not inserted>" << std::endl; // can happen, for ex for genericAction
     150             :     }
     151             : 
     152             :     
     153           0 :     output << "JointsInvolvedCount: " << std::endl;;
     154           0 :     output << jointsInvolvedCount << std::endl;
     155             :     
     156           0 :     output << "JointPos:" << std::endl;
     157           0 :     output << getJointPos() << std::endl;
     158             : 
     159           0 :     output << std::endl;
     160             : 
     161           0 :     std::cout << output.str();
     162           0 : }