UniversalFindActions.cpp
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  * http://www.apache.org/licenses/LICENSE-2.0
10  *
11  * Unless required by applicable law or agreed to in writing, software
12  * distributed under the License is distributed on an "AS IS" BASIS,
13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14  * See the License for the specific language governing permissions and
15  * limitations under the License.
16  */
17 
18 #include <ros/ros.h>
19 //#include <end_effector/UniversalRosEndEffectorExecutor.h>
20 #include <end_effector/FindActions.h>
21 #include <end_effector/GraspingActions/Action.h>
22 #include <end_effector/GraspingActions/ActionComposed.h>
23 #include <end_effector/GraspingActions/ActionTimed.h>
24 #include <end_effector/GraspingActions/ActionGeneric.h>
25 #include <end_effector/ParserMoveIt.h>
26 #include <end_effector/Parser.h> //to take urdf from conf file
27 
28 #include <end_effector/MapActionHandler.h>
29 
30 int main ( int argc, char **argv ) {
31 
32  ros::init ( argc, argv, "FindActions" );
33 
34  ros::NodeHandle nh;
35  ROSEE::Parser parser(nh);
36  parser.init();
37  //Load the ROS Server with urdf and srdf
38  nh.setParam("/robot_description", parser.getUrdfString());
39  nh.setParam("/robot_description_semantic", parser.getSrdfString());
40  ROS_INFO_STREAM("FINDACTIONS: Set urdf and srdf file in the param server");
41 
42  std::shared_ptr <ROSEE::ParserMoveIt> parserMoveIt = std::make_shared <ROSEE::ParserMoveIt> ();
43  if (! parserMoveIt->init ("robot_description") ) {
44  ROS_ERROR_STREAM ("FAILED parserMoveit Init, stopping execution");
45  return -1;
46  }
47  //xml is necessary... but parsermoveit has no access to it, so we must pass it here
48  parserMoveIt->parseNonLinearMimicRelations(parser.getUrdfString());
49 
50 
51  std::string folderForActions = parser.getActionPath();
52 
53  ROSEE::FindActions actionsFinder (parserMoveIt);
54 
55  auto maps = actionsFinder.findPinch(folderForActions + "/primitives/");
56 
57  std::map <std::string, ROSEE::ActionTrig> trigMap = actionsFinder.findTrig (ROSEE::ActionPrimitive::Type::Trig,
58  folderForActions + "/primitives/") ;
59 
60  std::map <std::string, ROSEE::ActionTrig> tipFlexMap = actionsFinder.findTrig (ROSEE::ActionPrimitive::Type::TipFlex,
61  folderForActions + "/primitives/");
62 
63  std::map <std::string, ROSEE::ActionTrig> fingFlexMap = actionsFinder.findTrig (ROSEE::ActionPrimitive::Type::FingFlex,
64  folderForActions + "/primitives/");
65  unsigned int nFinger = 3;
66  std::map < std::string, ROSEE::ActionSingleJointMultipleTips> singleJointMultipleTipsMap =
67  actionsFinder.findSingleJointMultipleTips (nFinger, folderForActions + "/primitives/") ;
68 
69  nFinger = 2;
70  std::map < std::string, ROSEE::ActionSingleJointMultipleTips> singleJointMultipleTipsMap2 =
71  actionsFinder.findSingleJointMultipleTips (nFinger, folderForActions + "/primitives/") ;
72 
73  nFinger = 5;
74  std::map < std::string, ROSEE::ActionSingleJointMultipleTips> singleJointMultipleTipsMap5 =
75  actionsFinder.findSingleJointMultipleTips (nFinger, folderForActions + "/primitives/") ;
76 
77  auto mulPinch = actionsFinder.findMultiplePinch(3, folderForActions + "/primitives/" );
78 
79 
82  ROSEE::MapActionHandler mapsHandler;
83  mapsHandler.parseAllPrimitives(folderForActions + "/primitives/");
84 
85 
86  /******************************* PRINTS OF PARSED PRIMITIVES *********************************************/
87  std::cout << "PARSED MAP OF PINCHESTIGHT FROM YAML FILE:" << std::endl;
88  for (auto &i : mapsHandler.getPrimitiveMap("pinchTight")) {
89  i.second->print();
90  }
91  std::cout << "PARSED MAP OF PINCHESLOOSE FROM YAML FILE:" << std::endl;
92  for (auto &i : mapsHandler.getPrimitiveMap("pinchLoose")) {
93  i.second->print();
94  }
95  std::cout << "PARSED MAP OF TRIGS FROM YAML FILE:" << std::endl;
96  for (auto &i : mapsHandler.getPrimitiveMap("trig")) {
97  i.second->print();
98  }
99  std::cout << "PARSED MAP OF TIPFLEX FROM YAML FILE:" << std::endl;
100  for (auto &i : mapsHandler.getPrimitiveMap("tipFlex")) {
101  i.second->print();
102  }
103  std::cout << "PARSED MAP OF FINGFLEX FROM YAML FILE:" << std::endl;
104  for (auto &i : mapsHandler.getPrimitiveMap("fingFlex")) {
105  i.second->print();
106  }
107  std::cout << "PARSED MAP OF SINGLEJOINTMULTIPLETIPS_3 FROM YAML FILE:" << std::endl;
108  for (auto &i : mapsHandler.getPrimitiveMap("singleJointMultipleTips_3")) {
109  i.second->print();
110  }
111  std::cout << "DEBUG MULTIPINCH PARSED: " << std::endl;
112  for (auto &it : mapsHandler.getPrimitiveMap("multiplePinchTight_3")) {
113  it.second->print();
114  }
115 
116 
117  ROSEE::YamlWorker yamlWorker;
185  if (parser.getEndEffectorName().compare("schunk") == 0) {
186 
187 
188  //Schuk actions created for paper
189 
190  // lets create the ActionComposed object
191  ROSEE::ActionComposed schunkGrasp ("grasp");
192 
193  //We fill the action with trigs done with all the long fingers
194  schunkGrasp.sumAction (mapsHandler.getPrimitive("trig", "index"));
195  schunkGrasp.sumAction (mapsHandler.getPrimitive("trig", "middle"));
196  schunkGrasp.sumAction (mapsHandler.getPrimitive("trig", "ring"));
197  schunkGrasp.sumAction (mapsHandler.getPrimitive("trig", "pinky"));
198 
199  // for the thumb, we do not want a complete trig, so we pass a scale factor < 1
200  schunkGrasp.sumAction (mapsHandler.getPrimitive("trig", "thumb"), 0.3);
201 
202  //remember to emit the yaml file, so we can use the action each time we want it later
203  yamlWorker.createYamlFile (&schunkGrasp, folderForActions + "/generics/");
204 
205  ROSEE::ActionTimed actionTimed ("timed_wide_grasp");
206 
207  actionTimed.insertAction( mapsHandler.getPrimitive("singleJointMultipleTips_3", "left_hand_Finger_Spread"),
208  0, 0.7, 0, 0.4, "FingerSpread");
209 
210  actionTimed.insertAction( mapsHandler.getPrimitive("singleJointMultipleTips_3", "left_hand_Thumb_Opposition"),
211  0, 0.7, 0, 0.47, "Opposition");
212 
213  ROSEE::Action::Ptr schunkTipFlexs = std::make_shared<ROSEE::ActionComposed>("TipFlexes", true);
214 
215  ROSEE::ActionComposed::Ptr schunkTipFlexsCasted = std::dynamic_pointer_cast<ROSEE::ActionComposed>(schunkTipFlexs);
216 
217  schunkTipFlexsCasted->sumAction (mapsHandler.getPrimitive("tipFlex", "index"), 0.84);
218  schunkTipFlexsCasted->sumAction (mapsHandler.getPrimitive("tipFlex", "middle"), 0.76);
219  schunkTipFlexsCasted->sumAction (mapsHandler.getPrimitive("trig", "thumb"), 0.8);
220 
221  actionTimed.insertAction( schunkTipFlexs, 0.6, 0.2, 0, 1, "TipFlexes");
222 
223  actionTimed.print();
224 
225  yamlWorker.createYamlFile ( &actionTimed, folderForActions + "/timeds/" );
226 
227  // mapsHandler.parseAllTimeds(folderForActions + "/timeds/");
228  //std::cout << "The timed action parsed: " << std::endl;
229  //mapsHandler.getTimed("timed_wide_grasp")->print();
230 
231 
232  } else if (parser.getEndEffectorName().compare("robotiq_3f") == 0) {
233 
234  ROSEE::ActionTimed actionTimed ("timed_wide_grasp");
235 
236  actionTimed.insertAction( mapsHandler.getPrimitive("singleJointMultipleTips_2", "palm_finger_1_joint"),
237  0, 1, 0, 1, "OpenWide");
238 
239  actionTimed.insertAction( mapsHandler.getPrimitive("singleJointMultipleTips_3", "finger_1_joint_1"),
240  0, 0, 0, 0.6, "Grasp");
241 
242  yamlWorker.createYamlFile ( &actionTimed, folderForActions + "/timeds/" );
243 
244 
245  } else if (parserMoveIt->getHandName().compare("heri_II") == 0 ) {
246 
247  //first, we create the "grasp without trigger finger (the third one)
248 
249  //TODO correct way to do this?
250  ROSEE::Action::Ptr grasp3f = std::make_shared<ROSEE::ActionComposed>("grasp3f", true);
251  std::shared_ptr<ROSEE::ActionComposed> grasp3f_casted = std::dynamic_pointer_cast<ROSEE::ActionComposed> (grasp3f);
252 
253  grasp3f_casted->sumAction(mapsHandler.getPrimitive("trig", "finger_1"));
254  grasp3f_casted->sumAction(mapsHandler.getPrimitive("trig", "finger_2"));
255  grasp3f_casted->sumAction(mapsHandler.getPrimitive("trig", "thumb"));
256 
257  // yamlWorker.createYamlFile (&grasp3f_casted, folderForActions + "/generics/");
258 
259  ROSEE::ActionTimed actionTimed ("drill");
260 
261  actionTimed.insertAction( grasp3f, 0, 0, 0, 1, "Grasp3f");
262  actionTimed.insertAction( mapsHandler.getPrimitive("trig", "finger_3"), 3, 0, 0, 1, "TrigOn");
263  actionTimed.insertAction( mapsHandler.getPrimitive("trig", "finger_3"), 4, 0, 0, 0, "TrigOff");
264 
265  yamlWorker.createYamlFile ( &actionTimed, folderForActions + "/timeds/" );
266  mapsHandler.parseAllTimeds(folderForActions + "/timeds/");
267 
268  std::cout << "The timed action parsed: " << std::endl;
269  mapsHandler.getTimed("drill")->print();
270  }
271 
272  return 0;
273 }
274 
ROSEE::MapActionHandler::parseAllPrimitives
bool parseAllPrimitives(std::string pathFolder)
Definition: MapActionHandler.cpp:318
ROSEE::YamlWorker
Definition: YamlWorker.h:47
ros::NodeHandle::setParam
void setParam(const std::string &key, bool b) const
ROSEE::Parser::getEndEffectorName
std::string getEndEffectorName() const
getter for the configure End-Effector name
Definition: Parser.cpp:450
ROSEE::ActionTimed
An action composed by other ones that must be executed one after other with some wait time (also 0) i...
Definition: ActionTimed.h:39
Parser.h
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
ROSEE::MapActionHandler::getTimed
std::shared_ptr< ROSEE::ActionTimed > getTimed(std::string name) const
Definition: MapActionHandler.cpp:191
ROSEE::FindActions::findMultiplePinch
std::map< std::set< std::string >, ROSEE::ActionMultiplePinchTight > findMultiplePinch(unsigned int nFinger, std::string path2saveYaml, bool strict=true)
Finder for MultiplePinch (a pinch done with more than 2 finger). This function return the found multp...
Definition: FindActions.cpp:214
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
MapActionHandler.h
ros.h
ActionGeneric.h
Action.h
ROSEE::MapActionHandler::parseAllTimeds
bool parseAllTimeds(std::string pathFolder)
Definition: MapActionHandler.cpp:351
ROSEE::FindActions::findTrig
std::map< std::string, ROSEE::ActionTrig > findTrig(ROSEE::ActionPrimitive::Type actionType, std::string path2saveYaml)
Function to look for trigs (trig, tipFlex and fingFlex). The type of trig to be looked for is choosen...
Definition: FindActions.cpp:73
ROSEE::ActionComposed::Ptr
std::shared_ptr< ActionComposed > Ptr
Definition: ActionComposed.h:48
ROSEE::FindActions
Class to check which fingertips collide (for the pinch action at the moment)
Definition: FindActions.h:36
ActionComposed.h
main
int main(int argc, char **argv)
Definition: UniversalFindActions.cpp:30
ParserMoveIt.h
ROSEE::YamlWorker::createYamlFile
std::string createYamlFile(const std::map< std::set< std::string >, ActionPrimitive * > mapOfActions, const std::string actionName, std::string pathFolder)
Create/overwrite yaml file and emit info on it about each ActionPrimitive inside the given mapOfActio...
Definition: YamlWorker.cpp:39
ROSEE::Parser::getSrdfString
std::string getSrdfString() const
get the whole srdf file parsed as a string
Definition: Parser.cpp:459
ROSEE::FindActions::findSingleJointMultipleTips
std::map< std::string, ROSEE::ActionSingleJointMultipleTips > findSingleJointMultipleTips(unsigned int nFinger, std::string path2saveYaml)
Definition: FindActions.cpp:137
ROSEE::MapActionHandler::getPrimitiveMap
std::vector< ActionPrimitiveMap > getPrimitiveMap(ROSEE::ActionPrimitive::Type type) const
getter to take all the primitives maps of one type (
Definition: MapActionHandler.cpp:32
ROSEE::MapActionHandler::getPrimitive
ROSEE::ActionPrimitive::Ptr getPrimitive(std::string primitiveName, std::set< std::string > key) const
Definition: MapActionHandler.cpp:52
ROSEE::Parser::getActionPath
std::string getActionPath() const
get the path where emit and parse grasping actions
Definition: Parser.cpp:464
ROSEE::ActionTimed::print
void print() const override
Print info about this action.
Definition: ActionTimed.cpp:106
ROSEE::ActionComposed::sumAction
virtual bool sumAction(ROSEE::Action::Ptr action, double jointPosScaleFactor=1.0, unsigned int jointPosIndex=0)
Function to add another action to this one.
Definition: ActionComposed.cpp:56
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
ROSEE::Parser::getUrdfString
std::string getUrdfString() const
get the whole urdf file parsed as a string
Definition: Parser.cpp:455
ROSEE::Parser::init
bool init()
Initialization function using the ROS param ROSEEConfigPath.
Definition: Parser.cpp:362
ROSEE::Action::Ptr
std::shared_ptr< Action > Ptr
Definition: Action.h:75
ROSEE::FindActions::findPinch
std::pair< std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchTight >, std::map< std::pair< std::string, std::string >, ROSEE::ActionPinchLoose > > findPinch(std::string path2saveYaml)
Function to look for pinches, both Tight and Loose ones. It fill the maps (returned as pair),...
Definition: FindActions.cpp:13
ROSEE::Parser
Class responsible for parsing the YAML file providing the URDF, SRDF, the EE-HAL library implementati...
Definition: Parser.h:46
ROSEE::ActionComposed
A ActionComposed, which is formed by one or more Primitives (or even other composed)....
Definition: ActionComposed.h:44
ActionTimed.h
ros::NodeHandle
ROSEE::MapActionHandler
Definition: MapActionHandler.h:39
FindActions.h
ROSEE::ActionTimed::insertAction
bool insertAction(ROSEE::Action::Ptr action, double marginBefore=0.0, double marginAfter=0.0, unsigned int jointPosIndex=0, double percentJointPos=1, std::string newActionName="")
Insert an action as last one in the time line.
Definition: ActionTimed.cpp:291

end-effector
Author(s): Luca Muratore , Davide Torielli , Liana Bertoni
autogenerated on Sat Dec 14 2024 03:49:26