ROSEndEffector
ROS End-Effector package: provides a ROS-based set of standard interfaces to command robotics end-effectors in an agnostic fashion.
ActionPinchLoose.h
Go to the documentation of this file.
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 
24 #include <moveit/planning_scene/planning_scene.h>
25 #include <yaml-cpp/yaml.h>
26 #include <iostream>
27 
28 namespace ROSEE {
29 
44 {
45 
46 public:
47 
48  typedef std::map < std::pair<std::string, std::string>, ActionPinchLoose > Map;
49 
53  typedef std::pair <JointPos, double> StateWithDistance;
54 
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 
69  std::vector < ROSEE::ActionPinchLoose::StateWithDistance > getActionStates() const;
70 
71 
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 
95  struct distComp {
96  bool operator() (const StateWithDistance& a, const StateWithDistance& b) const
97  {return (std::abs(a.second) < std::abs(b.second) );}
98  };
99 
105  std::set < StateWithDistance, distComp > actionStates;
106 
107 };
108 
109 }
110 
111 #endif // __ROSEE_ACTIONPINCHLOOSE_H
std::vector< ROSEE::JointPos > getAllJointPos() const override
Return all the joint position stored.
std::map< std::string, std::vector< double > > JointPos
The map to describe the position of all actuated joints.
Definition: Action.h:40
std::set< StateWithDistance, distComp > actionStates
For each pair, we want a set of action because we want to store (in general) more possible way to do ...
A base virtual class for the PinchTight and PinchLoose classes.
struct to put in order the actionStates set.
The action of pinch with two tips.
JointPos getJointPos() const override
Get the position related to this action.
void print() const override
Overridable functions, if we want to make them more action-specific.
std::pair< JointPos, double > StateWithDistance
A pair to "link" the JointPos with the optional info &#39;distance&#39;.
std::map< std::pair< std::string, std::string >, ActionPinchLoose > Map
const unsigned int maxStoredActionStates
bool operator()(const StateWithDistance &a, const StateWithDistance &b) const
bool fillFromYaml(YAML::const_iterator yamlIt) override
function to fill members of the Action with infos taken from yaml files
void emitYaml(YAML::Emitter &) const override
Function to fill the argument passed with info about the action.
std::vector< ROSEE::ActionPinchLoose::StateWithDistance > getActionStates() const
Specific get for this action to return the state with distance info.
bool insertActionState(JointPos, double distance)
function to insert a single action in the actionStates set of possible action.