ROSEndEffector
ROS End-Effector package: provides a ROS-based set of standard interfaces to command robotics end-effectors in an agnostic fashion.
Public Member Functions | Private Member Functions | Private Attributes | List of all members
ROSEE::FindActions Class Reference

Class to check which fingertips collide (for the pinch action at the moment) More...

#include <FindActions.h>

+ Collaboration diagram for ROSEE::FindActions:

Public Member Functions

 FindActions (std::shared_ptr< ROSEE::ParserMoveIt > parserMoveit)
 
std::pair< std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchTight >, std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > > findPinch (std::string path2saveYaml)
 Function to look for pinches, both Tight and Loose ones. More...
 
std::map< std::set< std::string >, ROSEE::ActionMultiplePinchTightfindMultiplePinch (unsigned int nFinger, std::string path2saveYaml, bool strict=true)
 Finder for MultiplePinch (a pinch done with more than 2 finger). More...
 
std::map< std::string, ROSEE::ActionTrigfindTrig (ROSEE::ActionPrimitive::Type actionType, std::string path2saveYaml)
 Function to look for trigs (trig, tipFlex and fingFlex). More...
 
std::map< std::string, ROSEE::ActionSingleJointMultipleTipsfindSingleJointMultipleTips (unsigned int nFinger, std::string path2saveYaml)
 

Private Member Functions

std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchTightcheckCollisions ()
 principal function which check for collisions with moveit functions when looking for tight pinches More...
 
void checkDistances (std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > *mapOfLoosePinches)
 Principal function which fill the mapOfLoosePinches basing on minumun distance between tips. More...
 
void checkWhichTipsCollideWithoutBounds (std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > *mapOfLoosePinches)
 Function similar to checkCollisions but used for Loose Pinches. More...
 
std::map< std::set< std::string >, ROSEE::ActionMultiplePinchTightcheckCollisionsForMultiplePinch (unsigned int nFinger, bool strict)
 support function for findMultiplePinch (a pinch done with more than 2 finger). More...
 
void removeBoundsOfNotCollidingTips (const std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > *mapOfLoosePinches, robot_model::RobotModelPtr kinematic_model_noBound)
 Support function to remove the joint limits from the model. More...
 
void fillNotCollidingTips (std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > *mapOfLoosePinches, const std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchTight > *mapOfPinches)
 function to "initialize" the map of ActionPinchLoose mapOfLoosePinches. More...
 
void changeFingertipsToFingerNames (std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchTight > *mapOfPinches, std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > *mapOfLoosePinches)
 this function take the two tight and loose maps and change the keys from fingertips names to their finger names. More...
 
std::map< std::string, ActionTrigtrig ()
 trig is the action of closing a SINGLE finger towards the palm. More...
 
std::map< std::string, ROSEE::ActionTrigtipFlex ()
 We start from each tip. More...
 
std::map< std::string, ROSEE::ActionTrigfingFlex ()
 We start from each tip. More...
 
bool insertJointPosForTrigInMap (std::map< std::string, ActionTrig > &trigMap, ROSEE::ActionTrig action, std::string jointName, double trigValue)
 Insert/update an ActionTrig in the trigMap. More...
 
ROSEE::JointsInvolvedCount setOnlyDependentJoints (std::pair< std::string, std::string > tipsNames, JointPos *jPos)
 Given the contact, we want to know the state of the joint to replicate it. More...
 
ROSEE::JointsInvolvedCount setOnlyDependentJoints (std::set< std::string > tipsNames, JointPos *jPos)
 Set to default pos the joints that do not move any of the tip included in the set. More...
 
JointPos getConvertedJointPos (const robot_state::RobotState *kinematic_state)
 Utility function to take the actuated joint positions from a kinematic_state and returns the same info as a JointPos map. More...
 
void setToDefaultPositionPassiveJoints (moveit::core::RobotState *kinematic_state)
 set to DEFAULT_JOINT_POS all the passive joints (defined so in the urdf file). More...
 
std::pair< std::string, std::string > getFingersPair (std::pair< std::string, std::string > tipsPair) const
 Giving as argument a pair of fingertips, this function return a pair of fingers that are the fingers which the two tips belong to. More...
 
std::set< std::string > getFingersSet (std::set< std::string > tipsSet) const
 Function used when looking for multiple pinches. More...
 
std::pair< std::string, std::string > getFingertipsPair (std::pair< std::string, std::string > fingersPair) const
 Given the fingersPair, this function return the pair of their fingers, in lexicographical order. More...
 
void setToRandomPositions (robot_state::RobotState *kinematic_state)
 This function set the random position of joint considering: More...
 

Private Attributes

std::shared_ptr< ROSEE::ParserMoveItparserMoveIt
 
std::map< std::string, std::pair< std::string, std::string > > mimicNLRelMap
 

Detailed Description

Class to check which fingertips collide (for the pinch action at the moment)

Warning
there is a problem with collisions: with the schunk hand, if we move only the middle (base phalange) toward the hand, a collision between index tip, middle tip and ring tip is detected. Easy reproducible with the moveit assistant, in the set pose section (it find a collision when visually is not present, when we move the middle). There are some caotic printing in bugmoveit branch, to replicate the problem also with this code. I dont know if it is a problem of schunk model, moveit, or both.

Definition at line 36 of file FindActions.h.

Constructor & Destructor Documentation

ROSEE::FindActions::FindActions ( std::shared_ptr< ROSEE::ParserMoveIt parserMoveit)

Definition at line 3 of file FindActions.cpp.

3  {
4 
6 
7  this->mimicNLRelMap = parserMoveIt->getMimicNLFatherOfJointMap();
8 }
std::map< std::string, std::pair< std::string, std::string > > mimicNLRelMap
Definition: FindActions.h:87
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84

Member Function Documentation

void ROSEE::FindActions::changeFingertipsToFingerNames ( std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchTight > *  mapOfPinches,
std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > *  mapOfLoosePinches 
)
private

this function take the two tight and loose maps and change the keys from fingertips names to their finger names.

Parameters
mapOfLoosePinches[out] Pointer to the map of ActionPinchLoose
mapOfPinches[out] pointer to the map of ActionPinchTight
Warning
The order in the pair is lexicographical so the first finger in the pair can refer to the second tip in the old key pair
std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchTight > ROSEE::FindActions::checkCollisions ( )
private

principal function which check for collisions with moveit functions when looking for tight pinches

Returns
the map of ActionPinchTight

create the actionPinch

Definition at line 249 of file FindActions.cpp.

