Line data    Source code

       1             : /*
       2             :  * Copyright (C) 2019 IIT-HHCM
       3             :  * Author: Luca Muratore
       4             :  * email:  luca.muratore@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             :  * http://www.apache.org/licenses/LICENSE-2.0
      10             :  * 
      11             :  * Unless required by applicable law or agreed to in writing, software
      12             :  * distributed under the License is distributed on an "AS IS" BASIS,
      13             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      14             :  * See the License for the specific language governing permissions and
      15             :  * limitations under the License.
      16             : */
      17             : 
      18             : #include <end_effector/EEInterface.h>
      19             : 
      20          72 : ROSEE::EEInterface::EEInterface ( const ROSEE::Parser& p  ) {
      21             :     
      22             :     // get number of joints from parse and resize dynamic structure
      23          72 :     _joints_num = p.getActuatedJointsNumber();
      24          72 :     _upper_limits.resize(_joints_num);
      25          72 :     _lower_limits.resize(_joints_num);
      26             :     
      27             :     // get the EE name
      28          72 :     _ee_name = p.getEndEffectorName();
      29             :     
      30             :     // get the ee description
      31          72 :     _ee_description = p.getFingerJointMap();
      32             :     
      33             :     // get he joints urdf description
      34          72 :     _urdf_joint_map = p.getUrdfJointMap();
      35             :     
      36          72 :     int id = 0;
      37             :     
      38             :     // save the finger names
      39         262 :     for( const auto& finger_joints_pair : _ee_description ) {
      40             :         
      41             :         // fingers
      42         380 :         std::string finger_name = finger_joints_pair.first;
      43         190 :         _fingers_names.push_back(finger_name);
      44             : 
      45         570 :         for( const auto& j : finger_joints_pair.second ) {
      46             :             
      47             :             // joint name
      48         380 :             _actuated_joints.push_back(j);
      49             :             
      50             :             // internal id
      51         380 :             _joints_internal_id_map[j] = id;
      52             :             
      53             :             // finger internal id map 
      54         380 :             _finger_joints_internal_id_map[finger_name].push_back(id);
      55             :             
      56             :             // upper limits
      57         380 :             _upper_limits[id] = _urdf_joint_map.at(j)->limits->upper;
      58             :             
      59             :             // lower limits
      60         380 :             _lower_limits[id] = _urdf_joint_map.at(j)->limits->lower;
      61             :             
      62             :             // increase internal id
      63         380 :             id++;
      64             :         }
      65             :     }
      66             : 
      67          72 : }
      68             : 
      69          33 : std::string ROSEE::EEInterface::getName() {
      70             :     
      71          33 :     return _ee_name;
      72             : }
      73             : 
      74             : 
      75          44 : Eigen::VectorXd ROSEE::EEInterface::getLowerPositionLimits() {
      76             : 
      77          44 :     return _lower_limits;
      78             : }
      79             : 
      80          24 : Eigen::VectorXd ROSEE::EEInterface::getUpperPositionLimits() {
      81             : 
      82          24 :     return _upper_limits;
      83             : }
      84             : 
      85          84 : bool ROSEE::EEInterface::getInternalIdForJoint ( std::string joint_name, int& internal_id ) {
      86             :         
      87          84 :     if ( _joints_internal_id_map.count(joint_name) ) {
      88             :         
      89          84 :         internal_id = _joints_internal_id_map.at(joint_name);
      90          84 :         return true;
      91             :     }
      92             :     
      93           0 :     return false;
      94             : 
      95             : }
      96             : 
      97             : 
      98           0 : bool ROSEE::EEInterface::getInternalIdsForFinger ( std::string finger_name, std::vector< int >& internal_ids ) {
      99             : 
     100           0 :     if ( _finger_joints_internal_id_map.count(finger_name) ) {
     101             :         
     102           0 :         internal_ids = _finger_joints_internal_id_map.at(finger_name);
     103           0 :         return true;
     104             :     }
     105             :     
     106           0 :     return false;
     107             : 
     108             : }
     109             : 
     110           8 : const std::vector< std::string >& ROSEE::EEInterface::getFingers() {
     111             :     
     112           8 :     return _fingers_names;
     113             : }
     114             : 
     115          27 : bool ROSEE::EEInterface::isFinger ( std::string finger_name ) {
     116             :     
     117          27 :     return ( _ee_description.count(finger_name) > 0 );
     118             : }
     119             : 
     120             : 
     121             : 
     122          16 : const std::vector< std::string >&  ROSEE::EEInterface::getActuatedJoints () {
     123             :     
     124          16 :     return _actuated_joints;
     125             : }
     126             : 
     127             : 
     128           3 : bool ROSEE::EEInterface::getActuatedJointsInFinger ( std::string finger_name, std::vector< std::string >& actuated_joints ) {
     129             : 
     130           3 :     if( !isFinger(finger_name) ) {
     131             :         
     132           0 :         return false;
     133             :     }
     134             :     
     135           3 :     actuated_joints = _ee_description.at(finger_name);
     136           3 :     return true;
     137             : }
     138             : 
     139           8 : int ROSEE::EEInterface::getActuatedJointsNum() {
     140             :     
     141           8 :     return _joints_num;
     142             : }
     143             : 
     144          16 : int ROSEE::EEInterface::getActuatedJointsNumInFinger ( std::string finger_name ) {
     145             :     
     146          16 :     if( !isFinger(finger_name) ) {
     147             :         
     148           0 :         return -1;
     149             :     } 
     150             : 
     151          16 :     return _ee_description.at(finger_name).size();
     152             : }
     153             : 
     154          26 : int ROSEE::EEInterface::getFingersNumber() {
     155             :     
     156          26 :     return _fingers_names.size();
     157             : }
     158             : 
     159             : 
     160          72 : ROSEE::EEInterface::~EEInterface() {
     161             : 
     162          72 : }