20 #ifndef OPENRAVE_PLANNER_PARAMETERS_H
21 #define OPENRAVE_PLANNER_PARAMETERS_H
31 _vXMLParameters.push_back(
"exploreprob");
32 _vXMLParameters.push_back(
"expectedsize");
41 virtual bool serialize(std::ostream& O,
int options=0)
const
43 if( !PlannerParameters::serialize(O,options&~1) ) {
46 O <<
"<exploreprob>" << _fExploreProb <<
"</exploreprob>" << std::endl;
47 O <<
"<expectedsize>" << _nExpectedDataSize <<
"</expectedsize>" << std::endl;
48 if( !(options & 1) ) {
49 O << _sExtraParameters << std::endl;
56 if( _bProcessingExploration ) {
61 case PE_Support:
return PE_Support;
62 case PE_Ignore:
return PE_Ignore;
65 _bProcessingExploration = name==
"exploreprob"||name==
"expectedsize";
66 return _bProcessingExploration ? PE_Support : PE_Pass;
70 virtual bool endElement(
const std::string& name)
73 if( _bProcessingExploration ) {
74 if( name ==
"exploreprob") {
77 else if( name ==
"expectedsize" ) {
78 _ss >> _nExpectedDataSize;
81 RAVELOG_WARN(str(boost::format(
"unknown tag %s\n")%name));
83 _bProcessingExploration =
false;
88 return PlannerParameters::endElement(name);
95 RAStarParameters() : fRadius(0.1f), fDistThresh(0.03f), fGoalCoeff(1), nMaxChildren(5), nMaxSampleTries(10), _bProcessingRA(false) {
96 _vXMLParameters.push_back(
"radius");
97 _vXMLParameters.push_back(
"distthresh");
98 _vXMLParameters.push_back(
"goalcoeff");
99 _vXMLParameters.push_back(
"maxchildren");
100 _vXMLParameters.push_back(
"maxsampletries");
110 virtual bool serialize(std::ostream& O,
int options)
const
112 if( !PlannerParameters::serialize(O,options&~1) ) {
115 O <<
"<radius>" << fRadius <<
"</radius>" << std::endl;
116 O <<
"<distthresh>" << fDistThresh <<
"</distthresh>" << std::endl;
117 O <<
"<goalcoeff>" << fGoalCoeff <<
"</goalcoeff>" << std::endl;
118 O <<
"<maxchildren>" << nMaxChildren <<
"</maxchildren>" << std::endl;
119 O <<
"<maxsampletries>" << nMaxSampleTries <<
"</maxsampletries>" << std::endl;
120 if( !(options & 1) ) {
121 O << _sExtraParameters << std::endl;
129 if( _bProcessingRA ) {
134 case PE_Support:
return PE_Support;
135 case PE_Ignore:
return PE_Ignore;
137 _bProcessingRA = name==
"radius"||name==
"distthresh"||name==
"goalcoeff"||name==
"maxchildren"||name==
"maxsampletries";
138 return _bProcessingRA ? PE_Support : PE_Pass;
140 virtual bool endElement(
const std::string& name)
142 if( _bProcessingRA ) {
143 if( name ==
"radius") {
146 else if( name ==
"distthresh") {
149 else if( name ==
"goalcoeff") {
152 else if( name ==
"maxchildren") {
155 else if( name ==
"maxsampletries") {
156 _ss >> nMaxSampleTries;
159 RAVELOG_WARN(str(boost::format(
"unknown tag %s\n")%name));
161 _bProcessingRA =
false;
165 return PlannerParameters::endElement(name);
173 _vXMLParameters.push_back(
"grasps");
174 _vXMLParameters.push_back(
"target");
175 _vXMLParameters.push_back(
"numgradsamples");
176 _vXMLParameters.push_back(
"visgraspthresh");
177 _vXMLParameters.push_back(
"graspdistthresh");
189 virtual bool serialize(std::ostream& O,
int options=0)
const
191 if( !PlannerParameters::serialize(O,options&~1) ) {
194 O <<
"<grasps>" << _vgrasps.size() <<
" ";
195 for(std::vector<Transform>::const_iterator it = _vgrasps.begin(); it != _vgrasps.end(); ++it) {
198 O <<
"</grasps>" << std::endl;
199 O <<
"<target>" << (!!_ptarget ? _ptarget->GetEnvironmentId() : 0) <<
"</target>" << std::endl;
200 O <<
"<numgradsamples>" << _nGradientSamples <<
"</numgradsamples>" << std::endl;
201 O <<
"<visgraspthresh>" << _fVisibiltyGraspThresh <<
"</visgraspthresh>" << std::endl;
202 O <<
"<graspdistthresh>" << _fGraspDistThresh <<
"</graspdistthresh>" << std::endl;
203 if( !(options & 1) ) {
204 O << _sExtraParameters << std::endl;
211 if( _bProcessingGS ) {
216 case PE_Support:
return PE_Support;
217 case PE_Ignore:
return PE_Ignore;
220 _bProcessingGS = name==
"grasps"||name==
"target"||name==
"numgradsamples"||name==
"visgraspthresh"||name==
"graspdistthresh";
221 return _bProcessingGS ? PE_Support : PE_Pass;
224 virtual bool endElement(
const std::string& name)
226 if( _bProcessingGS ) {
227 if( name ==
"grasps" ) {
230 _vgrasps.resize(ngrasps);
231 for(std::vector<Transform>::iterator it = _vgrasps.begin(); it != _vgrasps.end(); ++it) {
235 else if( name ==
"target" ) {
238 _ptarget = _penv->GetBodyFromEnvironmentId(
id);
240 else if( name ==
"numgradsamples" ) {
241 _ss >> _nGradientSamples;
243 else if( name ==
"visgraspthresh" ) {
244 _ss >> _fVisibiltyGraspThresh;
246 else if( name ==
"graspdistthresh") {
247 _ss >> _fGraspDistThresh;
250 RAVELOG_WARN(str(boost::format(
"unknown tag %s\n")%name));
252 _bProcessingGS =
false;
257 return PlannerParameters::endElement(name);
264 GraspParameters(
EnvironmentBasePtr penv) :
PlannerBase::PlannerParameters(), fstandoff(0), ftargetroll(0), vtargetdirection(0,0,1), btransformrobot(false), breturntrajectory(false), bonlycontacttarget(true), btightgrasp(false), bavoidcontact(false), fcoarsestep(0.1f), ffinestep(0.001f), ftranslationstepmult(0.1f), fgraspingnoise(0), _penv(penv) {
265 _vXMLParameters.push_back(
"fstandoff");
266 _vXMLParameters.push_back(
"targetbody");
267 _vXMLParameters.push_back(
"ftargetroll");
268 _vXMLParameters.push_back(
"vtargetdirection");
269 _vXMLParameters.push_back(
"vtargetposition");
270 _vXMLParameters.push_back(
"vmanipulatordirection");
271 _vXMLParameters.push_back(
"btransformrobot");
272 _vXMLParameters.push_back(
"breturntrajectory");
273 _vXMLParameters.push_back(
"bonlycontacttarget");
274 _vXMLParameters.push_back(
"btightgrasp");
275 _vXMLParameters.push_back(
"bavoidcontact");
276 _vXMLParameters.push_back(
"vavoidlinkgeometry");
277 _vXMLParameters.push_back(
"fcoarsestep");
278 _vXMLParameters.push_back(
"ffinestep");
279 _vXMLParameters.push_back(
"ftranslationstepmult");
280 _vXMLParameters.push_back(
"fgraspingnoise");
281 _bProcessingGrasp =
false;
306 virtual bool serialize(std::ostream& O,
int options=0)
const
308 if( !PlannerParameters::serialize(O, options&~1) ) {
311 O <<
"<fstandoff>" << fstandoff <<
"</fstandoff>" << std::endl;
312 O <<
"<targetbody>" << (int)(!targetbody ? 0 : targetbody->GetEnvironmentId()) <<
"</targetbody>" << std::endl;
313 O <<
"<ftargetroll>" << ftargetroll <<
"</ftargetroll>" << std::endl;
314 O <<
"<vtargetdirection>" << vtargetdirection <<
"</vtargetdirection>" << std::endl;
315 O <<
"<vtargetposition>" << vtargetposition <<
"</vtargetposition>" << std::endl;
316 O <<
"<vmanipulatordirection>" << vmanipulatordirection <<
"</vmanipulatordirection>" << std::endl;
317 O <<
"<btransformrobot>" << btransformrobot <<
"</btransformrobot>" << std::endl;
318 O <<
"<breturntrajectory>" << breturntrajectory <<
"</breturntrajectory>" << std::endl;
319 O <<
"<bonlycontacttarget>" << bonlycontacttarget <<
"</bonlycontacttarget>" << std::endl;
320 O <<
"<btightgrasp>" << btightgrasp <<
"</btightgrasp>" << std::endl;
321 O <<
"<bavoidcontact>" << bavoidcontact <<
"</bavoidcontact>" << std::endl;
322 O <<
"<vavoidlinkgeometry>" << std::endl;
323 for(std::vector<std::string>::const_iterator it = vavoidlinkgeometry.begin(); it != vavoidlinkgeometry.end(); ++it) {
326 O <<
"</vavoidlinkgeometry>" << std::endl;
327 O <<
"<fcoarsestep>" << fcoarsestep <<
"</fcoarsestep>" << std::endl;
328 O <<
"<ffinestep>" << ffinestep <<
"</ffinestep>" << std::endl;
329 O <<
"<ftranslationstepmult>" << ftranslationstepmult <<
"</ftranslationstepmult>" << std::endl;
330 O <<
"<fgraspingnoise>" << fgraspingnoise <<
"</fgraspingnoise>" << std::endl;
331 if( !(options & 1) ) {
332 O << _sExtraParameters << std::endl;
339 if( _bProcessingGrasp ) {
344 case PE_Support:
return PE_Support;
345 case PE_Ignore:
return PE_Ignore;
347 if( name ==
"vavoidlinkgeometry" ) {
348 vavoidlinkgeometry.resize(0);
352 static boost::array<std::string,16> tags = {{
"fstandoff",
"targetbody",
"ftargetroll",
"vtargetdirection",
"vtargetposition",
"vmanipulatordirection",
"btransformrobot",
"breturntrajectory",
"bonlycontacttarget",
"btightgrasp",
"bavoidcontact",
"vavoidlinkgeometry",
"fcoarsestep",
"ffinestep",
"ftranslationstepmult",
"fgraspingnoise"}};
353 _bProcessingGrasp = find(tags.begin(),tags.end(),name) != tags.end();
354 return _bProcessingGrasp ? PE_Support : PE_Pass;
358 virtual bool endElement(
const std::string& name)
361 if( _bProcessingGrasp ) {
362 if( name ==
"vavoidlinkgeometry" ) {
363 vavoidlinkgeometry = std::vector<std::string>((std::istream_iterator<std::string>(_ss)), std::istream_iterator<std::string>());
365 else if( name ==
"fstandoff") {
368 else if( name ==
"targetbody") {
371 targetbody = _penv->GetBodyFromEnvironmentId(
id);
373 else if( name ==
"ftargetroll") {
376 else if( name ==
"vtargetdirection") {
377 _ss >> vtargetdirection;
378 vtargetdirection.normalize3();
380 else if( name ==
"vtargetposition") {
381 _ss >> vtargetposition;
383 else if( name ==
"vmanipulatordirection") {
384 _ss >> vmanipulatordirection;
386 else if( name ==
"btransformrobot") {
387 _ss >> btransformrobot;
389 else if( name ==
"breturntrajectory") {
390 _ss >> breturntrajectory;
392 else if( name ==
"bonlycontacttarget") {
393 _ss >> bonlycontacttarget;
395 else if( name ==
"btightgrasp" ) {
398 else if( name ==
"bavoidcontact" ) {
399 _ss >> bavoidcontact;
401 else if( name ==
"fcoarsestep" ) {
404 else if( name ==
"ffinestep" ) {
407 else if( name ==
"fgraspingnoise" ) {
408 _ss >> fgraspingnoise;
410 else if( name ==
"ftranslationstepmult" ) {
411 _ss >> ftranslationstepmult;
414 RAVELOG_WARN(str(boost::format(
"unknown tag %s\n")%name));
416 _bProcessingGrasp =
false;
421 return PlannerParameters::endElement(name);
435 TrajectoryTimingParameters() : _interpolation(
""), _pointtolerance(0.2), _hastimestamps(false), _hasvelocities(false), _outputaccelchanges(true), _multidofinterp(0), _bProcessing(false) {
437 _vXMLParameters.push_back(
"interpolation");
438 _vXMLParameters.push_back(
"hastimestamps");
439 _vXMLParameters.push_back(
"hasvelocities");
440 _vXMLParameters.push_back(
"pointtolerance");
441 _vXMLParameters.push_back(
"outputaccelchanges");
442 _vXMLParameters.push_back(
"multidofinterp");
453 virtual bool serialize(std::ostream& O,
int options=0)
const
455 if( !PlannerParameters::serialize(O, options&~1) ) {
458 O <<
"<interpolation>" << _interpolation <<
"</interpolation>" << std::endl;
459 O <<
"<hastimestamps>" << _hastimestamps <<
"</hastimestamps>" << std::endl;
460 O <<
"<hasvelocities>" << _hasvelocities <<
"</hasvelocities>" << std::endl;
461 O <<
"<pointtolerance>" << _pointtolerance <<
"</pointtolerance>" << std::endl;
462 O <<
"<outputaccelchanges>" << _outputaccelchanges <<
"</outputaccelchanges>" << std::endl;
463 O <<
"<multidofinterp>" << _multidofinterp <<
"</multidofinterp>" << std::endl;
464 if( !(options & 1) ) {
465 O << _sExtraParameters << std::endl;
478 case PE_Support:
return PE_Support;
479 case PE_Ignore:
return PE_Ignore;
482 _bProcessing = name==
"interpolation" || name==
"hastimestamps" || name==
"hasvelocities" || name==
"pointtolerance" || name==
"outputaccelchanges" || name==
"multidofinterp";
483 return _bProcessing ? PE_Support : PE_Pass;
486 virtual bool endElement(
const std::string& name)
489 if( name ==
"interpolation") {
490 _ss >> _interpolation;
492 else if( name ==
"hastimestamps" ) {
493 _ss >> _hastimestamps;
495 else if( name ==
"hasvelocities" ) {
496 _ss >> _hasvelocities;
498 else if( name ==
"pointtolerance" ) {
499 _ss >> _pointtolerance;
501 else if( name ==
"outputaccelchanges" ) {
502 _ss >> _outputaccelchanges;
504 else if( name ==
"multidofinterp" ) {
505 _ss >> _multidofinterp;
508 RAVELOG_WARN(str(boost::format(
"unknown tag %s\n")%name));
510 _bProcessing =
false;
515 return PlannerParameters::endElement(name);
526 _vXMLParameters.push_back(
"maxlinkspeed");
527 _vXMLParameters.push_back(
"maxlinkaccel");
528 _vXMLParameters.push_back(
"maxmanipspeed");
529 _vXMLParameters.push_back(
"maxmanipaccel");
530 _vXMLParameters.push_back(
"mingripperdistance");
531 _vXMLParameters.push_back(
"velocitydistancethresh");
543 virtual bool serialize(std::ostream& O,
int options=0)
const
548 O <<
"<maxlinkspeed>" << maxlinkspeed <<
"</maxlinkspeed>" << std::endl;
549 O <<
"<maxlinkaccel>" << maxlinkaccel <<
"</maxlinkaccel>" << std::endl;
550 O <<
"<maxmanipspeed>" << maxmanipspeed <<
"</maxmanipspeed>" << std::endl;
551 O <<
"<maxmanipaccel>" << maxmanipaccel <<
"</maxmanipaccel>" << std::endl;
552 O <<
"<mingripperdistance>" << mingripperdistance <<
"</mingripperdistance>" << std::endl;
553 O <<
"<velocitydistancethresh>" << velocitydistancethresh <<
"</velocitydistancethresh>" << std::endl;
554 if( !(options & 1) ) {
555 O << _sExtraParameters << std::endl;
563 if( _bCProcessing ) {
568 case PE_Support:
return PE_Support;
569 case PE_Ignore:
return PE_Ignore;
571 _bCProcessing = name==
"maxlinkspeed" || name ==
"maxlinkaccel" || name==
"maxmanipspeed" || name ==
"maxmanipaccel" || name==
"mingripperdistance" || name==
"velocitydistancethresh";
572 return _bCProcessing ? PE_Support : PE_Pass;
575 virtual bool endElement(
const std::string& name)
577 if( _bCProcessing ) {
578 if( name ==
"maxlinkspeed") {
581 else if( name ==
"maxlinkaccel") {
584 else if( name ==
"maxmanipspeed") {
585 _ss >> maxmanipspeed;
587 else if( name ==
"maxmanipaccel") {
588 _ss >> maxmanipaccel;
590 else if( name ==
"mingripperdistance" ) {
591 _ss >> mingripperdistance;
593 else if( name ==
"velocitydistancethresh" ) {
594 _ss >> velocitydistancethresh;
597 RAVELOG_WARN(str(boost::format(
"unknown tag %s\n")%name));
599 _bCProcessing =
false;
604 return PlannerParameters::endElement(name);
633 virtual bool serialize(std::ostream& O,
int options=0)
const;
635 virtual bool endElement(
const std::string& name);
636 virtual void characters(
const std::string& ch);
646 _vXMLParameters.push_back(
"minimumgoalpaths");
653 virtual bool serialize(std::ostream& O,
int options=0)
const
655 if( !PlannerParameters::serialize(O, options&~1) ) {
658 O <<
"<minimumgoalpaths>" << _minimumgoalpaths <<
"</minimumgoalpaths>" << std::endl;
659 if( !(options & 1) ) {
660 O << _sExtraParameters << std::endl;
672 case PE_Support:
return PE_Support;
673 case PE_Ignore:
return PE_Ignore;
676 _bProcessing = name==
"minimumgoalpaths";
677 return _bProcessing ? PE_Support : PE_Pass;
680 virtual bool endElement(
const std::string& name)
683 if( name ==
"minimumgoalpaths") {
684 _ss >> _minimumgoalpaths;
687 RAVELOG_WARN(str(boost::format(
"unknown tag %s\n")%name));
689 _bProcessing =
false;
694 return PlannerParameters::endElement(name);
704 _vXMLParameters.push_back(
"goalbias");
711 virtual bool serialize(std::ostream& O,
int options=0)
const
713 if( !PlannerParameters::serialize(O, options&~1) ) {
716 O <<
"<goalbias>" << _fGoalBiasProb <<
"</goalbias>" << std::endl;
717 if( !(options & 1) ) {
718 O << _sExtraParameters << std::endl;
731 case PE_Support:
return PE_Support;
732 case PE_Ignore:
return PE_Ignore;
735 _bProcessing = name==
"goalbias";
736 return _bProcessing ? PE_Support : PE_Pass;
739 virtual bool endElement(
const std::string& name)
742 if( name ==
"goalbias") {
743 _ss >> _fGoalBiasProb;
746 RAVELOG_WARN(str(boost::format(
"unknown tag %s\n")%name));
748 _bProcessing =
false;
753 return PlannerParameters::endElement(name);