249  {
250 
251  std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchTight > mapOfPinches;
252 
253  planning_scene::PlanningScene planning_scene ( parserMoveIt->getRobotModel() );
254  collision_detection::CollisionRequest collision_request;
255  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  collision_request.contacts = true; //set to compute collisions
263  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  collision_detection::AllowedCollisionMatrix acm;
270  acm.setEntry(parserMoveIt->getRobotModel()->getLinkModelNames(),
271  parserMoveIt->getRobotModel()->getLinkModelNames(), true); //true==not considered collisions
272  acm.setEntry(parserMoveIt->getFingertipNames(),
273  parserMoveIt->getFingertipNames(), false); //false== considered collisions
274 
275  robot_state::RobotState kinematic_state(parserMoveIt->getRobotModel());
276 
277  for (int i = 0; i < N_EXP_COLLISION; i++){
278 
279  std::stringstream logCollision;
280  collision_result.clear();
281 
282  setToRandomPositions(&kinematic_state);
283 
284  planning_scene.checkSelfCollision(collision_request, collision_result, kinematic_state, acm);
285 
286  if (collision_result.collision) {
287 
288  //for each collision with this joints state...
289  for (auto cont : collision_result.contacts){
290 
291  //store joint states
292  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  cont.first.second << std::endl;
297 
298  for (auto contInfo : cont.second){
299  logCollision << "\tWith a depth of contact: " << contInfo.depth;
300  }
301 
302  JointsInvolvedCount jointsInvolvedCount = setOnlyDependentJoints(cont.first, &jointPos);
303 
305 
306  // get the finger name
307  auto fingerPair = getFingersPair(cont.first);
308 
309  ActionPinchTight pinch (fingerPair, jointPos, cont.second.at(0) );
310  pinch.setJointsInvolvedCount ( jointsInvolvedCount );
311  auto itFind = mapOfPinches.find ( fingerPair );
312  if ( itFind == mapOfPinches.end() ) {
313  //if here, we have to create store the new created action
314  mapOfPinches.insert ( std::make_pair (fingerPair, pinch) );
315  logCollision << ", NEW INSERTION";
316 
317  } else { //Check if it is the best depth among the found collision among that pair
318  if (itFind->second.insertActionState( jointPos, cont.second.at(0)) ) {
319  logCollision << ", NEW INSERTION";
320  }
321  }
322  logCollision << std::endl;
323  logCollision << jointPos;
324  }
325  //this print is for debugging purposes
326  //std::cout << logCollision.str() << std::endl;
327  }
328  }
329 
330  return mapOfPinches;
331 }
#define N_EXP_COLLISION
Definition: FindActions.h:18
std::map< std::string, std::vector< double > > JointPos
The map to describe the position of all actuated joints.
Definition: Action.h:40
std::map< std::string, unsigned int > JointsInvolvedCount
The map to describe, how many times a joint is set by the action.
Definition: Action.h:63
std::pair< std::string, std::string > getFingersPair(std::pair< std::string, std::string > tipsPair) const
Giving as argument a pair of fingertips, this function return a pair of fingers that are the fingers ...
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
JointPos getConvertedJointPos(const robot_state::RobotState *kinematic_state)
Utility function to take the actuated joint positions from a kinematic_state and returns the same inf...
void setToRandomPositions(robot_state::RobotState *kinematic_state)
This function set the random position of joint considering:
The action of pinch with two tips.
ROSEE::JointsInvolvedCount setOnlyDependentJoints(std::pair< std::string, std::string > tipsNames, JointPos *jPos)
Given the contact, we want to know the state of the joint to replicate it.
std::map< std::set< std::string >, ROSEE::ActionMultiplePinchTight > ROSEE::FindActions::checkCollisionsForMultiplePinch ( unsigned int  nFinger,
bool  strict 
)
private

support function for findMultiplePinch (a pinch done with more than 2 finger).

This is done similarly to normal pinch, but there are more checks to see if the collision is among more than only two fingertips.

Moveit, when looking for collisions, return only pairs of links that collide. So we need to handle all the found pairs and "put them togheter" in someway. We need at least

Parameters
nFinger- 1 collision: eg. for triPinch -> 2collision, for 4finger pinch -> 3 collision. But, with only this check, we can also find a configuration whith two distint normal pinch. To solve, we also check if the number of tips that collide in this configuration is exaclty
nFinger,egwith 2 collision we can have 4 finger colliding because there are two normal distinct pinch and not a 3-pinch... so we exlude these collisions. The
strictcan solves another problem. If it is true (default) we take only the multiple pinch where each finger collide with all the other finger involved in the pinch. If it is false, we can find also pinch where the tips are "in line" : finger_2 collide with finger_1 and finger_3, but finger_1 and 3 do not collide. With strict we will find less groups of fingers that collide, but, in someway, they collide "better".
nFinger(2 < nFinger <= max_finger). The type of the multiple pinch that we want. The name of the returned action will be based on this param : "MultiplePinchTight-(nFinger)"
stricttrue to look only for "strict" multiple pinch. Look this funcion description
Returns
A map with as keys set of size nFinger, and as value an ActionMultiplePinchTight object

Definition at line 499 of file FindActions.cpp.

