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 <ros_end_effector/GraspingActions/ActionPinchTight.h>
20 :
21 99 : ROSEE::ActionPinchTight::ActionPinchTight() :
22 99 : ActionPinchGeneric ("pinchTight", 2, 3, ActionPrimitive::Type::PinchTight) { }
23 :
24 0 : ROSEE::ActionPinchTight::ActionPinchTight(unsigned int jointStateSetMaxSize) :
25 0 : ActionPinchGeneric ("pinchTight", 2, jointStateSetMaxSize, ActionPrimitive::Type::PinchTight) { }
26 :
27 14429 : ROSEE::ActionPinchTight::ActionPinchTight (std::pair <std::string, std::string> fingerNamesPair,
28 14429 : JointPos jp, collision_detection::Contact cont) :
29 14429 : ActionPinchGeneric ("pinchTight", 2, 3, ActionPrimitive::Type::PinchTight ) {
30 :
31 : //different from insertState, here we are sure the set is empty (we are in costructor)
32 14429 : fingersInvolved.insert(fingerNamesPair.first);
33 14429 : fingersInvolved.insert(fingerNamesPair.second);
34 14429 : actionStates.insert (std::make_pair (jp, cont) );
35 14429 : }
36 :
37 0 : ROSEE::ActionPinchTight::ActionPinchTight (std::string finger1, std::string finger2,
38 0 : JointPos jp, collision_detection::Contact cont) :
39 0 : ActionPinchGeneric ("pinchTight", 2, 3, ActionPrimitive::Type::PinchTight ) {
40 :
41 : //different from insertState, here we are sure the set is empty (we are in costructor)
42 0 : fingersInvolved.insert(finger1);
43 0 : fingersInvolved.insert(finger2);
44 0 : actionStates.insert (std::make_pair (jp, cont) );
45 0 : }
46 :
47 15 : ROSEE::JointPos ROSEE::ActionPinchTight::getJointPos() const {
48 15 : return (actionStates.begin()->first);
49 : }
50 :
51 0 : ROSEE::JointPos ROSEE::ActionPinchTight::getJointPos( unsigned int index) const {
52 0 : auto it = actionStates.begin();
53 0 : unsigned int i = 1;
54 0 : while (i < index ) {
55 0 : ++ it;
56 : }
57 0 : return (it->first);
58 : }
59 :
60 224 : std::vector < ROSEE::JointPos > ROSEE::ActionPinchTight::getAllJointPos() const{
61 :
62 224 : std::vector < JointPos > retVect;
63 224 : retVect.reserve ( actionStates.size() );
64 :
65 896 : for (auto it : actionStates ) {
66 672 : retVect.push_back(it.first);
67 : }
68 :
69 224 : return retVect;
70 : }
71 :
72 :
73 28 : std::vector < ROSEE::ActionPinchTight::StateWithContact > ROSEE::ActionPinchTight::getActionStates () const {
74 :
75 28 : std::vector < ROSEE::ActionPinchTight::StateWithContact > retVect;
76 28 : retVect.reserve ( actionStates.size() );
77 :
78 112 : for (auto it : actionStates ) {
79 84 : retVect.push_back(it);
80 : }
81 :
82 28 : return retVect;
83 :
84 : }
85 :
86 14361 : bool ROSEE::ActionPinchTight::insertActionState (ROSEE::JointPos jp, collision_detection::Contact cont) {
87 :
88 14361 : auto pairRet = actionStates.insert ( std::make_pair (jp, cont) ) ;
89 :
90 14361 : if (! pairRet.second ) {
91 : //TODO print error no insertion because depth is equal... very improbable
92 870 : return false;
93 : }
94 :
95 13491 : if (actionStates.size() > maxStoredActionStates) {
96 : //max capacity reached, we have to delete the last one
97 13355 : auto it = pairRet.first;
98 :
99 13355 : if ( (++it) == actionStates.end() ){
100 : // the new inserted is the last one and has to be erased
101 12663 : actionStates.erase(pairRet.first);
102 12663 : return false;
103 : }
104 :
105 : // the new inserted is not the last one that has to be erased
106 692 : auto lastElem = actionStates.end();
107 692 : --lastElem;
108 692 : actionStates.erase(lastElem);
109 : }
110 :
111 828 : return true;
112 : }
113 :
114 :
115 55 : void ROSEE::ActionPinchTight::print () const {
116 :
117 110 : std::stringstream output;
118 55 : output << "ActionName: " << name << std::endl;
119 :
120 55 : output << "FingersInvolved: [";
121 165 : for (auto fingName : fingersInvolved){
122 110 : output << fingName << ", " ;
123 : }
124 55 : output.seekp (-2, output.cur); //to remove the last comma (and space)
125 55 : output << "]" << std::endl;
126 :
127 55 : output << "JointsInvolvedCount: " << std::endl;;
128 55 : output << jointsInvolvedCount << std::endl;
129 :
130 55 : unsigned int nActState = 1;
131 220 : for (auto itemSet : actionStates) { //the element in the set
132 165 : output << "Action_State_" << nActState << " :" << std::endl;
133 :
134 165 : output << "\t" << "JointStates:" << std::endl;
135 165 : output << itemSet.first;
136 165 : output << "\t" << "MoveitContact:" << std::endl;
137 165 : output << "\t\tbody_name_1: " << itemSet.second.body_name_1 << std::endl;
138 165 : output << "\t\tbody_name_2: " << itemSet.second.body_name_2 << std::endl;
139 165 : output << "\t\tbody_type_1: " << itemSet.second.body_type_1 << std::endl;
140 165 : output << "\t\tbody_type_2: " << itemSet.second.body_type_2 << std::endl;
141 165 : output << "\t\tdepth: " << itemSet.second.depth << std::endl;
142 165 : output << "\t\tnormal: " << "["<< itemSet.second.normal.x() << ", "
143 165 : << itemSet.second.normal.y() << ", " << itemSet.second.normal.z() << "]" << std::endl;
144 165 : output << "\t\tpos: " << "["<< itemSet.second.pos.x() << ", "
145 165 : << itemSet.second.pos.y() << ", " << itemSet.second.pos.z() << "]" << std::endl;
146 :
147 165 : nActState++;
148 : }
149 55 : output << std::endl;
150 :
151 55 : std::cout << output.str();
152 :
153 55 : }
154 :
155 :
156 68 : void ROSEE::ActionPinchTight::emitYaml ( YAML::Emitter& out ) const {
157 :
158 68 : out << YAML::Key << YAML::Flow << fingersInvolved;
159 :
160 68 : unsigned int nCont = 1;
161 68 : out << YAML::Value << YAML::BeginMap;
162 68 : out << YAML::Key << "PrimitiveType" << YAML::Value << primitiveType;
163 68 : out << YAML::Key << "ActionName" << YAML::Value << name;
164 68 : out << YAML::Key << "JointsInvolvedCount" << YAML::Value << YAML::BeginMap;
165 595 : for (const auto &jointCount : jointsInvolvedCount ) {
166 527 : out << YAML::Key << jointCount.first;
167 527 : out << YAML::Value << jointCount.second;
168 : }
169 68 : out << YAML::EndMap;
170 :
171 272 : for (const auto & actionState : actionStates) { //.second is the set of ActionState
172 :
173 408 : std::string contSeq = "ActionState_" + std::to_string(nCont);
174 204 : out << YAML::Key << contSeq;
175 :
176 204 : out << YAML::Value << YAML::BeginMap;
177 : //actionState.first, the jointstates map
178 204 : out << YAML::Key << "JointPos" << YAML::Value << YAML::BeginMap;
179 1785 : for (const auto &joint : actionState.first) {
180 1581 : out << YAML::Key << joint.first;
181 1581 : out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
182 : }
183 204 : out << YAML::EndMap;
184 :
185 : //actionState.second, the optional
186 204 : out << YAML::Key << "Optional" << YAML::Value;
187 204 : emitYamlForContact ( actionState.second, out );
188 :
189 204 : out << YAML::EndMap;
190 204 : nCont++;
191 : }
192 68 : out << YAML::EndMap;
193 :
194 68 : }
195 :
196 :
197 99 : bool ROSEE::ActionPinchTight::fillFromYaml ( YAML::const_iterator yamlIt ) {
198 :
199 198 : std::vector <std::string> fingInvolvedVect = yamlIt->first.as <std::vector < std::string >> ();
200 297 : for (const auto &it : fingInvolvedVect) {
201 198 : fingersInvolved.insert(it);
202 : }
203 :
204 693 : for ( YAML::const_iterator keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue) {
205 :
206 1188 : std::string key = keyValue->first.as<std::string>();
207 594 : if (key.compare("JointsInvolvedCount") == 0) {
208 99 : jointsInvolvedCount = keyValue->second.as < JointsInvolvedCount > ();
209 :
210 495 : } else if (key.compare ("ActionName") == 0 ) {
211 99 : name = keyValue->second.as <std::string> ();
212 :
213 396 : } else if (key.compare ("PrimitiveType") == 0) {
214 : ROSEE::ActionPrimitive::Type parsedType = static_cast<ROSEE::ActionPrimitive::Type> (
215 99 : keyValue->second.as <unsigned int>() );
216 99 : if (parsedType != primitiveType ) {
217 0 : std::cerr << "[ERROR ActionPinchTight::" << __func__ << " parsed a type " << parsedType <<
218 0 : " but this object has primitive type " << primitiveType << std::endl;
219 0 : return false;
220 : }
221 :
222 297 : } else if (key.compare(0, 12, "ActionState_") == 0) { //compare 12 caracters from index 0 of key
223 :
224 594 : JointPos jointPos;
225 594 : collision_detection::Contact contact;
226 891 : for(YAML::const_iterator asEl = keyValue->second.begin(); asEl != keyValue->second.end(); ++asEl) {
227 :
228 : //asEl can be the map JointPos or the map Optional
229 594 : if (asEl->first.as<std::string>().compare ("JointPos") == 0 ) {
230 297 : jointPos = asEl->second.as < JointPos >();
231 297 : } else if (asEl->first.as<std::string>().compare ("Optional") == 0 ) {
232 :
233 594 : YAML::Node cont = asEl->second["MoveItContact"];
234 297 : contact.body_name_1 = cont["body_name_1"].as < std::string >();
235 297 : contact.body_name_2 = cont["body_name_2"].as < std::string >();
236 297 : switch (cont["body_type_1"].as < int >())
237 : {
238 297 : case 0:
239 297 : contact.body_type_1 = collision_detection::BodyType::ROBOT_LINK;
240 297 : break;
241 0 : case 1:
242 0 : contact.body_type_1 = collision_detection::BodyType::ROBOT_ATTACHED;
243 0 : break;
244 0 : case 2:
245 0 : contact.body_type_1 = collision_detection::BodyType::WORLD_OBJECT;
246 0 : break;
247 0 : default:
248 0 : std::cout << "some error, body_type_1" << cont["body_type_1"].as < int >()
249 0 : << "unknown" << std::endl;
250 0 : contact.body_type_1 = collision_detection::BodyType::WORLD_OBJECT;
251 : }
252 297 : switch (cont["body_type_2"].as < int >())
253 : {
254 297 : case 0:
255 297 : contact.body_type_2 = collision_detection::BodyType::ROBOT_LINK;
256 297 : break;
257 0 : case 1:
258 0 : contact.body_type_2 = collision_detection::BodyType::ROBOT_ATTACHED;
259 0 : break;
260 0 : case 2:
261 0 : contact.body_type_2 = collision_detection::BodyType::WORLD_OBJECT;
262 0 : break;
263 0 : default:
264 0 : std::cout << "some error, body_type_2" << cont["body_type_2"].as < int >()
265 0 : << "unknown" << std::endl;
266 0 : contact.body_type_2 = collision_detection::BodyType::WORLD_OBJECT;
267 : }
268 297 : contact.depth = cont["depth"].as<double>();
269 594 : std::vector < double > normVect = cont["normal"].as < std::vector <double> >();
270 594 : std::vector < double > posVect = cont["pos"].as < std::vector <double> >();
271 297 : contact.normal = Eigen::Vector3d (normVect.data() );
272 297 : contact.pos = Eigen::Vector3d (posVect.data() );
273 :
274 : } else {
275 : //ERRROr, only joinstates and optional at this level
276 : std::cerr << "[ERROR ActionPinchTight::" << __func__ << "not know key "
277 0 : << asEl->first.as<std::string>() <<
278 0 : " found in the yaml file at this level" << std::endl;
279 0 : return false;
280 : }
281 : }
282 297 : actionStates.insert ( std::make_pair (jointPos, contact));
283 :
284 : } else {
285 : std::cerr << "[ERROR ActionPinchTight::" << __func__ << "not know key " << key <<
286 0 : " found in the yaml file" << std::endl;
287 0 : return false;
288 : }
289 : }
290 :
291 99 : return true;
292 : }
293 :
294 204 : bool ROSEE::ActionPinchTight::emitYamlForContact (collision_detection::Contact moveitContact, YAML::Emitter& out) const {
295 :
296 204 : out << YAML::BeginMap;
297 204 : out << YAML::Key << "MoveItContact" << YAML::Value << YAML::BeginMap;
298 204 : out << YAML::Key << "body_name_1";
299 204 : out << YAML::Value << moveitContact.body_name_1;
300 204 : out << YAML::Key << "body_name_2";
301 204 : out << YAML::Value << moveitContact.body_name_2;
302 204 : out << YAML::Key << "body_type_1";
303 204 : out << YAML::Value << moveitContact.body_type_1;
304 204 : out << YAML::Key << "body_type_2";
305 204 : out << YAML::Value << moveitContact.body_type_2;
306 204 : out << YAML::Key << "depth";
307 204 : out << YAML::Value << moveitContact.depth;
308 204 : out << YAML::Key << "normal";
309 408 : std::vector < double > normal ( moveitContact.normal.data(), moveitContact.normal.data() + moveitContact.normal.rows());
310 204 : out << YAML::Value << YAML::Flow << normal;
311 204 : out << YAML::Key << "pos";
312 408 : std::vector < double > pos ( moveitContact.pos.data(), moveitContact.pos.data() + moveitContact.pos.rows());
313 204 : out << YAML::Value << YAML::Flow << pos;
314 204 : out << YAML::EndMap;
315 204 : out << YAML::EndMap;
316 :
317 408 : return true;
318 183 : }
|
