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 <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             : 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          17 :         bool operator() (const StateWithDepth& a, const StateWithDepth& b) const
      97          17 :         {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