499  {
500 
501  std::map < std::set <std::string> , ROSEE::ActionMultiplePinchTight > mapOfMultPinches;
502 
503  unsigned int nMinCollision = strict ?
504  ROSEE::Utils::binomial_coefficent(nFinger, 2) : (nFinger-1);
505 
506  planning_scene::PlanningScene planning_scene ( parserMoveIt->getRobotModel() );
507  collision_detection::CollisionRequest collision_request;
508  collision_detection::CollisionResult collision_result;
509  collision_request.contacts = true; //set to compute collisions
510  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  collision_detection::AllowedCollisionMatrix acm;
516  acm.setEntry(parserMoveIt->getRobotModel()->getLinkModelNames(),
517  parserMoveIt->getRobotModel()->getLinkModelNames(), true); //true==not considered collisions
518  acm.setEntry(parserMoveIt->getFingertipNames(),
519  parserMoveIt->getFingertipNames(), false); //false== considered collisions
520 
521  robot_state::RobotState kinematic_state(parserMoveIt->getRobotModel());
522 
523  for (int i = 0; i < N_EXP_COLLISION_MULTPINCH; i++){
524 
525  collision_result.clear();
526  setToRandomPositions(&kinematic_state);
527 
528  planning_scene.checkSelfCollision(collision_request, collision_result, kinematic_state, acm);
529 
530  if (collision_result.contacts.size() >= nMinCollision ) {
531 
532  double depthSum = 0;
533  std::set <std::string> tipsColliding;
534  for (auto cont : collision_result.contacts){
535 
536  tipsColliding.insert(cont.first.first);
537  tipsColliding.insert(cont.first.second);
538  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  if (tipsColliding.size() != nFinger) {
544  continue;
545  }
546 
547  //store joint states
548  JointPos jointPos = getConvertedJointPos(&kinematic_state);
549  JointsInvolvedCount jointsInvolvedCount = setOnlyDependentJoints(tipsColliding, &jointPos);
550 
551  auto fingerColliding = getFingersSet(tipsColliding);
552 
553  ActionMultiplePinchTight pinch (fingerColliding, jointPos, depthSum );
554  pinch.setJointsInvolvedCount ( jointsInvolvedCount );
555  auto itFind = mapOfMultPinches.find ( fingerColliding );
556  if ( itFind == mapOfMultPinches.end() ) {
557  //if here, we have to create store the new created action
558  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  if (itFind->second.insertActionState( jointPos, depthSum ) ) {
562  //print debug
563  } else {
564  //pring debug
565  }
566  }
567  }
568  }
569  return mapOfMultPinches;
570 }
std::map< std::string, std::vector< double > > JointPos
The map to describe the position of all actuated joints.
Definition: Action.h:40
std::map< std::string, unsigned int > JointsInvolvedCount
The map to describe, how many times a joint is set by the action.
Definition: Action.h:63
Class to describe the action of "pinching" with more than 2 tips (with 2 tips there is the ActionPinc...
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
#define N_EXP_COLLISION_MULTPINCH
Definition: FindActions.h:20
JointPos getConvertedJointPos(const robot_state::RobotState *kinematic_state)
Utility function to take the actuated joint positions from a kinematic_state and returns the same inf...
static int binomial_coefficent(int n, int k)
Definition: Utils.h:77
void setToRandomPositions(robot_state::RobotState *kinematic_state)
This function set the random position of joint considering:
ROSEE::JointsInvolvedCount setOnlyDependentJoints(std::pair< std::string, std::string > tipsNames, JointPos *jPos)
Given the contact, we want to know the state of the joint to replicate it.
std::set< std::string > getFingersSet(std::set< std::string > tipsSet) const
Function used when looking for multiple pinches.
void ROSEE::FindActions::checkDistances ( std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > *  mapOfLoosePinches)
private

Principal function which fill the mapOfLoosePinches basing on minumun distance between tips.

Parameters
mapOfLoosePinches[out] pointer to the mapOfLoosePinches to be filled

Definition at line 334 of file FindActions.cpp.

334  {
335 
336  robot_state::RobotState kinematic_state(parserMoveIt->getRobotModel());
337 
338  for (int i = 0; i < N_EXP_DISTANCES; i++){
339 
340  setToRandomPositions(&kinematic_state);
341 
342  //for each pair remaining in notCollidingTips, check if a new min distance is found
343  for (auto &mapEl : *mapOfLoosePinches) {
344 
345  // restore all joint pos
346  JointPos jointPosLoose = getConvertedJointPos(&kinematic_state);
347 
348  auto tips = getFingertipsPair(mapEl.first);
349 
350  JointsInvolvedCount jointsInvolvedCount = setOnlyDependentJoints(tips, &jointPosLoose);
351 
352  Eigen::Affine3d tip1Trasf = kinematic_state.getGlobalLinkTransform(tips.first);
353  Eigen::Affine3d tip2Trasf = kinematic_state.getGlobalLinkTransform(tips.second);
354  double distance = (tip1Trasf.translation() - tip2Trasf.translation() ) .norm() ;
355 
356  mapEl.second.insertActionState( jointPosLoose, distance ) ;
357  mapEl.second.setJointsInvolvedCount ( jointsInvolvedCount );
358  }
359  }
360 }
#define N_EXP_DISTANCES
Definition: FindActions.h:19
std::map< std::string, std::vector< double > > JointPos
The map to describe the position of all actuated joints.
Definition: Action.h:40
std::map< std::string, unsigned int > JointsInvolvedCount
The map to describe, how many times a joint is set by the action.
Definition: Action.h:63
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
std::pair< std::string, std::string > getFingertipsPair(std::pair< std::string, std::string > fingersPair) const
Given the fingersPair, this function return the pair of their fingers, in lexicographical order...
JointPos getConvertedJointPos(const robot_state::RobotState *kinematic_state)
Utility function to take the actuated joint positions from a kinematic_state and returns the same inf...
void setToRandomPositions(robot_state::RobotState *kinematic_state)
This function set the random position of joint considering:
ROSEE::JointsInvolvedCount setOnlyDependentJoints(std::pair< std::string, std::string > tipsNames, JointPos *jPos)
Given the contact, we want to know the state of the joint to replicate it.
void ROSEE::FindActions::checkWhichTipsCollideWithoutBounds ( std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > *  mapOfLoosePinches)
private

Function similar to checkCollisions but used for Loose Pinches.

First, we temporarily remove bounds of joints linked to the non-colliding tips (with removeBoundsOfNotCollidingTips), and we check for collision with this function. If some collision are found, this means that tips movements make them go towards each other, (also with the bounds) but the joint limits do not permit them to touch. This is a loose pinch. If they do not collide even without bounds, this means that they never go towards each other, so this is not a tight neither loose pinch. We also create new kinematic_model object so we dont modify the original kinematic_model, and we can change the joint limits of the new copy safely

Parameters
mapOfLoosePinches[out] map of loose pinches that will be filled with info about these particular actions

Definition at line 445 of file FindActions.cpp.

446  {
447 
448  robot_model::RobotModelPtr kinematic_model_noBound = parserMoveIt->getCopyModel();
449 
450  removeBoundsOfNotCollidingTips (mapOfLoosePinches, kinematic_model_noBound );
451 
452  collision_detection::AllowedCollisionMatrix acm;
453  acm.setEntry(kinematic_model_noBound->getLinkModelNames(),
454  kinematic_model_noBound->getLinkModelNames(), true); //true == not considered collisions
455 
456  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  std::string tip1 = parserMoveIt->getFingertipOfFinger(it.first.first);
460  std::string tip2 = parserMoveIt->getFingertipOfFinger(it.first.second);
461  acm.setEntry(tip1, tip2, false); //false == considered collisions
462  }
463 
464  planning_scene::PlanningScene planning_scene(kinematic_model_noBound);
465 
466  collision_detection::CollisionRequest collision_request;
467  collision_detection::CollisionResult collision_result;
468  collision_request.contacts = true; //set to compute collisions
469  collision_request.max_contacts = 1000;
470 
471  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  std::set < std::pair<std::string, std::string> > collidingFingers ;
475  for (int i = 0; i < N_EXP_COLLISION; i++){
476  collision_result.clear();
477  setToRandomPositions(&kinematic_state);
478 
479  planning_scene.checkSelfCollision(collision_request, collision_result, kinematic_state, acm);
480 
481  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  collidingFingers.insert ( getFingersPair (cont.first) );
484  }
485  }
486 
487  //erase from loose map the not colliding tips (: not colliding even without bounds)
488  for (auto mapEl = mapOfLoosePinches->cbegin(); mapEl != mapOfLoosePinches->cend() ; /*no increment*/ ) {
489 
490  if (collidingFingers.count(mapEl->first) == 0 ) {
491  mapOfLoosePinches->erase(mapEl++);
492  } else {
493  ++mapEl;
494  }
495  }
496 }
#define N_EXP_COLLISION
Definition: FindActions.h:18
std::pair< std::string, std::string > getFingersPair(std::pair< std::string, std::string > tipsPair) const
Giving as argument a pair of fingertips, this function return a pair of fingers that are the fingers ...
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
void removeBoundsOfNotCollidingTips(const std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > *mapOfLoosePinches, robot_model::RobotModelPtr kinematic_model_noBound)
Support function to remove the joint limits from the model.
void setToRandomPositions(robot_state::RobotState *kinematic_state)
This function set the random position of joint considering:
void ROSEE::FindActions::fillNotCollidingTips ( std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > *  mapOfLoosePinches,
const std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchTight > *  mapOfPinches 
)
private

function to "initialize" the map of ActionPinchLoose mapOfLoosePinches.

It is done adding all the tips pairs and then removing the pairs that are present in the map of ActionPinchTight mapOfPinches. Note that the values of the map, the ActionPinchLoose are action with only tips name (so no info right now).

Parameters
mapOfLoosePinches[out] Pointer to the map of ActionPinchLoose to be initialized
mapOfPinches[in] pointer to the map of ActionPinchTight, already filled before, that is used to erase the get the tips that collide and to remove them from the mapOfLoosePinches

Definition at line 719 of file FindActions.cpp.

