Line data Source code
1 : #include <ros_end_effector/FindActions.h>
2 :
3 144 : ROSEE::FindActions::FindActions ( std::shared_ptr < ROSEE::ParserMoveIt > parserMoveIt ){
4 :
5 144 : this->parserMoveIt = parserMoveIt;
6 :
7 144 : this->mimicNLRelMap = parserMoveIt->getMimicNLFatherOfJointMap();
8 144 : }
9 :
10 :
11 : std::pair < std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchTight >,
12 : std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose > >
13 68 : ROSEE::FindActions::findPinch ( std::string path2saveYaml ){
14 :
15 136 : std::map < std::pair <std::string, std::string> , ActionPinchTight > mapOfPinches = checkCollisions();
16 136 : std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose > mapOfLoosePinches;
17 68 : fillNotCollidingTips(&mapOfLoosePinches, &mapOfPinches);
18 :
19 68 : checkWhichTipsCollideWithoutBounds ( &mapOfLoosePinches ) ;
20 :
21 68 : if (mapOfLoosePinches.size() != 0 ){
22 17 : checkDistances (&mapOfLoosePinches) ;
23 : }
24 :
25 : /// EMITTING PART ................
26 68 : if (mapOfPinches.size() == 0 ) { //print if no collision at all
27 : std::cout << "[FINDACTIONS::" << __func__ << "]: I found no collisions between tips. Are you sure your hand"
28 : << " has some fingertips that can collide? If yes, check your urdf/srdf, or be sure to"
29 : << " have set the mesh or some collision geometry for the links, or"
30 34 : << " set a bigger value in N_EXP_COLLISION" << std::endl;
31 :
32 : } else {
33 :
34 34 : ROSEE::YamlWorker yamlWorker;
35 68 : std::map < std::set <std::string> , ActionPrimitive* > mapForWorker;
36 :
37 102 : for (auto& it : mapOfPinches) { // auto& and not auto alone!
38 :
39 68 : ActionPrimitive* pointer = &(it.second);
40 136 : std::set < std::string > keys ;
41 68 : keys.insert (it.first.first) ;
42 68 : keys.insert (it.first.second) ;
43 68 : mapForWorker.insert (std::make_pair ( keys, pointer ) );
44 : }
45 :
46 34 : yamlWorker.createYamlFile(mapForWorker, "pinchTight", path2saveYaml);
47 : }
48 :
49 68 : if (mapOfLoosePinches.size() == 0 ) {
50 : std::cout << "[FINDACTIONS::" << __func__ << "]: I found no loose pinches. This mean that some error happened or that" <<
51 51 : " all the tips pairs collide with each other for at least one hand configuration." << std::endl;
52 :
53 : } else {
54 :
55 17 : ROSEE::YamlWorker yamlWorker;
56 34 : std::map < std::set <std::string> , ActionPrimitive* > mapForWorker;
57 :
58 34 : for (auto& it : mapOfLoosePinches) { // auto& and not auto alone!
59 :
60 17 : ActionPrimitive* pointer = &(it.second);
61 34 : std::set < std::string > keys ;
62 17 : keys.insert (it.first.first) ;
63 17 : keys.insert (it.first.second) ;
64 17 : mapForWorker.insert (std::make_pair ( keys, pointer ) );
65 : }
66 :
67 17 : yamlWorker.createYamlFile(mapForWorker, "pinchLoose", path2saveYaml);
68 : }
69 :
70 136 : return std::make_pair(mapOfPinches, mapOfLoosePinches);
71 : }
72 :
73 132 : std::map <std::string, ROSEE::ActionTrig> ROSEE::FindActions::findTrig ( ROSEE::ActionPrimitive::Type actionType,
74 : std::string path2saveYaml) {
75 :
76 :
77 132 : std::map <std::string, ActionTrig> trigMap;
78 :
79 132 : switch (actionType) {
80 60 : case ROSEE::ActionPrimitive::Type::Trig : {
81 60 : trigMap = trig();
82 60 : break;
83 : }
84 36 : case ROSEE::ActionPrimitive::Type::TipFlex : {
85 36 : trigMap = tipFlex();
86 36 : break;
87 : }
88 36 : case ROSEE::ActionPrimitive::Type::FingFlex : {
89 36 : trigMap = fingFlex();
90 36 : break;
91 : }
92 0 : default: {
93 0 : std::cout << "[ERROR FINDACTIONS::" << __func__ << "]: Passing as argument a action type which is not a type of trig. " << std::endl
94 0 : << "I am returing an empty map" << std::endl;
95 0 : return trigMap;
96 : }
97 : }
98 :
99 132 : if (trigMap.size() == 0 ) { //if so, no sense to continue
100 33 : return trigMap;
101 : }
102 :
103 : //for involvedJoints. Ok here because I know that for the trigs, a non setted joint is
104 : //a joint which is in a default position
105 363 : for (auto & mapEl : trigMap) {
106 :
107 528 : ROSEE::JointsInvolvedCount jointsInvolvedCount;
108 2013 : for ( auto joint : mapEl.second.getJointPos() ) {
109 1749 : jointsInvolvedCount.insert (std::make_pair(joint.first, 0));
110 3069 : for (auto dof : joint.second) {
111 1749 : if (dof != DEFAULT_JOINT_POS){
112 429 : jointsInvolvedCount.at(joint.first) = 1;
113 429 : break;
114 : }
115 : }
116 : }
117 264 : mapEl.second.setJointsInvolvedCount (jointsInvolvedCount);
118 : }
119 :
120 198 : std::map < std::set <std::string> , ActionPrimitive* > mapForWorker;
121 :
122 363 : for (auto& it : trigMap) { // auto& and not auto alone!
123 :
124 264 : ActionPrimitive* pointer = &(it.second);
125 528 : std::set < std::string > keys ;
126 264 : keys.insert (it.first) ;
127 264 : mapForWorker.insert (std::make_pair ( keys, pointer ) );
128 : }
129 :
130 99 : ROSEE::YamlWorker yamlWorker;
131 99 : yamlWorker.createYamlFile(mapForWorker, trigMap.begin()->second.getName(), path2saveYaml);
132 :
133 99 : return trigMap;
134 : }
135 :
136 :
137 10 : std::map <std::string, ROSEE::ActionSingleJointMultipleTips> ROSEE::FindActions::findSingleJointMultipleTips(unsigned int nFinger, std::string path2saveYaml) {
138 :
139 10 : std::map <std::string, ROSEE::ActionSingleJointMultipleTips> mapOfSingleJointMultipleTips;
140 :
141 10 : if (nFinger <= 0) {
142 : std::cout << "[ERROR FINDACTIONS::" << __func__ << "] please pass a positive number " <<
143 0 : " as number of fingers. You passed " << nFinger << " ! " << std::endl;
144 0 : return mapOfSingleJointMultipleTips;
145 : }
146 :
147 10 : if (nFinger == 1) {
148 : std::cout << "[ERROR FINDACTIONS::" << __func__ << "] with 1 finger, you are looking for a ActionTrig, "
149 4 : << "and not a ActionSingleJointMultipleTips. Returning an empty map" << std::endl;
150 4 : return mapOfSingleJointMultipleTips;
151 : }
152 :
153 6 : if (nFinger > parserMoveIt->getNFingers() ) {
154 0 : std::cout << "[ERROR FINDACTIONS::" << __func__ << "] I can not find an action which moves " << nFinger <<
155 0 : " fingers if the hand has only " << parserMoveIt->getNFingers() << " fingers. Returning an empty map" << std::endl;
156 0 : return mapOfSingleJointMultipleTips;
157 : }
158 :
159 12 : std::string actionName = "singleJointMultipleTips_" + std::to_string(nFinger); //action name same for each action
160 :
161 7 : for (auto mapEl : parserMoveIt->getFingertipsOfJointMap() ) {
162 :
163 35 : if (mapEl.second.size() != nFinger ) {
164 34 : continue;
165 : }
166 :
167 2 : std::vector<double> furtherPos = parserMoveIt->getBiggerBoundFromZero(mapEl.first);
168 2 : std::vector<double> nearerPos = parserMoveIt->getSmallerBoundFromZero(mapEl.first);
169 :
170 : //create and initialize JointPos map
171 2 : JointPos jpFar;
172 2 : for (auto it : parserMoveIt->getActiveJointModels()){
173 2 : std::vector <double> jPos (it->getVariableCount(), DEFAULT_JOINT_POS);
174 1 : jpFar.insert ( std::make_pair ( it->getName(), jPos ));
175 : }
176 2 : JointPos jpNear = jpFar;
177 :
178 1 : jpFar.at ( mapEl.first ) = furtherPos;
179 1 : jpNear.at ( mapEl.first ) = nearerPos;
180 :
181 2 : std::vector<std::string> fingersInvolved;
182 3 : for (auto tip : mapEl.second){
183 2 : fingersInvolved.push_back(parserMoveIt->getFingerOfFingertip (tip) );
184 : }
185 :
186 2 : ActionSingleJointMultipleTips action (actionName, fingersInvolved, mapEl.first, jpFar, jpNear);
187 :
188 1 : mapOfSingleJointMultipleTips.insert (std::make_pair(mapEl.first, action));
189 : }
190 :
191 : //// EMITTING
192 6 : if (mapOfSingleJointMultipleTips.size() == 0 ) {
193 5 : std::cout << "[FINDACTIONS::" << __func__ << "] no singleJointMultipleTips with " << nFinger << " found" << std::endl;
194 5 : return mapOfSingleJointMultipleTips;
195 : }
196 :
197 2 : std::map < std::set <std::string> , ActionPrimitive* > mapForWorker;
198 :
199 2 : for (auto& it : mapOfSingleJointMultipleTips) { // auto& and not auto alone!
200 :
201 1 : ActionPrimitive* pointer = &(it.second);
202 2 : std::set<std::string> set;
203 1 : set.insert (it.first);
204 1 : mapForWorker.insert (std::make_pair ( set, pointer ) );
205 : }
206 :
207 1 : ROSEE::YamlWorker yamlWorker;
208 1 : yamlWorker.createYamlFile(mapForWorker, actionName, path2saveYaml);
209 :
210 1 : return mapOfSingleJointMultipleTips;
211 : }
212 :
213 :
214 4 : std::map<std::set<std::string>, ROSEE::ActionMultiplePinchTight> ROSEE::FindActions::findMultiplePinch(unsigned int nFinger, std::string path2saveYaml,
215 : bool strict ) {
216 :
217 4 : std::map<std::set<std::string>, ROSEE::ActionMultiplePinchTight> multiplePinchMap;
218 4 : if (nFinger < 3 ) {
219 : std::cerr << "[ERROR " << __func__ << "] for this find pass at least 3 as number " <<
220 0 : " of fingertips for the pinch" << std::endl;
221 0 : return multiplePinchMap;
222 : }
223 :
224 4 : multiplePinchMap = checkCollisionsForMultiplePinch(nFinger, strict);
225 :
226 : //// EMITTING YAML
227 4 : if (multiplePinchMap.size() == 0 ) {
228 3 : return multiplePinchMap;
229 : }
230 2 : std::map < std::set <std::string> , ActionPrimitive* > mapForWorker;
231 :
232 2 : for (auto& it : multiplePinchMap) { // auto& and not auto alone!
233 :
234 1 : ActionPrimitive* pointer = &(it.second);
235 1 : mapForWorker.insert (std::make_pair ( it.first, pointer ) );
236 : }
237 :
238 1 : ROSEE::YamlWorker yamlWorker;
239 1 : yamlWorker.createYamlFile(mapForWorker, multiplePinchMap.begin()->second.getName(), path2saveYaml);
240 :
241 1 : return multiplePinchMap;
242 : }
243 :
244 :
245 :
246 : /*********************************** PRIVATE FUNCTIONS ***********************************************************************/
247 : /**************************************** PINCHES ***********************************************************************/
248 :
249 68 : std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchTight > ROSEE::FindActions::checkCollisions () {
250 :
251 68 : std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchTight > mapOfPinches;
252 :
253 136 : planning_scene::PlanningScene planning_scene ( parserMoveIt->getRobotModel() );
254 136 : collision_detection::CollisionRequest collision_request;
255 136 : collision_detection::CollisionResult collision_result; // std::cout << "before" << std::endl;
256 : // kinematic_state.printStatePositions();
257 : //std::vector<double> one_dof;
258 : // one_dof.push_back (1);
259 : // kinematic_state.setJointPositions("SFP1_1__SFP1_2", one_dof);
260 : // std::cout << "after" << std::endl;
261 : // kinematic_state.printStatePositions();
262 68 : collision_request.contacts = true; //set to compute collisions
263 68 : collision_request.max_contacts = 1000;
264 :
265 : // Consider only collisions among fingertips
266 : // If an object pair does not appear in the acm, it is assumed that collisions between those
267 : // objects is no tolerated. So we must fill it with all the nonFingertips
268 :
269 136 : collision_detection::AllowedCollisionMatrix acm;
270 68 : acm.setEntry(parserMoveIt->getRobotModel()->getLinkModelNames(),
271 136 : parserMoveIt->getRobotModel()->getLinkModelNames(), true); //true==not considered collisions
272 68 : acm.setEntry(parserMoveIt->getFingertipNames(),
273 136 : parserMoveIt->getFingertipNames(), false); //false== considered collisions
274 :
275 136 : robot_state::RobotState kinematic_state(parserMoveIt->getRobotModel());
276 :
277 340068 : for (int i = 0; i < N_EXP_COLLISION; i++){
278 :
279 680000 : std::stringstream logCollision;
280 340000 : collision_result.clear();
281 :
282 340000 : setToRandomPositions(&kinematic_state);
283 :
284 340000 : planning_scene.checkSelfCollision(collision_request, collision_result, kinematic_state, acm);
285 :
286 340000 : if (collision_result.collision) {
287 :
288 : //for each collision with this joints state...
289 28753 : for (auto cont : collision_result.contacts){
290 :
291 : //store joint states
292 28858 : JointPos jointPos = getConvertedJointPos(&kinematic_state);
293 :
294 : //moveit contacts is a map between a pair (2 strings with link names) and a vector of Contact object ?? I don't know why the contact is a vector, I have always find only one element
295 : logCollision << "[FINDACTIONS::" << __func__ << "] Collision between " << cont.first.first << " and " <<
296 14429 : cont.first.second << std::endl;
297 :
298 28858 : for (auto contInfo : cont.second){
299 14429 : logCollision << "\tWith a depth of contact: " << contInfo.depth;
300 : }
301 :
302 28858 : JointsInvolvedCount jointsInvolvedCount = setOnlyDependentJoints(cont.first, &jointPos);
303 :
304 : ///create the actionPinch
305 :
306 : // get the finger name
307 28858 : auto fingerPair = getFingersPair(cont.first);
308 :
309 28858 : ActionPinchTight pinch (fingerPair, jointPos, cont.second.at(0) );
310 14429 : pinch.setJointsInvolvedCount ( jointsInvolvedCount );
311 14429 : auto itFind = mapOfPinches.find ( fingerPair );
312 14429 : if ( itFind == mapOfPinches.end() ) {
313 : //if here, we have to create store the new created action
314 68 : mapOfPinches.insert ( std::make_pair (fingerPair, pinch) );
315 68 : logCollision << ", NEW INSERTION";
316 :
317 : } else { //Check if it is the best depth among the found collision among that pair
318 14361 : if (itFind->second.insertActionState( jointPos, cont.second.at(0)) ) {
319 828 : logCollision << ", NEW INSERTION";
320 : }
321 : }
322 14429 : logCollision << std::endl;
323 14429 : logCollision << jointPos;
324 : }
325 : //this print is for debugging purposes
326 : //std::cout << logCollision.str() << std::endl;
327 : }
328 : }
329 :
330 136 : return mapOfPinches;
331 : }
332 :
333 :
334 17 : void ROSEE::FindActions::checkDistances (std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose >* mapOfLoosePinches) {
335 :
336 34 : robot_state::RobotState kinematic_state(parserMoveIt->getRobotModel());
337 :
338 85017 : for (int i = 0; i < N_EXP_DISTANCES; i++){
339 :
340 85000 : setToRandomPositions(&kinematic_state);
341 :
342 : //for each pair remaining in notCollidingTips, check if a new min distance is found
343 170000 : for (auto &mapEl : *mapOfLoosePinches) {
344 :
345 : // restore all joint pos
346 170000 : JointPos jointPosLoose = getConvertedJointPos(&kinematic_state);
347 :
348 170000 : auto tips = getFingertipsPair(mapEl.first);
349 :
350 170000 : JointsInvolvedCount jointsInvolvedCount = setOnlyDependentJoints(tips, &jointPosLoose);
351 :
352 85000 : Eigen::Affine3d tip1Trasf = kinematic_state.getGlobalLinkTransform(tips.first);
353 85000 : Eigen::Affine3d tip2Trasf = kinematic_state.getGlobalLinkTransform(tips.second);
354 85000 : double distance = (tip1Trasf.translation() - tip2Trasf.translation() ) .norm() ;
355 :
356 85000 : mapEl.second.insertActionState( jointPosLoose, distance ) ;
357 85000 : mapEl.second.setJointsInvolvedCount ( jointsInvolvedCount );
358 : }
359 : }
360 17 : }
361 :
362 :
363 68 : void ROSEE::FindActions::removeBoundsOfNotCollidingTips (
364 : const std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose >* mapOfLoosePinches,
365 : robot_model::RobotModelPtr kinematic_model_noBound) {
366 :
367 136 : for (auto mapEl : *mapOfLoosePinches ) {
368 :
369 : //for each joint of first tip...
370 136 : auto tips = getFingertipsPair(mapEl.first);
371 : /// C++ Question: WHY if I use directly parser....at() in the for the string joint is corrupted?
372 136 : auto joints = parserMoveIt->getJointsOfFingertipMap().at (tips.first);
373 187 : for ( std::string joint : joints ) {
374 119 : auto jointModel = kinematic_model_noBound ->getJointModel(joint);
375 :
376 119 : auto type = jointModel->getType() ;
377 119 : if (type == moveit::core::JointModel::REVOLUTE ) {
378 : //at(0) because we are sure to have 1 dof being revolute
379 119 : auto bound = jointModel->getVariableBounds().at(0);
380 119 : bound.max_position_ = EIGEN_PI;
381 119 : bound.min_position_ = -EIGEN_PI;
382 : //at(0) because we are sure to have 1 dof being revolute
383 119 : jointModel->setVariableBounds ( jointModel->getVariableNames().at(0), bound );
384 :
385 0 : } else if ( type == moveit::core::JointModel::PRISMATIC ) {
386 : // we cant set infinite here... lets double the limits?
387 : std::cout << "[WARNING FINDACTIONS::" << __func__ << "] I am doubling the bounds for your prismatic joint "
388 0 : << "but I am not sure it is enough to make the tips colliding to find the loose pinches " <<
389 : std::endl;
390 0 : auto bound = jointModel->getVariableBounds().at(0);
391 0 : bound.max_position_ *= 2;
392 0 : bound.min_position_ *= 2;
393 : //at(0) because we are sure to have 1 dof being prismatic
394 0 : jointModel->setVariableBounds ( jointModel->getVariableNames().at(0), bound );
395 :
396 : } else {
397 :
398 0 : std::cout << "[FINDACTIONS::" << __func__ << "] Why are you using a type "
399 0 : << kinematic_model_noBound ->getJointModel(joint)->getType()
400 : << " joint? Code not ready to temporarily delete the multiple dof bounds"
401 0 : << " in the job done to find the loose pinches " << std::endl << std::endl;
402 :
403 0 : continue;
404 : }
405 : }
406 :
407 : //for each joint of second tip...
408 136 : auto joints2 = parserMoveIt->getJointsOfFingertipMap().at (tips.second);
409 187 : for ( auto joint : joints2 ) {
410 :
411 119 : auto jointModel = kinematic_model_noBound ->getJointModel(joint);
412 119 : auto type = jointModel->getType() ;
413 119 : if (type == moveit::core::JointModel::REVOLUTE ) {
414 : //at(0) because we are sure to have 1 dof being revolute
415 119 : auto bound = jointModel->getVariableBounds().at(0);
416 119 : bound.max_position_ = EIGEN_PI;
417 119 : bound.min_position_ = -EIGEN_PI;
418 : //at(0) because we are sure to have 1 dof being revolute
419 119 : jointModel->setVariableBounds ( jointModel->getVariableNames().at(0), bound );
420 :
421 0 : } else if ( type == moveit::core::JointModel::PRISMATIC ) {
422 : // we cant set infinite here... lets double the limits?
423 : std::cout << "[WARNING FINDACTIONS::" << __func__ << "] I am doubling the bounds for your prismatic joint "
424 0 : << "but I am not sure it is enough to make the tips colliding to find the loose pinches " << std::endl;
425 0 : auto bound = jointModel->getVariableBounds().at(0);
426 0 : bound.max_position_ *= 2;
427 0 : bound.min_position_ *= 2;
428 : //at(0) because we are sure to have 1 dof being revolute
429 0 : jointModel->setVariableBounds ( jointModel->getVariableNames().at(0), bound );
430 :
431 : } else {
432 :
433 0 : std::cout << "[FINDACTIONS::" << __func__ << "] Why are you using a type "
434 0 : << kinematic_model_noBound ->getJointModel(joint)->getType()
435 : << " joint? Code not ready to temporarily delete the multiple dof bounds"
436 0 : << " in the working done to find the loose pinches " << std::endl << std::endl;
437 :
438 0 : continue;
439 : }
440 : }
441 : }
442 68 : }
443 :
444 :
445 68 : void ROSEE::FindActions::checkWhichTipsCollideWithoutBounds (
446 : std::map < std::pair <std::string, std::string>, ROSEE::ActionPinchLoose >* mapOfLoosePinches ) {
447 :
448 136 : robot_model::RobotModelPtr kinematic_model_noBound = parserMoveIt->getCopyModel();
449 :
450 68 : removeBoundsOfNotCollidingTips (mapOfLoosePinches, kinematic_model_noBound );
451 :
452 136 : collision_detection::AllowedCollisionMatrix acm;
453 68 : acm.setEntry(kinematic_model_noBound->getLinkModelNames(),
454 : kinematic_model_noBound->getLinkModelNames(), true); //true == not considered collisions
455 :
456 136 : for( auto it : *mapOfLoosePinches) {
457 : //we want to look for collision only on the pair inside the map
458 : //take the tip from the keys (that now are fingers)
459 136 : std::string tip1 = parserMoveIt->getFingertipOfFinger(it.first.first);
460 136 : std::string tip2 = parserMoveIt->getFingertipOfFinger(it.first.second);
461 68 : acm.setEntry(tip1, tip2, false); //false == considered collisions
462 : }
463 :
464 136 : planning_scene::PlanningScene planning_scene(kinematic_model_noBound);
465 :
466 136 : collision_detection::CollisionRequest collision_request;
467 136 : collision_detection::CollisionResult collision_result;
468 68 : collision_request.contacts = true; //set to compute collisions
469 68 : collision_request.max_contacts = 1000;
470 :
471 136 : robot_state::RobotState kinematic_state(kinematic_model_noBound);
472 :
473 : // similar to checkcollisions here, but we dont want to store anything, only check if collision happen
474 136 : std::set < std::pair<std::string, std::string> > collidingFingers ;
475 340068 : for (int i = 0; i < N_EXP_COLLISION; i++){
476 340000 : collision_result.clear();
477 340000 : setToRandomPositions(&kinematic_state);
478 :
479 340000 : planning_scene.checkSelfCollision(collision_request, collision_result, kinematic_state, acm);
480 :
481 351578 : for (auto cont : collision_result.contacts){
482 : //moveit contacts is a map between a pair (2 strings with link names) and a vector of Contact object , so cont.first contain the pair of fingerTIPS which collide.
483 11578 : collidingFingers.insert ( getFingersPair (cont.first) );
484 : }
485 : }
486 :
487 : //erase from loose map the not colliding tips (: not colliding even without bounds)
488 136 : for (auto mapEl = mapOfLoosePinches->cbegin(); mapEl != mapOfLoosePinches->cend() ; /*no increment*/ ) {
489 :
490 68 : if (collidingFingers.count(mapEl->first) == 0 ) {
491 51 : mapOfLoosePinches->erase(mapEl++);
492 : } else {
493 17 : ++mapEl;
494 : }
495 : }
496 68 : }
497 :
498 :
499 4 : std::map<std::set<std::string>, ROSEE::ActionMultiplePinchTight> ROSEE::FindActions::checkCollisionsForMultiplePinch(unsigned int nFinger, bool strict) {
500 :
501 4 : std::map < std::set <std::string> , ROSEE::ActionMultiplePinchTight > mapOfMultPinches;
502 :
503 6 : unsigned int nMinCollision = strict ?
504 6 : ROSEE::Utils::binomial_coefficent(nFinger, 2) : (nFinger-1);
505 :
506 8 : planning_scene::PlanningScene planning_scene ( parserMoveIt->getRobotModel() );
507 8 : collision_detection::CollisionRequest collision_request;
508 8 : collision_detection::CollisionResult collision_result;
509 4 : collision_request.contacts = true; //set to compute collisions
510 4 : collision_request.max_contacts = 1000;
511 :
512 : // Consider only collisions among fingertips
513 : // If an object pair does not appear in the acm, it is assumed that collisions between those
514 : // objects is no tolerated. So we must fill it with all the nonFingertips
515 8 : collision_detection::AllowedCollisionMatrix acm;
516 4 : acm.setEntry(parserMoveIt->getRobotModel()->getLinkModelNames(),
517 8 : parserMoveIt->getRobotModel()->getLinkModelNames(), true); //true==not considered collisions
518 4 : acm.setEntry(parserMoveIt->getFingertipNames(),
519 8 : parserMoveIt->getFingertipNames(), false); //false== considered collisions
520 :
521 8 : robot_state::RobotState kinematic_state(parserMoveIt->getRobotModel());
522 :
523 12004 : for (int i = 0; i < N_EXP_COLLISION_MULTPINCH; i++){
524 :
525 12000 : collision_result.clear();
526 12000 : setToRandomPositions(&kinematic_state);
527 :
528 12000 : planning_scene.checkSelfCollision(collision_request, collision_result, kinematic_state, acm);
529 :
530 12000 : if (collision_result.contacts.size() >= nMinCollision ) {
531 :
532 2 : double depthSum = 0;
533 4 : std::set <std::string> tipsColliding;
534 6 : for (auto cont : collision_result.contacts){
535 :
536 4 : tipsColliding.insert(cont.first.first);
537 4 : tipsColliding.insert(cont.first.second);
538 4 : depthSum += std::abs(cont.second.at(0).depth);
539 : }
540 :
541 : //eg with 2 collision we can have 4 finger colliding because there are two
542 : //normal distinct pinch and not a 3-pinch... so we exlude these collisions
543 2 : if (tipsColliding.size() != nFinger) {
544 0 : continue;
545 : }
546 :
547 : //store joint states
548 4 : JointPos jointPos = getConvertedJointPos(&kinematic_state);
549 4 : JointsInvolvedCount jointsInvolvedCount = setOnlyDependentJoints(tipsColliding, &jointPos);
550 :
551 4 : auto fingerColliding = getFingersSet(tipsColliding);
552 :
553 4 : ActionMultiplePinchTight pinch (fingerColliding, jointPos, depthSum );
554 2 : pinch.setJointsInvolvedCount ( jointsInvolvedCount );
555 2 : auto itFind = mapOfMultPinches.find ( fingerColliding );
556 2 : if ( itFind == mapOfMultPinches.end() ) {
557 : //if here, we have to create store the new created action
558 1 : mapOfMultPinches.insert ( std::make_pair (fingerColliding, pinch) );
559 :
560 : } else { //Check if it is the best depth among the found collision among that pair
561 1 : if (itFind->second.insertActionState( jointPos, depthSum ) ) {
562 : //print debug
563 : } else {
564 : //pring debug
565 : }
566 : }
567 : }
568 : }
569 8 : return mapOfMultPinches;
570 : }
571 :
572 :
573 :
574 :
575 : /********************************************** TRIGS ***************************************************************/
576 :
577 60 : std::map <std::string, ROSEE::ActionTrig> ROSEE::FindActions::trig() {
578 :
579 60 : std::map <std::string, ActionTrig> trigMap;
580 :
581 345 : for (auto mapEl : parserMoveIt->getFingertipsOfJointMap()) {
582 :
583 300 : if (mapEl.second.size() != 1) { //the joint must move ONLY a fingertip
584 15 : continue;
585 : }
586 :
587 285 : if ( parserMoveIt->checkIfContinuosJoint(mapEl.first) == true ) {
588 0 : continue; //we dont want to use a continuos joint for the trig
589 : }
590 :
591 : /// Go in the max range
592 285 : double trigMax = parserMoveIt->getBiggerBoundFromZero(mapEl.first).at(0) ;
593 :
594 570 : ActionTrig action ("trig", ActionPrimitive::Type::Trig);
595 285 : action.setFingerInvolved ( parserMoveIt->getFingerOfFingertip( mapEl.second.at(0)) ) ;
596 :
597 : // mapEl.second.at(0) : sure to have only 1 element for the if before
598 285 : insertJointPosForTrigInMap(trigMap, action, mapEl.first, trigMax);
599 : }
600 :
601 60 : return trigMap;
602 : }
603 :
604 :
605 36 : std::map <std::string, ROSEE::ActionTrig> ROSEE::FindActions::tipFlex() {
606 :
607 36 : std::map <std::string, ROSEE::ActionTrig> tipFlexMap;
608 :
609 108 : for (auto tipName : parserMoveIt->getFingertipNames() ) {
610 :
611 90 : if (parserMoveIt->getNExclusiveJointsOfTip ( tipName, false ) < 2 ) {
612 : //if so, we have a simple trig (or none if 0) and not also a tip/finger flex
613 18 : continue;
614 : }
615 :
616 144 : std::string theInterestingJoint = parserMoveIt->getFirstActuatedParentJoint ( tipName, false );
617 72 : double tipFlexMax = parserMoveIt->getBiggerBoundFromZero ( theInterestingJoint ).at(0) ;
618 :
619 :
620 144 : ActionTrig action ("tipFlex", ActionPrimitive::Type::TipFlex);
621 72 : action.setFingerInvolved (parserMoveIt->getFingerOfFingertip(tipName)) ;
622 :
623 72 : if (! insertJointPosForTrigInMap(tipFlexMap, action, theInterestingJoint, tipFlexMax) ) {
624 : //if here, we have updated the joint position for a action that was already present in the map.
625 : //this is ok for normal trig because more joints are included in the action, but for the
626 : //tipflex and fingflex for definition only a joint is involved (the active one nearer to the tip)
627 0 : std::cout << "[FATAL ERROR FINDACTIONS::" << __func__ << "]: Inserting in tipFlexMap a tip already present??" << std::endl
628 0 : << "I am returning a not completely filled map" << std::endl;
629 0 : return tipFlexMap;
630 : }
631 : }
632 :
633 36 : return tipFlexMap;
634 : }
635 :
636 :
637 36 : std::map <std::string, ROSEE::ActionTrig> ROSEE::FindActions::fingFlex() {
638 :
639 36 : std::map <std::string, ROSEE::ActionTrig> fingFlexMap;
640 :
641 108 : for (auto tipName : parserMoveIt->getFingertipNames() ) {
642 :
643 90 : if (parserMoveIt->getNExclusiveJointsOfTip ( tipName, false ) < 2 ) {
644 : //if so, we have a simple trig (or none if 0) and not also a tip/finger flex
645 18 : continue;
646 : }
647 :
648 144 : std::string theInterestingJoint = parserMoveIt->getFirstActuatedJointInFinger ( tipName );
649 72 : double fingFlexMax = parserMoveIt->getBiggerBoundFromZero ( theInterestingJoint ).at(0) ;
650 :
651 144 : ActionTrig action ("fingFlex", ActionPrimitive::Type::FingFlex);
652 72 : action.setFingerInvolved ( parserMoveIt->getFingerOfFingertip ( tipName) ) ;
653 72 : if (! insertJointPosForTrigInMap(fingFlexMap, action, theInterestingJoint, fingFlexMax) ) {
654 : //if here, we have updated the joint position for a action that was already present in the map.
655 : //this is ok for normal trig because more joints are included in the action, but for the
656 : //tipflex and fingflex for definition only a joint is involved (the active one farther from the tip
657 : //but still inside the finger)
658 0 : std::cout << "[FATAL ERROR FINDACTIONS::" << __func__ << "]: Inserting in fingFlexMap a tip already present??n" << std::endl
659 0 : << "I am returning a not completely filled map" << std::endl;
660 0 : return fingFlexMap;
661 : }
662 : }
663 36 : return fingFlexMap;
664 : }
665 :
666 :
667 429 : bool ROSEE::FindActions::insertJointPosForTrigInMap ( std::map <std::string, ActionTrig>& trigMap,
668 : ROSEE::ActionTrig action, std::string jointName, double trigValue) {
669 :
670 429 : auto itMap = trigMap.find ( action.getFingerInvolved() );
671 429 : if ( itMap == trigMap.end() ) {
672 : //still no action for this finger in the map
673 :
674 528 : JointPos jp;
675 2013 : for (auto it : parserMoveIt->getActiveJointModels()){
676 3498 : std::vector <double> jPos (it->getVariableCount(), DEFAULT_JOINT_POS);
677 1749 : jp.insert ( std::make_pair ( it->getName(), jPos ));
678 : }
679 :
680 : //HACK at(0) because 1dof joint
681 264 : jp.at ( jointName ).at(0) = trigValue;
682 :
683 264 : action.setJointPos(jp);
684 264 : trigMap.insert ( std::make_pair ( action.getFingerInvolved(), action ) );
685 :
686 264 : return true;
687 :
688 : } else {
689 : //action already created, but we have to modify the position of a joint
690 : //itMap->second is an iterator to the already present element
691 330 : JointPos jp = itMap->second.getJointPos();
692 : //HACK at(0) because 1dof joint
693 165 : jp.at (jointName).at(0) = trigValue;
694 165 : itMap->second.setJointPos(jp);
695 :
696 165 : return false;
697 : }
698 :
699 : }
700 :
701 :
702 : /********************************************** SUPPORT FUNCTIONS ***************************************************************/
703 :
704 :
705 99431 : ROSEE::JointPos ROSEE::FindActions::getConvertedJointPos(const robot_state::RobotState* kinematic_state) {
706 :
707 99431 : JointPos jp;
708 286215 : for ( auto actJ : parserMoveIt->getActiveJointModels()) {
709 : //joint can have multiple pos, so double*, but we want to store in a vector
710 186784 : const double* pos = kinematic_state->getJointPositions(actJ);
711 186784 : unsigned posSize = sizeof(pos) / sizeof(double);
712 373568 : std::vector <double> vecPos(pos, pos + posSize);
713 186784 : jp.insert(std::make_pair(actJ->getName(), vecPos));
714 : }
715 99431 : return jp;
716 : }
717 :
718 :
719 68 : void ROSEE::FindActions::fillNotCollidingTips (
720 : std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose >* mapOfLoosePinches,
721 : const std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchTight >* mapOfPinches) {
722 :
723 : // first fill mapOfLoosePinches with all pairs ...
724 238 : for ( auto fingerEl1 : parserMoveIt->getFingertipOfFingerMap() ) {
725 612 : for ( auto fingerEl2 : parserMoveIt->getFingertipOfFingerMap() ) {
726 :
727 : // important to put in order in the pair, then in the set thing are autoordered
728 442 : if (fingerEl1.first < fingerEl2.first) {
729 136 : mapOfLoosePinches->insert (std::make_pair (std::make_pair (fingerEl1.first, fingerEl2.first), ActionPinchLoose(fingerEl1.first, fingerEl2.first)));
730 :
731 306 : } else if (fingerEl1.first > fingerEl2.first) {
732 136 : mapOfLoosePinches->insert (std::make_pair (std::make_pair (fingerEl2.first, fingerEl1.first), ActionPinchLoose(fingerEl2.first, fingerEl1.first)));
733 : }
734 : }
735 : }
736 :
737 : // ... then remove all the colliding tips
738 136 : for (const auto mapEl : *mapOfPinches){
739 68 : mapOfLoosePinches->erase(mapEl.first);
740 : }
741 68 : }
742 :
743 :
744 :
745 :
746 99429 : ROSEE::JointsInvolvedCount ROSEE::FindActions::setOnlyDependentJoints(
747 : std::pair < std::string, std::string > tipsNames, JointPos *jPos) {
748 :
749 99429 : JointsInvolvedCount jointsInvolvedCount;
750 :
751 286195 : for (auto &jp : *jPos) { //for each among ALL joints
752 :
753 186766 : jointsInvolvedCount.insert ( std::make_pair (jp.first, 1) );
754 :
755 : /** other way around, second is better?
756 : std::vector <std::string> jointOfTips1 = jointsOfFingertipMap.at(tipsNames.first);
757 : std::vector <std::string> jointOfTips2 = jointsOfFingertipMap.at(tipsNames.second);
758 :
759 : // if the joint is not linked with neither of the two colliding tips...
760 : if ( std::find( jointOfTips1.begin(), jointOfTips1.end(), jp.first) == jointOfTips1.end() &&
761 : std::find( jointOfTips2.begin(), jointOfTips2.end(), jp.first) == jointOfTips2.end() ) {
762 :
763 : std::fill ( jp.second.begin(), jp.second.end(), DEFAULT_JOINT_POS);
764 :
765 : IF USE THIS JOINTINVOLVEDCOUNT REMEMBER
766 : }
767 : */
768 :
769 : //the tips of the joint
770 373532 : std::vector < std::string> tips = parserMoveIt->getFingertipsOfJointMap().at(jp.first);
771 :
772 : //check if the two tips that collide are among the ones that the joint moves
773 437630 : if (std::find (tips.begin(), tips.end(), tipsNames.first) == tips.end() &&
774 250864 : std::find (tips.begin(), tips.end(), tipsNames.second) == tips.end() ) {
775 : // not dependant, set to default the position
776 26430 : std::fill ( jp.second.begin(), jp.second.end(), DEFAULT_JOINT_POS);
777 26430 : jointsInvolvedCount.at ( jp.first ) = 0;
778 : }
779 : }
780 :
781 99429 : return jointsInvolvedCount;
782 : }
783 :
784 :
785 2 : ROSEE::JointsInvolvedCount ROSEE::FindActions::setOnlyDependentJoints(
786 : std::set< std::string > tipsNames, JointPos *jPos) {
787 :
788 2 : JointsInvolvedCount jointsInvolvedCount;
789 :
790 20 : for (auto &jp : *jPos) { //for each among ALL joints
791 :
792 18 : jointsInvolvedCount.insert ( std::make_pair (jp.first, 0) );
793 :
794 : //the tips of the joint
795 36 : std::vector < std::string> tips = parserMoveIt->getFingertipsOfJointMap().at(jp.first);
796 :
797 : // if at least one tip of tipsNames is moved by jp.first joint, set the counter
798 : // and break the loop (because useless to continue
799 : // if no tip of tipsNames is moved by the joint, the count remain to zero and the
800 : // for ends normally
801 36 : for ( auto fingInv : tipsNames ) {
802 36 : if (std::find (tips.begin(), tips.end(), fingInv) != tips.end()) {
803 18 : jointsInvolvedCount.at ( jp.first ) = 1 ;
804 18 : break;
805 : }
806 : }
807 :
808 18 : if (jointsInvolvedCount.at ( jp.first ) == 0 ) {
809 0 : std::fill ( jp.second.begin(), jp.second.end(), DEFAULT_JOINT_POS);
810 : //not used joint, set to default state (all its dof)
811 : }
812 :
813 : }
814 2 : return jointsInvolvedCount;
815 : }
816 :
817 777000 : void ROSEE::FindActions::setToDefaultPositionPassiveJoints(moveit::core::RobotState * kinematic_state) {
818 :
819 777000 : const double pos = DEFAULT_JOINT_POS; //idk why but setJointPositions want a pointer as 2nd arg
820 777000 : for (auto joint : parserMoveIt->getPassiveJointNames()){
821 0 : kinematic_state->setJointPositions(joint, &pos);
822 : }
823 :
824 777000 : }
825 :
826 777000 : void ROSEE::FindActions::setToRandomPositions(moveit::core::RobotState * kinematic_state) {
827 :
828 : //NOTE this function will consider the mimic LINEAR. if in mimic tag only nlFunPos is written, default LINEAR args are
829 : //considered : multiplier 1 and offset 0. Then the joint which mimic someone will have same position of father, it is not
830 : // an error of randomic. Also, this is not a problem for us because below we overwrite the mimic sons, and we keep only
831 : // the random pos of actuated joints.
832 777000 : kinematic_state->setToRandomPositions();
833 :
834 : //when setting a joint pos (also a single one) moveit call also updateMimic using the standard linear params
835 : //we cant take the single functions inside the setJoint pos of moveit, because are private, so we
836 : //must always bear the updateMimic. So, here the joint pos of nonlinear mimic joint are ovewritten
837 : //with the correct non linear function
838 :
839 777000 : if (mimicNLRelMap.size() > 0 ) { //necessary if we have the for immediately after? faster?
840 :
841 0 : for (auto el : mimicNLRelMap) {
842 :
843 0 : double mimPos = 0;
844 :
845 : try {
846 : //HACK single dof joint
847 0 : double fatherPos = kinematic_state->getJointPositions(el.second.first)[0];
848 :
849 0 : mu::Parser p;
850 : //we assume that there is always a x in the expression
851 0 : p.DefineVar("x", &fatherPos);
852 0 : p.SetExpr(el.second.second);
853 0 : mimPos = p.Eval();
854 : }
855 :
856 0 : catch (mu::Parser::exception_type &e)
857 : {
858 0 : std::cout << e.GetMsg() << std::endl;
859 : std::cout << "[FINDACTIONS " << __func__ << "] Parsing of non linear function for mimic joint "
860 : << "'" << el.first << "'. Please be sure to put characther 'x' as (unique) variable for father position"
861 : << "in the expression. Have you used syntax valid for muparser?. Expression found: '"
862 0 : << el.second.second << "'" << std::endl;
863 :
864 0 : exit(-1); //TODO is it good to use exit?
865 :
866 : }
867 :
868 : //HACK single dof joint
869 0 : std::vector<double> one_dof ;
870 0 : one_dof.push_back(mimPos);
871 :
872 : //we enforce the bounds, in this way. calling at the end kinematic_state->enforceBounds() call internally updateMimicJoint
873 : //and we would have again problems.
874 0 : kinematic_state->getJointModel(el.first)->enforcePositionBounds(one_dof.data());
875 0 : kinematic_state->setJointPositions(el.first, one_dof);
876 :
877 :
878 : }
879 :
880 :
881 : }
882 :
883 777000 : setToDefaultPositionPassiveJoints(kinematic_state);
884 :
885 777000 : }
886 :
887 26007 : std::pair <std::string, std::string> ROSEE::FindActions::getFingersPair (std::pair <std::string, std::string> tipsPair) const {
888 :
889 : std::pair <std::string, std::string> fingersPair = std::make_pair (
890 52014 : parserMoveIt->getFingerOfFingertip(tipsPair.first),
891 104028 : parserMoveIt->getFingerOfFingertip(tipsPair.second) );
892 :
893 : //So we have the key pair always in lexicographical order
894 26007 : if ( fingersPair.first.compare (fingersPair.second) > 0 ) {
895 0 : auto temp = fingersPair.first;
896 0 : fingersPair.first = fingersPair.second;
897 0 : fingersPair.second = temp;
898 :
899 26007 : } else if (fingersPair.first.compare (fingersPair.second) == 0 ) {
900 : std::cout << "[FINDACTIONS " << __func__ << "] STRANGE ERROR: '" << tipsPair.first <<
901 : "' and '" << tipsPair.second << "' are in the same finger '" << fingersPair.first <<
902 0 : "' so this pair can't perform a pinch" << std::endl;
903 0 : return std::pair<std::string, std::string>();
904 : }
905 :
906 26007 : return fingersPair;
907 : }
908 :
909 2 : std::set <std::string> ROSEE::FindActions::getFingersSet (std::set <std::string> tipsSet) const {
910 :
911 4 : std::set <std::string> fingersSet;
912 8 : for (auto it : tipsSet) {
913 :
914 6 : fingersSet.insert ( parserMoveIt->getFingerOfFingertip ( it ) );
915 : }
916 :
917 : //If size is less, there is a finger that we have try to insert more than once.
918 2 : if ( fingersSet.size() < tipsSet.size() ) {
919 : std::cout << "[FINDACTIONS " << __func__ << "] STRANGE ERROR: " <<
920 : "the tipsSet passed has some fingertips that belong to the same finger."
921 0 : << " I will return an empty set " << std::endl;
922 :
923 0 : return std::set <std::string>();
924 : }
925 :
926 2 : return fingersSet;
927 : }
928 :
929 85068 : std::pair <std::string, std::string> ROSEE::FindActions::getFingertipsPair (std::pair <std::string, std::string> fingersPair) const {
930 :
931 : std::pair <std::string, std::string> tipsPair = std::make_pair (
932 170136 : parserMoveIt->getFingertipOfFinger(fingersPair.first),
933 340272 : parserMoveIt->getFingertipOfFinger(fingersPair.second) );
934 :
935 : //So we have the key pair always in lexicographical order
936 85068 : if ( tipsPair.first.compare (tipsPair.second) > 0 ) {
937 0 : auto temp = tipsPair.first;
938 0 : tipsPair.first = tipsPair.second;
939 0 : tipsPair.second = temp;
940 :
941 85068 : } else if (tipsPair.first.compare (tipsPair.second) == 0 ) {
942 : std::cout << "[FINDACTIONS " << __func__ << "] STRANGE ERROR: '" << fingersPair.first <<
943 : "' and '" << fingersPair.second << "' have the same fingertip '" << tipsPair.first <<
944 0 : "' so this pair can't perform a pinch" << std::endl;
945 0 : return std::pair<std::string, std::string>();
946 : }
947 :
948 85068 : return tipsPair;
949 60 : }
|
