ROSEndEffector
ROS End-Effector package: provides a ROS-based set of standard interfaces to command robotics end-effectors in an agnostic fashion.
Functions
UniversalFindActions.cpp File Reference
#include <ros/ros.h>
#include <end_effector/FindActions.h>
#include <end_effector/GraspingActions/Action.h>
#include <end_effector/GraspingActions/ActionComposed.h>
#include <end_effector/GraspingActions/ActionTimed.h>
#include <end_effector/GraspingActions/ActionGeneric.h>
#include <end_effector/ParserMoveIt.h>
#include <end_effector/Parser.h>
#include <end_effector/MapActionHandler.h>
+ Include dependency graph for UniversalFindActions.cpp:

Go to the source code of this file.

Functions

int main (int argc, char **argv)
 

Function Documentation

int main ( int  argc,
char **  argv 
)

********************* PARSING TEST and print... these things should not be here

**************************** COMPOSITE ACTION THINGS ************************************************

if (mapsHandler.getPrimitiveMap(ROSEE::ActionPrimitive::Type::Trig).size() > 0 && mapsHandler.getPrimitiveMap(ROSEE::ActionPrimitive::Type::Trig).at(0).size() == parserMoveIt->getNFingers() ) {

std::cout << "A composed action with Independent inner action: " << std::endl; ROSEE::ActionComposed grasp ("grasp", true);

for (auto trig : mapsHandler.getPrimitiveMap("trig")) { grasp.sumAction (trig.second) ; } grasp.print();

yamlWorker.createYamlFile (&grasp, folderForActions + "/generics/");

mapsHandler.parseAllGenerics (folderForActions + "/generics/");

std::cout << "PARSED COMPOSEd" << std::endl; mapsHandler.getGeneric("grasp")->print();

} else { //look if we have a single singleJointMultipleTips_MAXFINGER: it is 99% a grasp 

std::cout << "A singleJointMultipleTips that move all fingers: " << std::endl;

std::map < std::string, ROSEE::ActionSingleJointMultipleTips> singleJointMultipleTipsMap = actionsFinder.findSingleJointMultipleTips (parserMoveIt->getNFingers(), folderForActions + "/primitives/") ;

if (singleJointMultipleTipsMap.size() == 1) { //if more, we do not know which is the one for grasping
    std::cout << "No Composed Grasp with trig but I found a SingleJointMultipleTips_" <<
    parserMoveIt->getNFingers() <<" that probably is a grasp (ie a joint that move all fingers)" << std::endl;
}
std::cout << "PARSED MAP OF singleJointMultipleTips_" << parserMoveIt->getNFingers() << "  FROM YAML FILE:" << std::endl;
for (auto &i : mapsHandler.getPrimitiveMap("singleJointMultipleTips_" + std::to_string(parserMoveIt->getNFingers()) )) {
    i.second->print();
}

}

**************************** SIMPLE ACTION MANUALLY CREATED ***********************************************

example only doable if maps is not empty

if (maps.first.size() != 0 ) {
    ROSEE::JointPos jp;

for now copy jp of another action jp = ROSEE::operator*(maps.first.begin()->second.getJointPos(), 2); auto jpc = maps.first.begin()->second.getJointsInvolvedCount();

ROSEE::ActionGeneric simpleAction("casual", jp, jpc); std::cout << std::endl << "Casual action manually created: " << std::endl;

simpleAction.print();

yamlWorker.createYamlFile( &simpleAction, folderForActions + "/generics/" );

mapsHandler.parseAllGenerics (folderForActions + "/generics/"); //NOTE already called before

std::cout << "The parsed casual: " << std::endl; mapsHandler.getGeneric("casual")->print(); }

**************************** TIMED ACTION THINGS***************************** EXAMPLE ONLY VALID FOR SPECIFIC HANDS

Definition at line 30 of file UniversalFindActions.cpp.

30  {
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 }
ROSEE::ActionComposed::Ptr
std::shared_ptr< ActionComposed > Ptr
Definition: ActionComposed.h:48
ROSEE::ActionComposed
A ActionComposed, which is formed by one or more Primitives (or even other composed).
Definition: ActionComposed.h:44
ROSEE::MapActionHandler::parseAllPrimitives
bool parseAllPrimitives(std::string pathFolder)
Definition: MapActionHandler.cpp:318
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::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::MapActionHandler::getPrimitive
ROSEE::ActionPrimitive::Ptr getPrimitive(std::string primitiveName, std::set< std::string > key) const
Definition: MapActionHandler.cpp:52
ROSEE::MapActionHandler::getTimed
std::shared_ptr< ROSEE::ActionTimed > getTimed(std::string name) const
Definition: MapActionHandler.cpp:191
ROSEE::Action::Ptr
std::shared_ptr< Action > Ptr
Definition: Action.h:75
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
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
Generated on Thu Nov 21 2024 14:46:20 for ROSEndEffector by   doxygen 1.8.11