721  {
722 
723  // first fill mapOfLoosePinches with all pairs ...
724  for ( auto fingerEl1 : parserMoveIt->getFingertipOfFingerMap() ) {
725  for ( auto fingerEl2 : parserMoveIt->getFingertipOfFingerMap() ) {
726 
727  // important to put in order in the pair, then in the set thing are autoordered
728  if (fingerEl1.first < fingerEl2.first) {
729  mapOfLoosePinches->insert (std::make_pair (std::make_pair (fingerEl1.first, fingerEl2.first), ActionPinchLoose(fingerEl1.first, fingerEl2.first)));
730 
731  } else if (fingerEl1.first > fingerEl2.first) {
732  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  for (const auto mapEl : *mapOfPinches){
739  mapOfLoosePinches->erase(mapEl.first);
740  }
741 }
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
std::map< std::set< std::string >, ROSEE::ActionMultiplePinchTight > ROSEE::FindActions::findMultiplePinch ( unsigned int  nFinger,
std::string  path2saveYaml,
bool  strict = true 
)

Finder for MultiplePinch (a pinch done with more than 2 finger).

This function return the found multpinch primitive but it also print this result in a yaml file. See checkCollisionsForMultiplePinch doc for more info.

Parameters
nFinger(2 < nFinger <= max_finger). The type of the multiple pinch that we want. The name of the returned action will be based on this param : "MultiplePinchTight-(nFinger)"
stricttrue to look only for "strict" multiple pinch, i.e. where all tips collide with each other (and do not collide in "line") See checkCollisionsForMultiplePinch doc for more info.
path2saveYamlthe path where to create/overwrite the yaml files.
Returns
map of founded ActionMultiplePinchTight,

Definition at line 214 of file FindActions.cpp.

215  {
216 
217  std::map<std::set<std::string>, ROSEE::ActionMultiplePinchTight> multiplePinchMap;
218  if (nFinger < 3 ) {
219  std::cerr << "[ERROR " << __func__ << "] for this find pass at least 3 as number " <<
220  " of fingertips for the pinch" << std::endl;
221  return multiplePinchMap;
222  }
223 
224  multiplePinchMap = checkCollisionsForMultiplePinch(nFinger, strict);
225 
227  if (multiplePinchMap.size() == 0 ) {
228  return multiplePinchMap;
229  }
230  std::map < std::set <std::string> , ActionPrimitive* > mapForWorker;
231 
232  for (auto& it : multiplePinchMap) { // auto& and not auto alone!
233 
234  ActionPrimitive* pointer = &(it.second);
235  mapForWorker.insert (std::make_pair ( it.first, pointer ) );
236  }
237 
238  ROSEE::YamlWorker yamlWorker;
239  yamlWorker.createYamlFile(mapForWorker, multiplePinchMap.begin()->second.getName(), path2saveYaml);
240 
241  return multiplePinchMap;
242 }
Class to describe the action of "pinching" with more than 2 tips (with 2 tips there is the ActionPinc...
std::map< std::set< std::string >, ROSEE::ActionMultiplePinchTight > checkCollisionsForMultiplePinch(unsigned int nFinger, bool strict)
support function for findMultiplePinch (a pinch done with more than 2 finger).
std::string createYamlFile(const std::map< std::set< std::string >, ActionPrimitive * > mapOfActions, const std::string actionName, std::string pathFolder)
Create/overwrite yaml file and emit info on it about each ActionPrimitive inside the given mapOfActio...
Definition: YamlWorker.cpp:39
std::pair< std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchTight >, std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > > ROSEE::FindActions::findPinch ( std::string  path2saveYaml)

Function to look for pinches, both Tight and Loose ones.

It fill the maps (returned as pair), but also print the infos in two yaml files (one for tight, one for loose) using a YamlWorker

Parameters
path2saveYamlthe path where to create/overwrite the yaml files.
Returns
a pair of maps. The first is the map of ActionPinchTight, the second the map of ActionPinchLoose

EMITTING PART ................

Definition at line 13 of file FindActions.cpp.

13  {
14 
15  std::map < std::pair <std::string, std::string> , ActionPinchTight > mapOfPinches = checkCollisions();
16  std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose > mapOfLoosePinches;
17  fillNotCollidingTips(&mapOfLoosePinches, &mapOfPinches);
18 
19  checkWhichTipsCollideWithoutBounds ( &mapOfLoosePinches ) ;
20 
21  if (mapOfLoosePinches.size() != 0 ){
22  checkDistances (&mapOfLoosePinches) ;
23  }
24 
26  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  << " set a bigger value in N_EXP_COLLISION" << std::endl;
31 
32  } else {
33 
34  ROSEE::YamlWorker yamlWorker;
35  std::map < std::set <std::string> , ActionPrimitive* > mapForWorker;
36 
37  for (auto& it : mapOfPinches) { // auto& and not auto alone!
38 
39  ActionPrimitive* pointer = &(it.second);
40  std::set < std::string > keys ;
41  keys.insert (it.first.first) ;
42  keys.insert (it.first.second) ;
43  mapForWorker.insert (std::make_pair ( keys, pointer ) );
44  }
45 
46  yamlWorker.createYamlFile(mapForWorker, "pinchTight", path2saveYaml);
47  }
48 
49  if (mapOfLoosePinches.size() == 0 ) {
50  std::cout << "[FINDACTIONS::" << __func__ << "]: I found no loose pinches. This mean that some error happened or that" <<
51  " all the tips pairs collide with each other for at least one hand configuration." << std::endl;
52 
53  } else {
54 
55  ROSEE::YamlWorker yamlWorker;
56  std::map < std::set <std::string> , ActionPrimitive* > mapForWorker;
57 
58  for (auto& it : mapOfLoosePinches) { // auto& and not auto alone!
59 
60  ActionPrimitive* pointer = &(it.second);
61  std::set < std::string > keys ;
62  keys.insert (it.first.first) ;
63  keys.insert (it.first.second) ;
64  mapForWorker.insert (std::make_pair ( keys, pointer ) );
65  }
66 
67  yamlWorker.createYamlFile(mapForWorker, "pinchLoose", path2saveYaml);
68  }
69 
70  return std::make_pair(mapOfPinches, mapOfLoosePinches);
71 }
void checkDistances(std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > *mapOfLoosePinches)
Principal function which fill the mapOfLoosePinches basing on minumun distance between tips...
void checkWhichTipsCollideWithoutBounds(std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > *mapOfLoosePinches)
Function similar to checkCollisions but used for Loose Pinches.
std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchTight > checkCollisions()
principal function which check for collisions with moveit functions when looking for tight pinches ...
The action of pinch with two tips.
void fillNotCollidingTips(std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > *mapOfLoosePinches, const std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchTight > *mapOfPinches)
function to "initialize" the map of ActionPinchLoose mapOfLoosePinches.
std::string createYamlFile(const std::map< std::set< std::string >, ActionPrimitive * > mapOfActions, const std::string actionName, std::string pathFolder)
Create/overwrite yaml file and emit info on it about each ActionPrimitive inside the given mapOfActio...
Definition: YamlWorker.cpp:39
std::map< std::string, ROSEE::ActionSingleJointMultipleTips > ROSEE::FindActions::findSingleJointMultipleTips ( unsigned int  nFinger,
std::string  path2saveYaml 
)

Definition at line 137 of file FindActions.cpp.

137  {
138 
139  std::map <std::string, ROSEE::ActionSingleJointMultipleTips> mapOfSingleJointMultipleTips;
140 
141  if (nFinger <= 0) {
142  std::cout << "[ERROR FINDACTIONS::" << __func__ << "] please pass a positive number " <<
143  " as number of fingers. You passed " << nFinger << " ! " << std::endl;
144  return mapOfSingleJointMultipleTips;
145  }
146 
147  if (nFinger == 1) {
148  std::cout << "[ERROR FINDACTIONS::" << __func__ << "] with 1 finger, you are looking for a ActionTrig, "
149  << "and not a ActionSingleJointMultipleTips. Returning an empty map" << std::endl;
150  return mapOfSingleJointMultipleTips;
151  }
152 
153  if (nFinger > parserMoveIt->getNFingers() ) {
154  std::cout << "[ERROR FINDACTIONS::" << __func__ << "] I can not find an action which moves " << nFinger <<
155  " fingers if the hand has only " << parserMoveIt->getNFingers() << " fingers. Returning an empty map" << std::endl;
156  return mapOfSingleJointMultipleTips;
157  }
158 
159  std::string actionName = "singleJointMultipleTips_" + std::to_string(nFinger); //action name same for each action
160 
161  for (auto mapEl : parserMoveIt->getFingertipsOfJointMap() ) {
162 
163  if (mapEl.second.size() != nFinger ) {
164  continue;
165  }
166 
167  std::vector<double> furtherPos = parserMoveIt->getBiggerBoundFromZero(mapEl.first);
168  std::vector<double> nearerPos = parserMoveIt->getSmallerBoundFromZero(mapEl.first);
169 
170  //create and initialize JointPos map
171  JointPos jpFar;
172  for (auto it : parserMoveIt->getActiveJointModels()){
173  std::vector <double> jPos (it->getVariableCount(), DEFAULT_JOINT_POS);
174  jpFar.insert ( std::make_pair ( it->getName(), jPos ));
175  }
176  JointPos jpNear = jpFar;
177 
178  jpFar.at ( mapEl.first ) = furtherPos;
179  jpNear.at ( mapEl.first ) = nearerPos;
180 
181  std::vector<std::string> fingersInvolved;
182  for (auto tip : mapEl.second){
183  fingersInvolved.push_back(parserMoveIt->getFingerOfFingertip (tip) );
184  }
185 
186  ActionSingleJointMultipleTips action (actionName, fingersInvolved, mapEl.first, jpFar, jpNear);
187 
188  mapOfSingleJointMultipleTips.insert (std::make_pair(mapEl.first, action));
189  }
190 
192  if (mapOfSingleJointMultipleTips.size() == 0 ) {
193  std::cout << "[FINDACTIONS::" << __func__ << "] no singleJointMultipleTips with " << nFinger << " found" << std::endl;
194  return mapOfSingleJointMultipleTips;
195  }
196 
197  std::map < std::set <std::string> , ActionPrimitive* > mapForWorker;
198 
199  for (auto& it : mapOfSingleJointMultipleTips) { // auto& and not auto alone!
200 
201  ActionPrimitive* pointer = &(it.second);
202  std::set<std::string> set;
203  set.insert (it.first);
204  mapForWorker.insert (std::make_pair ( set, pointer ) );
205  }
206 
207  ROSEE::YamlWorker yamlWorker;
208  yamlWorker.createYamlFile(mapForWorker, actionName, path2saveYaml);
209 
210  return mapOfSingleJointMultipleTips;
211 }
std::map< std::string, std::vector< double > > JointPos
The map to describe the position of all actuated joints.
Definition: Action.h:40
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
std::string createYamlFile(const std::map< std::set< std::string >, ActionPrimitive * > mapOfActions, const std::string actionName, std::string pathFolder)
Create/overwrite yaml file and emit info on it about each ActionPrimitive inside the given mapOfActio...
Definition: YamlWorker.cpp:39
#define DEFAULT_JOINT_POS
Definition: FindActions.h:21
std::map< std::string, ROSEE::ActionTrig > ROSEE::FindActions::findTrig ( ROSEE::ActionPrimitive::Type  actionType,
std::string  path2saveYaml 
)

Function to look for trigs (trig, tipFlex and fingFlex).

The type of trig to be looked for is choosen thanks to the argument actionType. This function return the map of wanted trig but also print info about that on a yaml file using a YamlWorker

Parameters
actionTypethe type of trig to look for (Trig, TipFlex, FingFlex)
path2saveYamlthe path where to create/overwrite the yaml file.
Returns
the map of chosen type of trig filled with infos about the possible actions.

Definition at line 73 of file FindActions.cpp.

74  {
75 
76 
77  std::map <std::string, ActionTrig> trigMap;
78 
79  switch (actionType) {
80  case ROSEE::ActionPrimitive::Type::Trig : {
81  trigMap = trig();
82  break;
83  }
84  case ROSEE::ActionPrimitive::Type::TipFlex : {
85  trigMap = tipFlex();
86  break;
87  }
88  case ROSEE::ActionPrimitive::Type::FingFlex : {
89  trigMap = fingFlex();
90  break;
91  }
92  default: {
93  std::cout << "[ERROR FINDACTIONS::" << __func__ << "]: Passing as argument a action type which is not a type of trig. " << std::endl
94  << "I am returing an empty map" << std::endl;
95  return trigMap;
96  }
97  }
98 
99  if (trigMap.size() == 0 ) { //if so, no sense to continue
100  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  for (auto & mapEl : trigMap) {
106 
107  ROSEE::JointsInvolvedCount jointsInvolvedCount;
108  for ( auto joint : mapEl.second.getJointPos() ) {
109  jointsInvolvedCount.insert (std::make_pair(joint.first, 0));
110  for (auto dof : joint.second) {
111  if (dof != DEFAULT_JOINT_POS){
112  jointsInvolvedCount.at(joint.first) = 1;
113  break;
114  }
115  }
116  }
117  mapEl.second.setJointsInvolvedCount (jointsInvolvedCount);
118  }
119 
120  std::map < std::set <std::string> , ActionPrimitive* > mapForWorker;
121 
122  for (auto& it : trigMap) { // auto& and not auto alone!
123 
124  ActionPrimitive* pointer = &(it.second);
125  std::set < std::string > keys ;
126  keys.insert (it.first) ;
127  mapForWorker.insert (std::make_pair ( keys, pointer ) );
128  }
129 
130  ROSEE::YamlWorker yamlWorker;
131  yamlWorker.createYamlFile(mapForWorker, trigMap.begin()->second.getName(), path2saveYaml);
132 
133  return trigMap;
134 }
std::map< std::string, ROSEE::ActionTrig > tipFlex()
We start from each tip.
std::map< std::string, unsigned int > JointsInvolvedCount
The map to describe, how many times a joint is set by the action.
Definition: Action.h:63
std::map< std::string, ROSEE::ActionTrig > fingFlex()
We start from each tip.
std::map< std::string, ActionTrig > trig()
trig is the action of closing a SINGLE finger towards the palm.
std::string createYamlFile(const std::map< std::set< std::string >, ActionPrimitive * > mapOfActions, const std::string actionName, std::string pathFolder)
Create/overwrite yaml file and emit info on it about each ActionPrimitive inside the given mapOfActio...
Definition: YamlWorker.cpp:39
#define DEFAULT_JOINT_POS
Definition: FindActions.h:21
std::map< std::string, ROSEE::ActionTrig > ROSEE::FindActions::fingFlex ( )
private

We start from each tip.

Given a tip, we check if ParserMoveIt::getNExclusiveJointsOfTip >= 2 (see tipFlex function). If so, we continue exploring the chain from the tip going up through the parents. We stop when a parent has more than 1 joint as child. This means that the last link is the first of the finger. Meanwhile we have stored the actuated, not continuos joint (in joint) that we were founding along the chain. The last stored is exaclty theInterestingJoint, which pose of we must set. All the other joints (actuated) will have the default position (if no strange errors).

Definition at line 637 of file FindActions.cpp.

637  {
638 
639  std::map <std::string, ROSEE::ActionTrig> fingFlexMap;
640 
641  for (auto tipName : parserMoveIt->getFingertipNames() ) {
642 
643  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  continue;
646  }
647 
648  std::string theInterestingJoint = parserMoveIt->getFirstActuatedJointInFinger ( tipName );
649  double fingFlexMax = parserMoveIt->getBiggerBoundFromZero ( theInterestingJoint ).at(0) ;
650 
651  ActionTrig action ("fingFlex", ActionPrimitive::Type::FingFlex);
652  action.setFingerInvolved ( parserMoveIt->getFingerOfFingertip ( tipName) ) ;
653  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  std::cout << "[FATAL ERROR FINDACTIONS::" << __func__ << "]: Inserting in fingFlexMap a tip already present??n" << std::endl
659  << "I am returning a not completely filled map" << std::endl;
660  return fingFlexMap;
661  }
662  }
663  return fingFlexMap;
664 }
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
bool insertJointPosForTrigInMap(std::map< std::string, ActionTrig > &trigMap, ROSEE::ActionTrig action, std::string jointName, double trigValue)
Insert/update an ActionTrig in the trigMap.
ROSEE::JointPos ROSEE::FindActions::getConvertedJointPos ( const robot_state::RobotState *  kinematic_state)
private

Utility function to take the actuated joint positions from a kinematic_state and returns the same info as a JointPos map.

Parameters
kinematic_state[in] pointer to the robot_state class
Returns
JointPos the map with the joint positions info

Definition at line 705 of file FindActions.cpp.

705  {
706 
707  JointPos jp;
708  for ( auto actJ : parserMoveIt->getActiveJointModels()) {
709  //joint can have multiple pos, so double*, but we want to store in a vector
710  const double* pos = kinematic_state->getJointPositions(actJ);
711  unsigned posSize = sizeof(pos) / sizeof(double);
712  std::vector <double> vecPos(pos, pos + posSize);
713  jp.insert(std::make_pair(actJ->getName(), vecPos));
714  }
715  return jp;
716 }
std::map< std::string, std::vector< double > > JointPos
The map to describe the position of all actuated joints.
Definition: Action.h:40
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
std::pair< std::string, std::string > ROSEE::FindActions::getFingersPair ( std::pair< std::string, std::string >  tipsPair) const
private

Giving as argument a pair of fingertips, this function return a pair of fingers that are the fingers which the two tips belong to.

Returns
a pair of string containing the fingers which the passed tips belong to

Definition at line 887 of file FindActions.cpp.

887  {
888 
889  std::pair <std::string, std::string> fingersPair = std::make_pair (
890  parserMoveIt->getFingerOfFingertip(tipsPair.first),
891  parserMoveIt->getFingerOfFingertip(tipsPair.second) );
892 
893  //So we have the key pair always in lexicographical order
894  if ( fingersPair.first.compare (fingersPair.second) > 0 ) {
895  auto temp = fingersPair.first;
896  fingersPair.first = fingersPair.second;
897  fingersPair.second = temp;
898 
899  } 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  "' so this pair can't perform a pinch" << std::endl;
903  return std::pair<std::string, std::string>();
904  }
905 
906  return fingersPair;
907 }
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
std::set< std::string > ROSEE::FindActions::getFingersSet ( std::set< std::string >  tipsSet) const
private

Function used when looking for multiple pinches.

It returns the set containing the fingers of the passed fingertips.

Parameters
tipsSetthe set of fingertips names
Returns
the set of fingers. Empty set if the some tips in the tipsSet are in the same finger (that is an error)

Definition at line 909 of file FindActions.cpp.

909  {
910 
911  std::set <std::string> fingersSet;
912  for (auto it : tipsSet) {
913 
914  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  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  << " I will return an empty set " << std::endl;
922 
923  return std::set <std::string>();
924  }
925 
926  return fingersSet;
927 }
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
std::pair< std::string, std::string > ROSEE::FindActions::getFingertipsPair ( std::pair< std::string, std::string >  fingersPair) const
private

Given the fingersPair, this function return the pair of their fingers, in lexicographical order.

Returns
a pair of string containing the fingers which the passed tips belong to

Definition at line 929 of file FindActions.cpp.

929  {
930 
931  std::pair <std::string, std::string> tipsPair = std::make_pair (
932  parserMoveIt->getFingertipOfFinger(fingersPair.first),
933  parserMoveIt->getFingertipOfFinger(fingersPair.second) );
934 
935  //So we have the key pair always in lexicographical order
936  if ( tipsPair.first.compare (tipsPair.second) > 0 ) {
937  auto temp = tipsPair.first;
938  tipsPair.first = tipsPair.second;
939  tipsPair.second = temp;
940 
941  } 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  "' so this pair can't perform a pinch" << std::endl;
945  return std::pair<std::string, std::string>();
946  }
947 
948  return tipsPair;
949 }
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
bool ROSEE::FindActions::insertJointPosForTrigInMap ( std::map< std::string, ActionTrig > &  trigMap,
ROSEE::ActionTrig  action,
std::string  jointName,
double  trigValue 
)
private

