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>

Public Member Functions

 FindActions (std::shared_ptr< ROSEE::ParserMoveIt > parserMoveit)
 
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). This function return the found multpinch primitive but it also print this result in a yaml file. See checkCollisionsForMultiplePinch doc for more info. More...
 
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. 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. More...
 
std::map< std::string, ROSEE::ActionSingleJointMultipleTipsfindSingleJointMultipleTips (unsigned int nFinger, std::string path2saveYaml)
 
std::map< std::string, ROSEE::ActionTrigfindTrig (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. More...
 

Private Member Functions

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::pair< std::string, std::string >, ROSEE::ActionPinchTightcheckCollisions ()
 principal function which check for collisions with moveit functions when looking for tight 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). This is done similarly to normal pinch, but there are more checks to see if the collision is among more than only two fingertips. 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. 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. 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. 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). More...
 
std::map< std::string, ROSEE::ActionTrigfingFlex ()
 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). 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...
 
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. It returns the set containing the fingers of the passed fingertips. 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...
 
bool insertJointPosForTrigInMap (std::map< std::string, ActionTrig > &trigMap, ROSEE::ActionTrig action, std::string jointName, double trigValue)
 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. 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. This is done when looking for Loose Pinches. 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. 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. 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...
 
void setToDefaultPositionPassiveJoints (moveit::core::RobotState *kinematic_state)
 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 More...
 
void setToRandomPositions (robot_state::RobotState *kinematic_state)
 This function set the random position of joint considering: More...
 
std::map< std::string, ROSEE::ActionTrigtipFlex ()
 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). More...
 
std::map< std::string, ActionTrigtrig ()
 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. More...
 

Private Attributes

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

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

◆ FindActions()

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

Definition at line 3 of file FindActions.cpp.

Member Function Documentation

◆ changeFingertipsToFingerNames()

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

◆ checkCollisions()

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.

◆ checkCollisionsForMultiplePinch()

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.

◆ checkDistances()

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.

◆ checkWhichTipsCollideWithoutBounds()

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.

◆ fillNotCollidingTips()

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.

◆ findMultiplePinch()

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.

◆ findPinch()

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.

◆ findSingleJointMultipleTips()

std::map< std::string, ROSEE::ActionSingleJointMultipleTips > ROSEE::FindActions::findSingleJointMultipleTips ( unsigned int  nFinger,
std::string  path2saveYaml 
)

Definition at line 137 of file FindActions.cpp.

◆ findTrig()

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.

◆ fingFlex()

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.

◆ getConvertedJointPos()

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.

◆ getFingersPair()

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.

◆ getFingersSet()

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.

◆ getFingertipsPair()

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.

◆ insertJointPosForTrigInMap()

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.

◆ removeBoundsOfNotCollidingTips()

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.

◆ setOnlyDependentJoints() [1/2]

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.

◆ setOnlyDependentJoints() [2/2]

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.

◆ setToDefaultPositionPassiveJoints()

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.

◆ setToRandomPositions()

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.

◆ tipFlex()

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.

◆ trig()

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.

Member Data Documentation

◆ mimicNLRelMap

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

Definition at line 87 of file FindActions.h.

◆ parserMoveIt

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:


end-effector
Author(s): Luca Muratore , Davide Torielli , Liana Bertoni
autogenerated on Sat Dec 14 2024 03:49:27