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 <ros_end_effector/EEInterface.h>
19 :
20 113 : ROSEE::EEInterface::EEInterface ( const ROSEE::Parser& p ) {
21 :
22 : // get number of joints from parse and resize dynamic structure
23 113 : _joints_num = p.getActuatedJointsNumber();
24 113 : _upper_limits.resize(_joints_num);
25 113 : _lower_limits.resize(_joints_num);
26 :
27 : // get the EE name
28 113 : _ee_name = p.getEndEffectorName();
29 :
30 : // get the ee description
31 113 : _ee_description = p.getFingerJointMap();
32 :
33 : // get he joints urdf description
34 113 : _urdf_joint_map = p.getUrdfJointMap();
35 :
36 113 : int id = 0;
37 :
38 : // save the finger names
39 407 : for( const auto& finger_joints_pair : _ee_description ) {
40 :
41 : // fingers
42 588 : std::string finger_name = finger_joints_pair.first;
43 294 : _fingers_names.push_back(finger_name);
44 :
45 894 : for( const auto& j : finger_joints_pair.second ) {
46 :
47 : // joint name
48 600 : _actuated_joints.push_back(j);
49 :
50 : // internal id
51 600 : _joints_internal_id_map[j] = id;
52 :
53 : // finger internal id map
54 600 : _finger_joints_internal_id_map[finger_name].push_back(id);
55 :
56 : // upper limits
57 600 : _upper_limits[id] = _urdf_joint_map.at(j)->limits->upper;
58 :
59 : // lower limits
60 600 : _lower_limits[id] = _urdf_joint_map.at(j)->limits->lower;
61 :
62 : // increase internal id
63 600 : id++;
64 : }
65 : }
66 :
67 113 : }
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 124521 : bool ROSEE::EEInterface::getInternalIdForJoint ( std::string joint_name, int& internal_id ) {
86 :
87 124521 : if ( _joints_internal_id_map.count(joint_name) ) {
88 :
89 124521 : internal_id = _joints_internal_id_map.at(joint_name);
90 124521 : 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 90 : const std::vector< std::string >& ROSEE::EEInterface::getFingers() {
111 :
112 90 : 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 57 : const std::vector< std::string >& ROSEE::EEInterface::getActuatedJoints () {
123 :
124 57 : 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 113 : ROSEE::EEInterface::~EEInterface() {
161 :
162 260 : }
|