Insert/update an ActionTrig in the trigMap.

This is done setting the jointName position to the given jointName. So, for a single action this function can be executed more than once (because more joint can be set). The Action action can be already present in the map; in this case it is updated setting the jointName position to the given jointName. If the Action action was not present before, it is inserted in the trigMap.

Parameters
trigMap[out] The map of ActionTrig to be updated
actionThe action involved in the updating
jointNamethe name of the joint of the action that must be set
trigValuethe value of the position of the joint
Returns
TRUE if action was not present before in the map and it is inserted now; FALSE if the action was already present and only the jointName value is updated to trigValue

Definition at line 667 of file FindActions.cpp.

668  {
669 
670  auto itMap = trigMap.find ( action.getFingerInvolved() );
671  if ( itMap == trigMap.end() ) {
672  //still no action for this finger in the map
673 
674  JointPos jp;
675  for (auto it : parserMoveIt->getActiveJointModels()){
676  std::vector <double> jPos (it->getVariableCount(), DEFAULT_JOINT_POS);
677  jp.insert ( std::make_pair ( it->getName(), jPos ));
678  }
679 
680  //HACK at(0) because 1dof joint
681  jp.at ( jointName ).at(0) = trigValue;
682 
683  action.setJointPos(jp);
684  trigMap.insert ( std::make_pair ( action.getFingerInvolved(), action ) );
685 
686  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  JointPos jp = itMap->second.getJointPos();
692  //HACK at(0) because 1dof joint
693  jp.at (jointName).at(0) = trigValue;
694  itMap->second.setJointPos(jp);
695 
696  return false;
697  }
698 
699 }
void setJointPos(JointPos)
Definition: ActionTrig.cpp:43
std::map< std::string, std::vector< double > > JointPos
The map to describe the position of all actuated joints.
Definition: Action.h:40
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
std::string getFingerInvolved() const
Specific method of trig to simply return a string instead of the full vector fingersInvolved that in ...
Definition: ActionTrig.cpp:31
#define DEFAULT_JOINT_POS
Definition: FindActions.h:21
void ROSEE::FindActions::removeBoundsOfNotCollidingTips ( const std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > *  mapOfLoosePinches,
robot_model::RobotModelPtr  kinematic_model_noBound 
)
private

