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::getPrimitiveMap
std::vector< ActionPrimitiveMap > getPrimitiveMap(ROSEE::ActionPrimitive::Type type) const
getter to take all the primitives maps of one type (
Definition: MapActionHandler.cpp:32
ros::NodeHandle
ROSEE::MapActionHandler::getTimed
std::shared_ptr< ROSEE::ActionTimed > getTimed(std::string name) const
Definition: MapActionHandler.cpp:191
ROSEE::FindActions::findSingleJointMultipleTips
std::map< std::string, ROSEE::ActionSingleJointMultipleTips > findSingleJointMultipleTips(unsigned int nFinger, std::string path2saveYaml)
Definition: FindActions.cpp:137
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
ROSEE::ActionComposed::Ptr
std::shared_ptr< ActionComposed > Ptr
Definition: ActionComposed.h:48
main
int main(int argc, char **argv)
Definition: UniversalFindActions.cpp:30
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSEE::Parser::init
bool init()
Initialization function using the ROS param ROSEEConfigPath.
Definition: Parser.cpp:362
ROSEE::ActionComposed
A ActionComposed, which is formed by one or more Primitives (or even other composed). It is useful for example to create an action that grasp only with bending the tips (e.g. to take a dish from above) If the ActionComposed has the boolean value independent to true, it means that include indipendent sub-actions, so, each joint is used by at maximum by ONLY ONE of the sub-action inside. In this case the jointsInvolvedCount will contain only 0 or 1 values. If the ActionComposed is not independent, each joint position is calculated as the mean of all the joint position of the contained sub-actions that uses that joint. So each mean can include different primitives, so we used the jointsInvolvedCount vector to store the number of sub action that use each joint.
Definition: ActionComposed.h:44
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
ROSEE::MapActionHandler::getPrimitive
ROSEE::ActionPrimitive::Ptr getPrimitive(std::string primitiveName, std::set< std::string > key) const
Definition: MapActionHandler.cpp:52
ROSEE::MapActionHandler::parseAllPrimitives
bool parseAllPrimitives(std::string pathFolder)
Definition: MapActionHandler.cpp:318
ROSEE::Parser::getEndEffectorName
std::string getEndEffectorName() const
getter for the configure End-Effector name
Definition: Parser.cpp:450
ROSEE::Parser::getUrdfString
std::string getUrdfString() const
get the whole urdf file parsed as a string
Definition: Parser.cpp:455
ROSEE::Parser::getActionPath
std::string getActionPath() const
get the path where emit and parse grasping actions
Definition: Parser.cpp:464
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
ROSEE::Action::Ptr
std::shared_ptr< Action > Ptr
Definition: Action.h:75
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::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
ROSEE::FindActions
Class to check which fingertips collide (for the pinch action at the moment)
Definition: FindActions.h:36
ROSEE::Parser
Class responsible for parsing the YAML file providing the URDF, SRDF, the EE-HAL library implementati...
Definition: Parser.h:46
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
ROSEE::ActionTimed::print
void print() const override
Print info about this action.
Definition: ActionTimed.cpp:106
ros::NodeHandle::setParam
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
parser
parser
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
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::getSrdfString
std::string getSrdfString() const
get the whole srdf file parsed as a string
Definition: Parser.cpp:459
ROSEE::MapActionHandler::parseAllTimeds
bool parseAllTimeds(std::string pathFolder)
Definition: MapActionHandler.cpp:351
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

end-effector
Author(s): Luca Muratore , Davide Torielli , Liana Bertoni
autogenerated on Tue Apr 5 2022 02:57:53