Line data Source code
1 : #ifndef __ROSEE_FIND_ACTIONS_
2 : #define __ROSEE_FIND_ACTIONS_
3 :
4 : #include <moveit/planning_scene/planning_scene.h>
5 :
6 : #include <ros_end_effector/YamlWorker.h>
7 : #include <ros_end_effector/ParserMoveIt.h>
8 : #include <ros_end_effector/GraspingActions/Action.h>
9 : #include <ros_end_effector/GraspingActions/ActionPinchTight.h>
10 : #include <ros_end_effector/GraspingActions/ActionPinchLoose.h>
11 : #include <ros_end_effector/GraspingActions/ActionTrig.h>
12 : #include <ros_end_effector/GraspingActions/ActionSingleJointMultipleTips.h>
13 : #include <ros_end_effector/GraspingActions/ActionMultiplePinchTight.h>
14 :
15 : #include <muParser.h>
16 :
17 :
18 : #define N_EXP_COLLISION 5000 //5000 is ok
19 : #define N_EXP_DISTANCES 5000 //? is ok
20 : #define N_EXP_COLLISION_MULTPINCH 3000
21 : #define DEFAULT_JOINT_POS 0.0
22 :
23 : namespace ROSEE
24 : {
25 :
26 : /**
27 : * @brief Class to check which fingertips collide (for the pinch action at the moment)
28 : *
29 : * @warning there is a problem with collisions: with the schunk hand, if we move only the middle (base phalange)
30 : * toward the hand, a collision between index tip, middle tip and ring tip is detected. Easy reproducible with the
31 : * moveit assistant, in the set pose section (it find a collision when visually is not present, when we move the
32 : * middle). There are some caotic printing in bugmoveit branch, to replicate the problem also with this code.
33 : * I dont know if it is a problem of schunk model, moveit, or both.
34 : *
35 : */
36 144 : class FindActions
37 : {
38 : public:
39 : FindActions ( std::shared_ptr < ROSEE::ParserMoveIt > parserMoveit ) ;
40 :
41 : /**
42 : * @brief Function to look for pinches, both Tight and Loose ones. It fill the maps (returned as pair), but also
43 : * print the infos in two yaml files (one for tight, one for loose) using a \ref YamlWorker
44 : * @param path2saveYaml the path where to create/overwrite the yaml files.
45 : * @return a pair of maps. The first is the map of \ref ActionPinchTight, the second the map of \ref ActionPinchLoose
46 : */
47 : std::pair < std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchTight >,
48 : std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose > >
49 : findPinch ( std::string path2saveYaml );
50 :
51 : /**
52 : * @brief Finder for MultiplePinch (a pinch done with more than 2 finger). This function
53 : * return the found multpinch primitive but it also print this result in a yaml file.
54 : * See \ref checkCollisionsForMultiplePinch doc for more info.
55 : *
56 : * @param nFinger (2 < nFinger <= max_finger). The type of the multiple pinch that we want.
57 : * The name of the returned action will be based on this param :
58 : * "MultiplePinchTight-(nFinger)"
59 : * @param strict true to look only for "strict" multiple pinch, i.e. where all tips collide
60 : * with each other (and do not collide in "line")
61 : * See \ref checkCollisionsForMultiplePinch doc for more info.
62 : * @param path2saveYaml the path where to create/overwrite the yaml files.
63 : * @return map of founded \ref ActionMultiplePinchTight,
64 : */
65 : std::map < std::set <std::string>, ROSEE::ActionMultiplePinchTight > findMultiplePinch (
66 : unsigned int nFinger, std::string path2saveYaml, bool strict = true );
67 :
68 : /**
69 : * @brief Function to look for trigs (trig, tipFlex and fingFlex). The type of trig to be looked for is choosen thanks
70 : * to the argument \p actionType. This function return the map of wanted trig but also print info about that on a yaml file
71 : * using a \ref YamlWorker
72 : *
73 : * @param actionType the type of trig to look for (Trig, TipFlex, FingFlex)
74 : * @param path2saveYaml the path where to create/overwrite the yaml file.
75 : * @return the map of chosen type of trig filled with infos about the possible actions.
76 : */
77 : std::map <std::string, ROSEE::ActionTrig> findTrig ( ROSEE::ActionPrimitive::Type actionType,
78 : std::string path2saveYaml );
79 :
80 : std::map < std::string, ROSEE::ActionSingleJointMultipleTips> findSingleJointMultipleTips ( unsigned int nFinger, std::string path2saveYaml );
81 :
82 : private:
83 :
84 : std::shared_ptr < ROSEE::ParserMoveIt > parserMoveIt;
85 :
86 : //lets store this, we access at each setRandomPos
87 : std::map<std::string, std::pair<std::string, std::string>> mimicNLRelMap;
88 :
89 : /**
90 : * @brief principal function which check for collisions with moveit functions when looking for tight pinches
91 : * @return the map of ActionPinchTight
92 : *
93 : */
94 : std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchTight > checkCollisions();
95 :
96 : /**
97 : * @brief Principal function which fill the \p mapOfLoosePinches basing on minumun distance between tips
98 : * @param mapOfLoosePinches [out] pointer to the mapOfLoosePinches to be filled
99 : */
100 : void checkDistances (std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose >* mapOfLoosePinches);
101 :
102 : /**
103 : * @brief Function similar to \ref checkCollisions but used for Loose Pinches.
104 : * First, we temporarily remove bounds of joints linked to the non-colliding tips (with \ref removeBoundsOfNotCollidingTips),
105 : * and we check for collision with this function.
106 : * If some collision are found, this means that tips movements make them go towards each other, (also with the bounds) but
107 : * the joint limits do not permit them to touch. This is a loose pinch.
108 : * 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
109 : * pinch.
110 : * We also create new kinematic_model object so we dont modify the original kinematic_model, and we can change the joint limits
111 : * of the new copy safely
112 : *
113 : * @param mapOfLoosePinches [out] map of loose pinches that will be filled with info about these particular actions
114 : */
115 : void checkWhichTipsCollideWithoutBounds (
116 : std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose >* mapOfLoosePinches);
117 :
118 : /**
119 : * @brief support function for \ref findMultiplePinch (a pinch done with more than 2 finger).
120 : * This is done similarly to normal pinch, but there are more checks to see
121 : * if the collision is among more than only two fingertips.
122 : *
123 : * Moveit, when looking for collisions, return only pairs of links that collide.
124 : * So we need to handle all the found pairs and "put them togheter" in someway.
125 : * We need at least \param nFinger - 1 collision: eg. for triPinch -> 2collision,
126 : * for 4finger pinch -> 3 collision. But, with only this check, we can also find
127 : * a configuration whith two distint normal pinch. To solve, we also check if the number of
128 : * tips that collide in this configuration is exaclty \param nFinger ,
129 : * eg with 2 collision we can have 4 finger colliding because there are two
130 : * normal distinct pinch and not a 3-pinch... so we exlude these collisions.
131 : * The \param strict can solves another problem. If it is true (default) we take only
132 : * the multiple pinch where each finger collide with all the other finger involved in
133 : * the pinch. If it is false, we can find also pinch where the tips are "in line" :
134 : * finger_2 collide with finger_1 and finger_3, but finger_1 and 3 do not collide.
135 : * With strict we will find less groups of fingers that collide, but, in someway, they
136 : * collide "better".
137 : *
138 : * @param nFinger (2 < nFinger <= max_finger). The type of the multiple pinch that we want.
139 : * The name of the returned action will be based on this param :
140 : * "MultiplePinchTight-(nFinger)"
141 : * @param strict true to look only for "strict" multiple pinch. Look this funcion description
142 : * @return A map with as keys set of size nFinger, and as value an
143 : * \ref ActionMultiplePinchTight object
144 : */
145 : std::map <std::set<std::string>, ROSEE::ActionMultiplePinchTight>
146 : checkCollisionsForMultiplePinch(unsigned int nFinger, bool strict);
147 :
148 : /**
149 : * @brief Support function to remove the joint limits from the model. This is done when looking for Loose Pinches.
150 : * @param mapOfLoosePinches [in] pointer to the map of loose pinches
151 : * @param kinematic_model_noBound the pointer to the robot model
152 : */
153 : void removeBoundsOfNotCollidingTips ( const std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose >* mapOfLoosePinches,
154 : robot_model::RobotModelPtr kinematic_model_noBound);
155 :
156 : /**
157 : * @brief function to "initialize" the map of ActionPinchLoose \p mapOfLoosePinches.
158 : * It is done adding all the tips pairs and then removing
159 : * the pairs that are present in the map of ActionPinchTight \p mapOfPinches. Note that the values of the map, the \ref ActionPinchLoose
160 : * are action with only tips name (so no info right now).
161 : *
162 : * @param mapOfLoosePinches [out] Pointer to the map of \ref ActionPinchLoose to be initialized
163 : * @param mapOfPinches [in] pointer to the map of \ref ActionPinchTight, already filled before, that is used to erase the get the tips that collide
164 : * and to remove them from the \p mapOfLoosePinches
165 : */
166 : void fillNotCollidingTips ( std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose >* mapOfLoosePinches,
167 : const std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchTight >* mapOfPinches );
168 :
169 : /**
170 : * @brief this function take the two tight and loose maps and change the keys from fingertips
171 : * names to their finger names.
172 : *
173 : * @param mapOfLoosePinches [out] Pointer to the map of \ref ActionPinchLoose
174 : * @param mapOfPinches [out] pointer to the map of \ref ActionPinchTight
175 : *
176 : * @warning The order in the pair is lexicographical so the first finger in the
177 : * pair can refer to the second tip in the old key pair
178 : */
179 : void changeFingertipsToFingerNames (
180 : std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchTight >* mapOfPinches,
181 : std::map < std::pair <std::string, std::string> , ROSEE::ActionPinchLoose >* mapOfLoosePinches) ;
182 :
183 :
184 : /**
185 : * @brief trig is the action of closing a SINGLE finger towards the palm.
186 : * The position is the bound which is farther from 0 (considered as default pos). All hands have more range of motion
187 : * in the flexion respect to extension (as human finger). NOT valid for other motion, like finger spread or
188 : * thumb addition/abduction.
189 : * @note If a joint is continuos, it is excluded from the trig action. (because I cant think about a continuos
190 : * joint that is useful for a trig action, but can be present in theory)
191 : * @return std::map <std::string, ROSEE::ActionTrig> the map witch key the tip/finger and as value its ActionTrig
192 : */
193 : std::map <std::string, ActionTrig> trig();
194 :
195 : /**
196 : * @brief We start from each tip. Given a tip, we look for all the joints that move this tip. If it has 2
197 : * or more joints that move exclusively that tip ( we count this number with \ref ParserMoveIt::getNExclusiveJointsOfTip ),
198 : * we say that a tipFlex is possible. If not, we can't move the tip independently from the rest of the
199 : * finger, so we have a trig action (if \ref ParserMoveIt::getNExclusiveJointsOfTip returns 1 ) or
200 : * nothing (\ref ParserMoveIt::getNExclusiveJointsOfTip returns 0).
201 : * If \ref ParserMoveIt::getNExclusiveJointsOfTip return >= 2, starting from the tip, we explore the parents joints,
202 : * until we found the first actuated joint. This one will be \ref theInterestingJoint which pose we must
203 : * set. All the other joints (actuated) will have the default position (if no strange errors).
204 : * @return std::map <std::string, ROSEE::ActionTrig> the map witch key the tip/finger and as value its ActionTipFlex
205 : */
206 : std::map <std::string, ROSEE::ActionTrig> tipFlex();
207 :
208 : /**
209 : * @brief We start from each tip. Given a tip, we check if \ref ParserMoveIt::getNExclusiveJointsOfTip >= 2
210 : * (see \ref tipFlex function).
211 : * If so, we continue exploring the chain from the tip going up through the parents. We stop when a parent has
212 : * more than 1 joint as child. This means that the last link is the first of the finger. Meanwhile we have
213 : * stored the actuated, not continuos joint (in \ref joint) that we were founding along the chain. The last stored
214 : * is exaclty \ref theInterestingJoint, which pose of we must set.
215 : * All the other joints (actuated) will have the default position (if no strange errors).
216 : */
217 : std::map <std::string, ROSEE::ActionTrig> fingFlex();
218 :
219 :
220 : /**
221 : * @brief Insert/update an ActionTrig in the \p trigMap. This is done setting the \p jointName position to the given
222 : * \p jointName. So, for a single action this function can be executed more than once (because more joint can be set).
223 : * The Action \p action can be already present in the map; in this case it is updated setting the \p jointName position to the given \p jointName.
224 : * If the Action \p action was not present before, it is inserted in the \p trigMap.
225 : *
226 : * @param trigMap [out] The map of ActionTrig to be updated
227 : * @param action The action involved in the updating
228 : * @param jointName the name of the joint of the action that must be set
229 : * @param trigValue the value of the position of the joint
230 : * @return TRUE if \p action was not present before in the map and it is inserted now;
231 : * FALSE if the action was already present and only the \p jointName value is updated to \p trigValue
232 : */
233 : bool insertJointPosForTrigInMap ( std::map <std::string, ActionTrig>& trigMap,
234 : ROSEE::ActionTrig action, std::string jointName, double trigValue);
235 :
236 :
237 :
238 : /**
239 : * @brief Given the contact, we want to know the state of the joint to replicate it. But we want to know
240 : * 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
241 : * @return JointsInvolvedCount, the map where each element is relative at one joint (joint name is the key).
242 : * The value is the number of times that joint is used, for primitive actions can be only 0 or 1
243 : */
244 : ROSEE::JointsInvolvedCount setOnlyDependentJoints(std::pair < std::string, std::string > tipsNames, JointPos *jPos);
245 :
246 :
247 : /**
248 : * @brief Set to default pos the joints that do not move any of the tip included in the
249 : * set \param tipsNames. Used by \ref findMultiplePinch function
250 : * @param tipsNames the tips involved
251 : * @param jPos pointer to the map \ref JointPos with value to be setted if necessary
252 : * @return JointsInvolvedCount map, where value are 0 or 1 according to the usage of joint
253 : */
254 : ROSEE::JointsInvolvedCount setOnlyDependentJoints( std::set< std::string > tipsNames,
255 : JointPos *jPos);
256 :
257 : /**
258 : * @brief Utility function to take the actuated joint positions from a \p kinematic_state and returns the same info as a \ref JointPos map
259 : * @param kinematic_state [in] pointer to the robot_state class
260 : * @return JointPos the map with the joint positions info
261 : */
262 : JointPos getConvertedJointPos(const robot_state::RobotState* kinematic_state);
263 :
264 :
265 : /**
266 : * @brief set to \ref DEFAULT_JOINT_POS all the passive joints (defined so in
267 : * the urdf file). this is necessary because moveit setToRandomPositions modify the position of passive joints,
268 : * we do not want that
269 : */
270 : void setToDefaultPositionPassiveJoints(moveit::core::RobotState * kinematic_state);
271 :
272 : /**
273 : * @brief Giving as argument a pair of fingertips, this function return a pair of fingers that
274 : * are the fingers which the two tips belong to.
275 : *
276 : * @return a pair of string containing the fingers which the passed tips belong to
277 : */
278 : std::pair < std::string, std::string > getFingersPair (std::pair <std::string, std::string> tipsPair) const;
279 :
280 : /**
281 : * @brief Function used when looking for multiple pinches. It returns the set containing
282 : * the fingers of the passed fingertips.
283 : * @param tipsSet the set of fingertips names
284 : * @return the set of fingers. Empty set if the some tips in the \ref tipsSet are in
285 : * the same finger (that is an error)
286 : */
287 : std::set <std::string> getFingersSet (std::set <std::string> tipsSet) const;
288 :
289 : /**
290 : * @brief Given the \ref fingersPair, this function return the pair of their fingers, in
291 : * lexicographical order
292 : *
293 : * @return a pair of string containing the fingers which the passed tips belong to
294 : *
295 : */
296 : std::pair < std::string, std::string > getFingertipsPair (std::pair <std::string, std::string> fingersPair) const;
297 :
298 : /**
299 : * @brief This function set the random position of joint considering:
300 : *
301 : * - Non linear mimic joint relationship, if present
302 : * - Passive joints, which default position will be assured
303 : * - Positional limit of also mimic joint will be enforced
304 : *
305 : * These three things are not present in the moveit setToRandomPositions. So we use the moveit one but then
306 : * we change a bit the things.
307 : */
308 : void setToRandomPositions(robot_state::RobotState* kinematic_state);
309 :
310 :
311 : };
312 :
313 : }
314 :
315 :
316 : #endif //__ROSEE_FIND_ACTIONS_
|