Support function to remove the joint limits from the model.

This is done when looking for Loose Pinches.

Parameters
mapOfLoosePinches[in] pointer to the map of loose pinches
kinematic_model_noBoundthe pointer to the robot model

C++ Question: WHY if I use directly parser....at() in the for the string joint is corrupted?

Definition at line 363 of file FindActions.cpp.

365  {
366 
367  for (auto mapEl : *mapOfLoosePinches ) {
368 
369  //for each joint of first tip...
370  auto tips = getFingertipsPair(mapEl.first);
372  auto joints = parserMoveIt->getJointsOfFingertipMap().at (tips.first);
373  for ( std::string joint : joints ) {
374  auto jointModel = kinematic_model_noBound ->getJointModel(joint);
375 
376  auto type = jointModel->getType() ;
377  if (type == moveit::core::JointModel::REVOLUTE ) {
378  //at(0) because we are sure to have 1 dof being revolute
379  auto bound = jointModel->getVariableBounds().at(0);
380  bound.max_position_ = EIGEN_PI;
381  bound.min_position_ = -EIGEN_PI;
382  //at(0) because we are sure to have 1 dof being revolute
383  jointModel->setVariableBounds ( jointModel->getVariableNames().at(0), bound );
384 
385  } 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  << "but I am not sure it is enough to make the tips colliding to find the loose pinches " <<
389  std::endl;
390  auto bound = jointModel->getVariableBounds().at(0);
391  bound.max_position_ *= 2;
392  bound.min_position_ *= 2;
393  //at(0) because we are sure to have 1 dof being prismatic
394  jointModel->setVariableBounds ( jointModel->getVariableNames().at(0), bound );
395 
396  } else {
397 
398  std::cout << "[FINDACTIONS::" << __func__ << "] Why are you using a type "
399  << kinematic_model_noBound ->getJointModel(joint)->getType()
400  << " joint? Code not ready to temporarily delete the multiple dof bounds"
401  << " in the job done to find the loose pinches " << std::endl << std::endl;
402 
403  continue;
404  }
405  }
406 
407  //for each joint of second tip...
408  auto joints2 = parserMoveIt->getJointsOfFingertipMap().at (tips.second);
409  for ( auto joint : joints2 ) {
410 
411  auto jointModel = kinematic_model_noBound ->getJointModel(joint);
412  auto type = jointModel->getType() ;
413  if (type == moveit::core::JointModel::REVOLUTE ) {
414  //at(0) because we are sure to have 1 dof being revolute
415  auto bound = jointModel->getVariableBounds().at(0);
416  bound.max_position_ = EIGEN_PI;
417  bound.min_position_ = -EIGEN_PI;
418  //at(0) because we are sure to have 1 dof being revolute
419  jointModel->setVariableBounds ( jointModel->getVariableNames().at(0), bound );
420 
421  } 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  << "but I am not sure it is enough to make the tips colliding to find the loose pinches " << std::endl;
425  auto bound = jointModel->getVariableBounds().at(0);
426  bound.max_position_ *= 2;
427  bound.min_position_ *= 2;
428  //at(0) because we are sure to have 1 dof being revolute
429  jointModel->setVariableBounds ( jointModel->getVariableNames().at(0), bound );
430 
431  } else {
432 
433  std::cout << "[FINDACTIONS::" << __func__ << "] Why are you using a type "
434  << kinematic_model_noBound ->getJointModel(joint)->getType()
435  << " joint? Code not ready to temporarily delete the multiple dof bounds"
436  << " in the working done to find the loose pinches " << std::endl << std::endl;
437 
438  continue;
439  }
440  }
441  }
442 }
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
std::pair< std::string, std::string > getFingertipsPair(std::pair< std::string, std::string > fingersPair) const
Given the fingersPair, this function return the pair of their fingers, in lexicographical order...
ROSEE::JointsInvolvedCount ROSEE::FindActions::setOnlyDependentJoints ( std::pair< std::string, std::string >  tipsNames,
JointPos jPos 
)
private

