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_ACTIONPINCHTIGHT_H
      20             : #define __ROSEE_ACTIONPINCHTIGHT_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 The action of pinch with two tips. The two tips must collide for some hand 
      31             :  * configuration to mark this configuration as a pinchTight. All the non involved fingers are set in the 
      32             :  * default state.
      33             :  * A pinchTight is defined by:
      34             :  *  - 2 tips ( that are inside \ref fingersInvolved ), so \ref nFingersInvolved == 2 ( members of base class \ref ActionPrimitive )
      35             :  *  - JointStates position: where the collision happens (inside \ref actionStates )
      36             :  *  - Optional info (inside \ref actionStates ): the contact of moveit. Now only the member depth is used. 
      37             :  *    It is used to order the actions in the \ref actionStates set, to have a "quality" measure 
      38             :  *    (make sense if \ref maxStoredActionStates > 1 ): the more the depth of compenetration is, the more we say the pinchTight is good
      39             :  */
      40       15216 : class ActionPinchTight : public ActionPinchGeneric
      41             : {
      42             :     
      43             : public:
      44             :     
      45             :     typedef std::map < std::pair<std::string, std::string>, ActionPinchTight > Map;
      46             : 
      47             :     
      48             :     /** @brief A pair to "link" the JointPos with infos about the collision among the two tips */
      49             :     typedef std::pair <JointPos, collision_detection::Contact> StateWithContact; 
      50             :     
      51             :     ActionPinchTight();
      52             :     ActionPinchTight(unsigned int maxStoredActionStates);
      53             :     ActionPinchTight (std::pair <std::string, std::string>, JointPos, collision_detection::Contact );
      54             :     ActionPinchTight (std::string finger1, std::string finger2, JointPos, collision_detection::Contact );
      55             :     
      56             :     JointPos getJointPos () const override;
      57             :     JointPos getJointPos (unsigned int index) const;
      58             :     
      59             :     std::vector < ROSEE::JointPos > getAllJointPos () const override;
      60             :     
      61             :     /** 
      62             :      * @brief Specific get for the ActionPinchTight to return the state with contact info 
      63             :      * @return The vector (of size \ref maxStoredActionStates) containing all the StateWithContact objects
      64             :      */
      65             :     std::vector < ROSEE::ActionPinchTight::StateWithContact > getActionStates() const;
      66             :     
      67             :     /** 
      68             :      * @brief function to insert a single action in the \ref actionStates set of possible action. 
      69             :      * If the action is not so good (based on depth now) the action is not inserted and 
      70             :      * the function return false 
      71             :      * @param JointPos The joints position
      72             :      * @param collision_detection::Contact the contact associated with the action
      73             :      * @return TRUE if the action is good and is inserted in the set \ref actionStates
      74             :      *         FALSE if the action given as param was not good as the others in the set \ref actionStates
      75             :      *           and the set was already full (\ref maxStoredActionStates)
      76             :      */
      77             :     bool insertActionState (JointPos, collision_detection::Contact);
      78             : 
      79             :     /** For the pinch, we override these function to print, emit and parse the optional info Contact,
      80             :      which is specific of the pinch */
      81             :     void print () const override;
      82             :     void emitYaml ( YAML::Emitter& ) const override;
      83             :     bool fillFromYaml( YAML::const_iterator yamlIt ) override;
      84             :     
      85             : private:
      86             :     
      87             :     /** private function to be called by the emitYaml */
      88             :     bool emitYamlForContact ( collision_detection::Contact, YAML::Emitter& ) const;
      89             : 
      90             :     
      91             :     /** 
      92             :      * @brief struct to put in order the actionStates. The first elements are the ones 
      93             :      * with greater depth
      94             :      * @FIX, even if is almost impossible, two different contact with same depth will be considered equal
      95             :      * with this definition of depthComp. Theoretically they are equal only if the joint status are equal 
      96             :      * (of only joints that act for the collision). In fact, we should have the possibility to have two 
      97             :      * contact with the same depth (if joint statuses are different), they will be equally good
      98             :      */
      99             :     struct depthComp {
     100       56858 :         bool operator() (const StateWithContact& a, const StateWithContact& b) const
     101       56858 :         {return (std::abs(a.second.depth) > std::abs(b.second.depth) );}
     102             :     };
     103             :     
     104             :     /** 
     105             :      * @brief For each pair, we want a set of action because we want to store (in general) more possible way
     106             :      * to do that action. The pinch among two tips can theoretically be done in infinite ways, we store 
     107             :      * the best ways found (ordering them by the depth of fingertips compenetration)
     108             :      */
     109             :     std::set < StateWithContact, depthComp > actionStates;
     110             :     
     111             : };
     112             : 
     113             : }
     114             : 
     115             : #endif // __ROSEE_ACTIONPINCHTIGHT_H