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_ACTIONPINCHLOOSE_H
      20             : #define __ROSEE_ACTIONPINCHLOOSE_H
      21             : 
      22             : #include <ros_end_effector/GraspingActions/ActionPrimitive.h>
      23             : #include <ros_end_effector/GraspingActions/ActionPinchGeneric.h>
      24             : #include <moveit/planning_scene/planning_scene.h>
      25             : #include <yaml-cpp/yaml.h>
      26             : #include <iostream>
      27             : 
      28             : namespace ROSEE {
      29             : 
      30             : /**
      31             :  * @brief The action of pinch with two tips. The two tips must not collide ever 
      32             :  * (otherwise we have a TightPinch). They only need to move towards each other moving the relative joints.
      33             :  * This PinchLoose is created because also if the tips do not collide (i.e. there is not a \ref ActionPinchTight)
      34             :  * we can have anyway a pinch at least to take object of a certain minimum size.
      35             :  * All the non involved fingers are set in the default state.
      36             :  * A pinchLoose is defined by:
      37             :  *  - 2 tips ( that are inside \ref fingersInvolved ), so \ref nFingersInvolved == 2 ( members of base class \ref Action )
      38             :  *  - JointStates position: where the collision happens (inside \ref actionStates )
      39             :  *  - Optional info (inside \ref actionStates ): the minimum distance found between the two tips.
      40             :  *    The distance is used to order, the actions in the \ref actionStates set (make sense if \ref maxStoredActionStates > 1 ): 
      41             :  *    the less the distance is, the more we say the pinchLoose is good
      42             :  */
      43        1721 : class ActionPinchLoose : public ActionPinchGeneric
      44             : {
      45             :     
      46             : public:
      47             :     
      48             :     typedef std::map < std::pair<std::string, std::string>, ActionPinchLoose > Map;
      49             :     
      50             :     /**
      51             :      * @brief A pair to "link" the JointPos with the optional info 'distance' 
      52             :      */
      53             :     typedef std::pair <JointPos, double> StateWithDistance; 
      54             :     
      55             :     ActionPinchLoose();
      56             :     ActionPinchLoose ( unsigned int maxStoredActionStates );
      57             :     ActionPinchLoose ( std::string tip1, std::string tip2);
      58             :     ActionPinchLoose ( std::pair <std::string, std::string>, JointPos, double distance );
      59             :     
      60             :     JointPos getJointPos () const override;
      61             :     JointPos getJointPos (unsigned int index) const;
      62             :     
      63             :     std::vector < ROSEE::JointPos > getAllJointPos () const override;    
      64             :     
      65             :     /** 
      66             :      * @brief Specific get for this action to return the state with distance info 
      67             :      * @return The vector (of size \ref maxStoredActionStates) containing all the StateWithDistance objects
      68             :      */
      69             :     std::vector < ROSEE::ActionPinchLoose::StateWithDistance > getActionStates() const;
      70             : 
      71             :     
      72             :     /** 
      73             :      * @brief function to insert a single action in the \ref actionStates set of possible action. 
      74             :      * If the action is not so good (based on distance) the action is not inserted and 
      75             :      * the function return false 
      76             :      * @param JointPos The joints position
      77             :      * @param distance the distance from the two tips.
      78             :      * @return TRUE if the action is good and is inserted in the setActionStates
      79             :      *         FALSE if the action given as param was not good as the others in the \ref actionStates
      80             :      *           and the set was already full (\ref maxStoredActionStates )
      81             :      */
      82             :     bool insertActionState (JointPos, double distance);
      83             : 
      84             :     /* For the pinch, we override these function to print, emit and parse the optional info Contact,
      85             :      which is specific of the pinch */
      86             :     void print () const override;
      87             :     void emitYaml ( YAML::Emitter&) const override;
      88             :     bool fillFromYaml( YAML::const_iterator yamlIt ) override;
      89             :     
      90             : private:
      91             :     
      92             :     /**
      93             :      * @brief struct to put in order the \ref actionStates set. The first elements are the ones with lesser distance 
      94             :      */
      95             :     struct distComp {
      96      339891 :         bool operator() (const StateWithDistance& a, const StateWithDistance& b) const
      97      339891 :         {return (std::abs(a.second) < std::abs(b.second) );}
      98             :     };
      99             :         
     100             :     /** 
     101             :      * @brief For each pair, we want a set of action because we want to store (in general) more possible way
     102             :      * to do that action. The PinchLoose among two tips can theoretically be done in infinite ways, we store 
     103             :      * the best ways found (ordering them by the distance between fingertips)
     104             :      */
     105             :     std::set < StateWithDistance, distComp > actionStates;
     106             : 
     107             : };
     108             : 
     109             : }
     110             : 
     111             : #endif // __ROSEE_ACTIONPINCHLOOSE_H