Given the contact, we want to know the state of the joint to replicate it.

But we want to know only the state of the joints that effectively act on the contact, that are the ones which moves one of the two tips (or both). So the other joints are put to the DEFAULT_JOINT_POS value

Returns
JointsInvolvedCount, the map where each element is relative at one joint (joint name is the key). The value is the number of times that joint is used, for primitive actions can be only 0 or 1

other way around, second is better? std::vector <std::string> jointOfTips1 = jointsOfFingertipMap.at(tipsNames.first); std::vector <std::string> jointOfTips2 = jointsOfFingertipMap.at(tipsNames.second);

if the joint is not linked with neither of the two colliding tips... if ( std::find( jointOfTips1.begin(), jointOfTips1.end(), jp.first) == jointOfTips1.end() && std::find( jointOfTips2.begin(), jointOfTips2.end(), jp.first) == jointOfTips2.end() ) {

std::fill ( jp.second.begin(), jp.second.end(), DEFAULT_JOINT_POS);

IF USE THIS JOINTINVOLVEDCOUNT REMEMBER }

Definition at line 746 of file FindActions.cpp.

747  {
748 
749  JointsInvolvedCount jointsInvolvedCount;
750 
751  for (auto &jp : *jPos) { //for each among ALL joints
752 
753  jointsInvolvedCount.insert ( std::make_pair (jp.first, 1) );
754 
769  //the tips of the joint
770  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  if (std::find (tips.begin(), tips.end(), tipsNames.first) == tips.end() &&
774  std::find (tips.begin(), tips.end(), tipsNames.second) == tips.end() ) {
775  // not dependant, set to default the position
776  std::fill ( jp.second.begin(), jp.second.end(), DEFAULT_JOINT_POS);
777  jointsInvolvedCount.at ( jp.first ) = 0;
778  }
779  }
780 
781  return jointsInvolvedCount;
782 }
std::map< std::string, unsigned int > JointsInvolvedCount
The map to describe, how many times a joint is set by the action.
Definition: Action.h:63
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
#define DEFAULT_JOINT_POS
Definition: FindActions.h:21
ROSEE::JointsInvolvedCount ROSEE::FindActions::setOnlyDependentJoints ( std::set< std::string >  tipsNames,
JointPos jPos 
)
private

Set to default pos the joints that do not move any of the tip included in the set.

Parameters
tipsNames.Used by findMultiplePinch function
tipsNamesthe tips involved
jPospointer to the map JointPos with value to be setted if necessary
Returns
JointsInvolvedCount map, where value are 0 or 1 according to the usage of joint

Definition at line 785 of file FindActions.cpp.

