Line data Source code
1 : /*
2 : * Copyright (C) 2020 IIT-HHCM
3 : * Author: Davide Torielli
4 : * email: davide.torielli@iit.it
5 : *
6 : * Licensed under the Apache License, Version 2.0 (the "License");
7 : * you may not use this file except in compliance with the License.
8 : * You may obtain a copy of the License at
9 : *
10 : * http://www.apache.org/licenses/LICENSE-2.0
11 : *
12 : * Unless required by applicable law or agreed to in writing, software
13 : * distributed under the License is distributed on an "AS IS" BASIS,
14 : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 : * See the License for the specific language governing permissions and
16 : * limitations under the License.
17 : */
18 :
19 : #ifndef __ROSEE_ACTIONMULTIPLEPINCHTIGHT_H
20 : #define __ROSEE_ACTIONMULTIPLEPINCHTIGHT_H
21 :
22 : #include <ros_end_effector/GraspingActions/ActionPinchGeneric.h>
23 : #include <moveit/planning_scene/planning_scene.h>
24 : #include <yaml-cpp/yaml.h>
25 : #include <iostream>
26 :
27 : namespace ROSEE {
28 :
29 : /**
30 : * @brief Class to describe the action of "pinching" with more than 2 tips (with 2 tips there is the \ref ActionPinchTight and
31 : * \ref ActionPinchLoose
32 : * The number of the finger used is fixed when the object is costructed, and it is stored in the father member \ref nFingersInvolved
33 : *
34 : * A pinchMultipleTight is defined by:
35 : * - X tips ( that are inside \ref fingersInvolved member of \ref Action ), so \ref nFingersInvolved == X ( members of base class \ref ActionPrimitive )
36 : * - JointStates position: where the collision among the tips happens (inside \ref actionStates )
37 : * - Optional info (inside \ref actionStates ): the sum of the depth of compenetration of all the tip pairs that collide.
38 : *
39 : * @note When exploring the model to find this action, there are two possibility: we can look for a position where all the X fingertips
40 : * collide among each other (causing EXACTLY bynomial_coeff(X, 2) collisions pair), or, with a "less strict" condition, a position where
41 : * not all fingertips must collide between each other (causing AT LEAST X-1 collisions).
42 : * The default is the "more strict" condition, because it can find "better looking" mulPinches, even if obviously we find less possible
43 : * way to perform the action. With this, it seems better to order the actionState considering as "best" the position where the tips collide less,
44 : * i.e. where the depthSum is lower. This is the opposite from the normal tight pinch.
45 : * This cause anyway a collision, but not a ugly one where the tips compenetrate too much. This thing can change if we invert the sign in the
46 : * \ref depthComp comparison
47 : */
48 22 : class ActionMultiplePinchTight : public ActionPinchGeneric
49 : {
50 :
51 : public:
52 :
53 : typedef std::map < std::set<std::string>, ActionMultiplePinchTight > Map;
54 :
55 : /** @brief A pair to "link" the JointPos with the depthSum info to order the StateWithDepth in the actionState set*/
56 : typedef std::pair <JointPos, double> StateWithDepth;
57 :
58 : ActionMultiplePinchTight();
59 : ActionMultiplePinchTight(unsigned int maxStoredActionStates);
60 : ActionMultiplePinchTight (std::set <std::string>, JointPos, double depthSum );
61 :
62 : JointPos getJointPos () const override;
63 : JointPos getJointPos (unsigned int index) const;
64 :
65 : std::vector < ROSEE::JointPos > getAllJointPos () const override;
66 :
67 : /**
68 : * @brief Specific get for the ActionMultiplePinchTight to return the state with the paired depthSum
69 : * @return The vector (of size \ref maxStoredActionStates) containing all the StateWithDepth objects
70 : */
71 : std::vector < ROSEE::ActionMultiplePinchTight::StateWithDepth > getActionStates() const;
72 :
73 : /**
74 : * @brief function to insert a single action in the \ref actionStates set of possible action.
75 : * If the action is not so good (depthSum) the action is not inserted and
76 : * the function return false
77 : * @param JointPos The joints position
78 : * @param depthSum the sum of all depth of collisions pairs
79 : * @return TRUE if the action is good and is inserted in the set \ref actionStates
80 : * FALSE if the action given as argument was not good as the others in the set \ref actionStates
81 : * and the set was already full (\ref maxStoredActionStates)
82 : */
83 : bool insertActionState (JointPos, double depthSum);
84 :
85 : void print () const override;
86 : void emitYaml ( YAML::Emitter& ) const override;
87 : bool fillFromYaml( YAML::const_iterator yamlIt ) override;
88 :
89 : private:
90 :
91 : /**
92 : * @brief struct to put in order the actionStates.
93 : * with "<" we put as best the position that has less sumDepth.
94 : */
95 : struct depthComp {
96 47 : bool operator() (const StateWithDepth& a, const StateWithDepth& b) const
97 47 : {return (std::abs(a.second) < std::abs(b.second) );}
98 : };
99 :
100 : /**
101 : * @brief For each multiple pinch possible, we want a set of action because we want to store (in general) more possible way
102 : * to do that action with that X fingers. The pinch among X tips can theoretically be done in infinite ways, so we store
103 : * the best ways found (ordering them with depthSum of fingertips compenetration)
104 : */
105 : std::set < StateWithDepth, depthComp > actionStates;
106 :
107 : };
108 :
109 : }
110 :
111 : #endif // __ROSEE_ACTIONMULTIPLEPINCHTIGHT_H
|