786  {
787 
788  JointsInvolvedCount jointsInvolvedCount;
789 
790  for (auto &jp : *jPos) { //for each among ALL joints
791 
792  jointsInvolvedCount.insert ( std::make_pair (jp.first, 0) );
793 
794  //the tips of the joint
795  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  for ( auto fingInv : tipsNames ) {
802  if (std::find (tips.begin(), tips.end(), fingInv) != tips.end()) {
803  jointsInvolvedCount.at ( jp.first ) = 1 ;
804  break;
805  }
806  }
807 
808  if (jointsInvolvedCount.at ( jp.first ) == 0 ) {
809  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  return jointsInvolvedCount;
815 }
std::map< std::string, unsigned int > JointsInvolvedCount
The map to describe, how many times a joint is set by the action.
Definition: Action.h:63
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
#define DEFAULT_JOINT_POS
Definition: FindActions.h:21
void ROSEE::FindActions::setToDefaultPositionPassiveJoints ( moveit::core::RobotState *  kinematic_state)
private

set to DEFAULT_JOINT_POS all the passive joints (defined so in the urdf file).

this is necessary because moveit setToRandomPositions modify the position of passive joints, we do not want that

Definition at line 817 of file FindActions.cpp.

817  {
818 
819  const double pos = DEFAULT_JOINT_POS; //idk why but setJointPositions want a pointer as 2nd arg
820  for (auto joint : parserMoveIt->getPassiveJointNames()){
821  kinematic_state->setJointPositions(joint, &pos);
822  }
823 
824 }
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
#define DEFAULT_JOINT_POS
Definition: FindActions.h:21
void ROSEE::FindActions::setToRandomPositions ( robot_state::RobotState *  kinematic_state)
private

This function set the random position of joint considering:

  • Non linear mimic joint relationship, if present
  • Passive joints, which default position will be assured
  • Positional limit of also mimic joint will be enforced

These three things are not present in the moveit setToRandomPositions. So we use the moveit one but then we change a bit the things.

Definition at line 826 of file FindActions.cpp.

826  {
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  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  if (mimicNLRelMap.size() > 0 ) { //necessary if we have the for immediately after? faster?
840 
841  for (auto el : mimicNLRelMap) {
842 
843  double mimPos = 0;
844 
845  try {
846  //HACK single dof joint
847  double fatherPos = kinematic_state->getJointPositions(el.second.first)[0];
848 
849  mu::Parser p;
850  //we assume that there is always a x in the expression
851  p.DefineVar("x", &fatherPos);
852  p.SetExpr(el.second.second);
853  mimPos = p.Eval();
854  }
855 
856  catch (mu::Parser::exception_type &e)
857  {
858  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  << el.second.second << "'" << std::endl;
863 
864  exit(-1); //TODO is it good to use exit?
865 
866  }
867 
868  //HACK single dof joint
869  std::vector<double> one_dof ;
870  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  kinematic_state->getJointModel(el.first)->enforcePositionBounds(one_dof.data());
875  kinematic_state->setJointPositions(el.first, one_dof);
876 
877 
878  }
879 
880 
881  }
882 
883  setToDefaultPositionPassiveJoints(kinematic_state);
884 
885 }
std::map< std::string, std::pair< std::string, std::string > > mimicNLRelMap
Definition: FindActions.h:87
void setToDefaultPositionPassiveJoints(moveit::core::RobotState *kinematic_state)
set to DEFAULT_JOINT_POS all the passive joints (defined so in the urdf file).
std::map< std::string, ROSEE::ActionTrig > ROSEE::FindActions::tipFlex ( )
private

We start from each tip.

Given a tip, we look for all the joints that move this tip. If it has 2 or more joints that move exclusively that tip ( we count this number with ParserMoveIt::getNExclusiveJointsOfTip ), we say that a tipFlex is possible. If not, we can't move the tip independently from the rest of the finger, so we have a trig action (if ParserMoveIt::getNExclusiveJointsOfTip returns 1 ) or nothing (ParserMoveIt::getNExclusiveJointsOfTip returns 0). If ParserMoveIt::getNExclusiveJointsOfTip return >= 2, starting from the tip, we explore the parents joints, until we found the first actuated joint. This one will be theInterestingJoint which pose we must set. All the other joints (actuated) will have the default position (if no strange errors).

Returns
std::map <std::string, ROSEE::ActionTrig> the map witch key the tip/finger and as value its ActionTipFlex

Definition at line 605 of file FindActions.cpp.

605  {
606 
607  std::map <std::string, ROSEE::ActionTrig> tipFlexMap;
608 
609  for (auto tipName : parserMoveIt->getFingertipNames() ) {
610 
611  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  continue;
614  }
615 
616  std::string theInterestingJoint = parserMoveIt->getFirstActuatedParentJoint ( tipName, false );
617  double tipFlexMax = parserMoveIt->getBiggerBoundFromZero ( theInterestingJoint ).at(0) ;
618 
619 
620  ActionTrig action ("tipFlex", ActionPrimitive::Type::TipFlex);
621  action.setFingerInvolved (parserMoveIt->getFingerOfFingertip(tipName)) ;
622 
623  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  std::cout << "[FATAL ERROR FINDACTIONS::" << __func__ << "]: Inserting in tipFlexMap a tip already present??" << std::endl
628  << "I am returning a not completely filled map" << std::endl;
629  return tipFlexMap;
630  }
631  }
632 
633  return tipFlexMap;
634 }
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
bool insertJointPosForTrigInMap(std::map< std::string, ActionTrig > &trigMap, ROSEE::ActionTrig action, std::string jointName, double trigValue)
Insert/update an ActionTrig in the trigMap.
std::map< std::string, ROSEE::ActionTrig > ROSEE::FindActions::trig ( )
private

trig is the action of closing a SINGLE finger towards the palm.

The position is the bound which is farther from 0 (considered as default pos). All hands have more range of motion in the flexion respect to extension (as human finger). NOT valid for other motion, like finger spread or thumb addition/abduction.

Note
If a joint is continuos, it is excluded from the trig action. (because I cant think about a continuos joint that is useful for a trig action, but can be present in theory)
Returns
std::map <std::string, ROSEE::ActionTrig> the map witch key the tip/finger and as value its ActionTrig

Go in the max range

Definition at line 577 of file FindActions.cpp.

577  {
578 
579  std::map <std::string, ActionTrig> trigMap;
580 
581  for (auto mapEl : parserMoveIt->getFingertipsOfJointMap()) {
582 
583  if (mapEl.second.size() != 1) { //the joint must move ONLY a fingertip
584  continue;
585  }
586 
587  if ( parserMoveIt->checkIfContinuosJoint(mapEl.first) == true ) {
588  continue; //we dont want to use a continuos joint for the trig
589  }
590 
592  double trigMax = parserMoveIt->getBiggerBoundFromZero(mapEl.first).at(0) ;
593 
594  ActionTrig action ("trig", ActionPrimitive::Type::Trig);
595  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  insertJointPosForTrigInMap(trigMap, action, mapEl.first, trigMax);
599  }
600 
601  return trigMap;
602 }
std::shared_ptr< ROSEE::ParserMoveIt > parserMoveIt
Definition: FindActions.h:84
bool insertJointPosForTrigInMap(std::map< std::string, ActionTrig > &trigMap, ROSEE::ActionTrig action, std::string jointName, double trigValue)
Insert/update an ActionTrig in the trigMap.

Member Data Documentation

std::map<std::string, std::pair<std::string, std::string> > ROSEE::FindActions::mimicNLRelMap
private

Definition at line 87 of file FindActions.h.

std::shared_ptr< ROSEE::ParserMoveIt > ROSEE::FindActions::parserMoveIt
private

Definition at line 84 of file FindActions.h.


The documentation for this class was generated from the following files: