17 #include "libopenrave.h"
29 stringstream::streampos pos = I.tellg();
32 string pbuf = buf.str();
33 const char* p = strcasestr(pbuf.c_str(),
"</PlannerParameters>");
37 ppsize=(p-pbuf.c_str())+20;
38 I.seekg((
size_t)pos+ppsize);
43 pp._plannerparametersdepth = 0;
52 BOOST_ASSERT(q1.size()==q2.size());
53 for(
size_t i = 0; i < q1.size(); ++i) {
58 bool addstates(std::vector<dReal>& q,
const std::vector<dReal>& qdelta,
int fromgoal)
60 BOOST_ASSERT(q.size()==qdelta.size());
61 for(
size_t i = 0; i < q.size(); ++i) {
69 BOOST_ASSERT(!!
_params->_setstatefn);
84 void PlannerBase::PlannerParameters::StateSaver::_Restore()
86 _params->_setstatefn(_values);
94 _sPostProcessingParameters =
"<_nmaxiterations>20</_nmaxiterations><_postprocessing planner=\"parabolicsmoother\"><_nmaxiterations>100</_nmaxiterations></_postprocessing>";
130 vinitialconfig.resize(0);
131 vgoalconfig.resize(0);
133 _vConfigLowerLimit.resize(0);
134 _vConfigUpperLimit.resize(0);
135 _vConfigResolution.resize(0);
136 _vConfigVelocityLimit.resize(0);
137 _vConfigAccelerationLimit.resize(0);
138 _sPostProcessingPlanner =
"";
139 _sPostProcessingParameters.resize(0);
140 _sExtraParameters.resize(0);
142 _fStepLength = 0.04f;
143 _plannerparametersdepth = 0;
146 std::stringstream ss;
147 ss << std::setprecision(std::numeric_limits<dReal>::digits10+1);
160 O << _configurationspecification << endl;
161 O <<
"<_vinitialconfig>";
162 FOREACHC(it, vinitialconfig) {
165 O <<
"</_vinitialconfig>" << endl;
166 O <<
"<_vgoalconfig>";
167 FOREACHC(it, vgoalconfig) {
170 O <<
"</_vgoalconfig>" << endl;
171 O <<
"<_vconfiglowerlimit>";
172 FOREACHC(it, _vConfigLowerLimit) {
175 O <<
"</_vconfiglowerlimit>" << endl;
176 O <<
"<_vconfigupperlimit>";
177 FOREACHC(it, _vConfigUpperLimit) {
180 O <<
"</_vconfigupperlimit>" << endl;
181 O <<
"<_vconfigvelocitylimit>";
182 FOREACHC(it, _vConfigVelocityLimit) {
185 O <<
"</_vconfigvelocitylimit>" << endl;
186 O <<
"<_vconfigaccelerationlimit>";
187 FOREACHC(it, _vConfigAccelerationLimit) {
190 O <<
"</_vconfigaccelerationlimit>" << endl;
191 O <<
"<_vconfigresolution>";
192 FOREACHC(it, _vConfigResolution) {
195 O <<
"</_vconfigresolution>" << endl;
197 O <<
"<_nmaxiterations>" << _nMaxIterations <<
"</_nmaxiterations>" << endl;
198 O <<
"<_fsteplength>" << _fStepLength <<
"</_fsteplength>" << endl;
199 O <<
"<_postprocessing planner=\"" << _sPostProcessingPlanner <<
"\">" << _sPostProcessingParameters <<
"</_postprocessing>" << endl;
200 if( !(options & 1) ) {
201 O << _sExtraParameters << endl;
209 if( !!__pcurreader ) {
210 if( __pcurreader->startElement(name, atts) == PE_Support ) {
216 if( __processingtag.size() > 0 ) {
219 if( name==
"plannerparameters" ) {
220 _plannerparametersdepth++;
224 if( name ==
"_postprocessing" ) {
225 _sslocal.reset(
new std::stringstream());
226 _sPostProcessingPlanner=
"";
227 _sPostProcessingParameters=
"";
228 FOREACHC(itatt,atts) {
229 if( itatt->first ==
"planner" ) {
230 _sPostProcessingPlanner = itatt->second;
237 if( find(_vXMLParameters.begin(),_vXMLParameters.end(),name) == _vXMLParameters.end() ) {
238 _sslocal.reset(
new std::stringstream());
239 *_sslocal <<
"<" << name <<
" ";
240 FOREACHC(itatt, atts) {
241 *_sslocal << itatt->first <<
"=\"" << itatt->second <<
"\" ";
243 *_sslocal <<
">" << endl;
248 if( name ==
"configuration" ) {
253 static const boost::array<std::string,10> names = {{
"_vinitialconfig",
"_vgoalconfig",
"_vconfiglowerlimit",
"_vconfigupperlimit",
"_vconfigvelocitylimit",
"_vconfigaccelerationlimit",
"_vconfigresolution",
"_nmaxiterations",
"_fsteplength",
"_postprocessing"}};
254 if( find(names.begin(),names.end(),name) != names.end() ) {
255 __processingtag = name;
263 if( !!__pcurreader ) {
264 if( __pcurreader->endElement(name) ) {
265 boost::shared_ptr<DummyXMLReader> pdummy = boost::dynamic_pointer_cast<
DummyXMLReader>(__pcurreader);
267 if( pdummy->GetFieldName() ==
"_postprocessing" ) {
268 _sPostProcessingParameters = _sslocal->str();
272 *_sslocal <<
"</" << name <<
">" << endl;
273 _sExtraParameters += _sslocal->str();
277 __pcurreader.reset();
280 else if( name ==
"plannerparameters" ) {
281 return --_plannerparametersdepth < 0;
283 else if( __processingtag.size() > 0 ) {
284 if( name ==
"_vinitialconfig") {
285 vinitialconfig = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
287 else if( name ==
"_vgoalconfig") {
288 vgoalconfig = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
290 else if( name ==
"_vconfiglowerlimit") {
291 _vConfigLowerLimit = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
293 else if( name ==
"_vconfigupperlimit") {
294 _vConfigUpperLimit = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
296 else if( name ==
"_vconfigresolution") {
297 _vConfigResolution = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
299 else if( name ==
"_vconfigvelocitylimit") {
300 _vConfigVelocityLimit = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
302 else if( name ==
"_vconfigaccelerationlimit") {
303 _vConfigAccelerationLimit = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
305 else if( name ==
"_nmaxiterations") {
306 _ss >> _nMaxIterations;
308 else if( name ==
"_fsteplength") {
311 if( name !=__processingtag ) {
312 RAVELOG_WARN(str(boost::format(
"invalid tag %s!=%s\n")%name%__processingtag));
314 __processingtag =
"";
323 if( !!__pcurreader ) {
324 __pcurreader->characters(ch);
334 O <<
"<" << v.
GetXMLId() <<
">" << endl;
336 O <<
"</" << v.
GetXMLId() <<
">" << endl;
343 FOREACHC(itlink, robot->GetLinks()) {
344 if( (*itlink)->IsStatic() ) {
345 FOREACHC(itdof, robot->GetActiveDOFIndices()) {
347 OPENRAVE_ASSERT_FORMAT(!robot->DoesAffect(pjoint->GetJointIndex(),(*itlink)->GetIndex()),
"robot %s link %s is static when it is affected by active joint %s", robot->GetName()%(*itlink)->GetName()%pjoint->GetName(),
ORE_InvalidState);
352 using namespace planningutils;
353 _distmetricfn = boost::bind(&SimpleDistanceMetric::Eval,boost::shared_ptr<SimpleDistanceMetric>(
new SimpleDistanceMetric(robot)),_1,_2);
355 boost::shared_ptr<SimpleNeighborhoodSampler> defaultsamplefn(
new SimpleNeighborhoodSampler(pconfigsampler,_distmetricfn));
356 _samplefn = boost::bind(&SimpleNeighborhoodSampler::Sample,defaultsamplefn,_1);
357 _sampleneighfn = boost::bind(&SimpleNeighborhoodSampler::Sample,defaultsamplefn,_1,_2,_3);
363 std::list<KinBodyPtr> listCheckCollisions; listCheckCollisions.push_back(robot);
364 boost::shared_ptr<LineCollisionConstraint> pcollision(
new LineCollisionConstraint(listCheckCollisions,
true));
365 _checkpathconstraintsfn = boost::bind(&LineCollisionConstraint::Check,pcollision,
PlannerParametersWeakPtr(shared_parameters()), _1, _2, _3, _4);
367 robot->GetActiveDOFLimits(_vConfigLowerLimit,_vConfigUpperLimit);
368 robot->GetActiveDOFVelocityLimits(_vConfigVelocityLimit);
369 robot->GetActiveDOFAccelerationLimits(_vConfigAccelerationLimit);
370 robot->GetActiveDOFResolutions(_vConfigResolution);
371 robot->GetActiveDOFValues(vinitialconfig);
372 _configurationspecification = robot->GetActiveConfigurationSpecification();
375 void _CallDiffStateFns(
const std::vector< std::pair<PlannerBase::PlannerParameters::DiffStateFn, int> >& vfunctions,
int nDOF,
int nMaxDOFForGroup, std::vector<dReal>& v0,
const std::vector<dReal>& v1)
377 if( vfunctions.size() == 1 ) {
378 vfunctions.at(0).first(v0,v1);
383 std::vector<dReal> vtemp0, vtemp1; vtemp0.reserve(nMaxDOFForGroup); vtemp1.reserve(nMaxDOFForGroup);
384 std::vector<dReal>::iterator itsource0 = v0.begin();
385 std::vector<dReal>::const_iterator itsource1 = v1.begin();
386 FOREACHC(itfn, vfunctions) {
387 vtemp0.resize(itfn->second);
388 std::copy(itsource0,itsource0+itfn->second,vtemp0.begin());
389 vtemp1.resize(itfn->second);
390 std::copy(itsource1,itsource1+itfn->second,vtemp1.begin());
391 itfn->first(vtemp0, vtemp1);
393 std::copy(vtemp0.begin(),vtemp0.end(),itsource0);
394 itsource0 += itfn->second;
395 itsource1 += itfn->second;
404 std::vector<dReal> c = c0;
407 for(
size_t i=0; i < c.size(); i++) {
408 dist += vweights2.at(i)*c.at(i)*c.at(i);
413 dReal _CallDistMetricFns(
const std::vector< std::pair<PlannerBase::PlannerParameters::DistMetricFn, int> >& vfunctions,
int nDOF,
int nMaxDOFForGroup,
const std::vector<dReal>& v0,
const std::vector<dReal>& v1)
415 if( vfunctions.size() == 1 ) {
416 return vfunctions.at(0).first(v0, v1);
421 std::vector<dReal> vtemp0, vtemp1; vtemp0.reserve(nMaxDOFForGroup); vtemp1.reserve(nMaxDOFForGroup);
422 std::vector<dReal>::const_iterator itsource0 = v0.begin(), itsource1 = v1.begin();
424 FOREACHC(itfn, vfunctions) {
425 vtemp0.resize(itfn->second);
426 std::copy(itsource0,itsource0+itfn->second,vtemp0.begin());
427 vtemp1.resize(itfn->second);
428 std::copy(itsource1,itsource1+itfn->second,vtemp1.begin());
429 f += itfn->first(vtemp0, vtemp1);
430 itsource0 += itfn->second;
431 itsource1 += itfn->second;
437 bool _CallSampleFns(
const std::vector< std::pair<PlannerBase::PlannerParameters::SampleFn, int> >& vfunctions,
int nDOF,
int nMaxDOFForGroup, std::vector<dReal>& v)
439 if( vfunctions.size() == 1 ) {
440 return vfunctions.at(0).first(v);
443 std::vector<dReal> vtemp; vtemp.reserve(nMaxDOFForGroup);
445 std::vector<dReal>::iterator itdest = v.begin();
446 FOREACHC(itfn, vfunctions) {
447 if( !itfn->first(vtemp) ) {
450 std::copy(vtemp.begin(),vtemp.end(),itdest);
451 itdest += itfn->second;
457 bool _CallSampleNeighFns(
const std::vector< std::pair<PlannerBase::PlannerParameters::SampleNeighFn, int> >& vfunctions,
const std::vector< std::pair<PlannerBase::PlannerParameters::DistMetricFn, int> >& vdistfunctions,
int nDOF,
int nMaxDOFForGroup, std::vector<dReal>& v,
const std::vector<dReal>& vCurSample,
dReal fRadius)
459 if( vfunctions.size() == 1 ) {
460 return vfunctions.at(0).first(v,vCurSample,fRadius);
464 std::vector<dReal> vtemp0, vtemp1; vtemp0.reserve(nMaxDOFForGroup); vtemp1.reserve(nMaxDOFForGroup);
465 std::vector<dReal>::const_iterator itsample = vCurSample.begin();
467 std::vector<dReal>::iterator itdest = v.begin();
469 FOREACHC(itfn, vfunctions) {
470 vtemp1.resize(itfn->second);
472 std::copy(itsample,itsample+itfn->second,itdest);
475 std::copy(itsample,itsample+itfn->second,vtemp1.begin());
476 if( !itfn->first(vtemp0,vtemp1,fRadius) ) {
479 fRadius -= vdistfunctions[ifn].first(vtemp0,vtemp1);
480 std::copy(vtemp0.begin(),vtemp0.end(),itdest);
482 itdest += itfn->second;
483 itsample += itfn->second;
490 void CallSetStateFns(
const std::vector< std::pair<PlannerBase::PlannerParameters::SetStateFn, int> >& vfunctions,
int nDOF,
int nMaxDOFForGroup,
const std::vector<dReal>& v)
492 if( vfunctions.size() == 1 ) {
493 return vfunctions.at(0).first(v);
496 std::vector<dReal> vtemp; vtemp.reserve(nMaxDOFForGroup);
498 std::vector<dReal>::const_iterator itsrc = v.begin();
499 FOREACHC(itfn, vfunctions) {
500 vtemp.resize(itfn->second);
501 std::copy(itsrc,itsrc+itfn->second,vtemp.begin());
503 itsrc += itfn->second;
508 void CallGetStateFns(
const std::vector< std::pair<PlannerBase::PlannerParameters::GetStateFn, int> >& vfunctions,
int nDOF,
int nMaxDOFForGroup, std::vector<dReal>& v)
510 if( vfunctions.size() == 1 ) {
511 vfunctions.at(0).first(v);
514 std::vector<dReal> vtemp; vtemp.reserve(nMaxDOFForGroup);
516 std::vector<dReal>::iterator itdest = v.begin();
517 FOREACHC(itfn, vfunctions) {
519 std::copy(vtemp.begin(),vtemp.end(),itdest);
520 itdest += itfn->second;
525 bool _CallNeighStateFns(
const std::vector< std::pair<PlannerBase::PlannerParameters::NeighStateFn, int> >& vfunctions,
int nDOF,
int nMaxDOFForGroup, std::vector<dReal>& v,
const std::vector<dReal>& vdelta,
int fromgoal)
527 if( vfunctions.size() == 1 ) {
528 return vfunctions.at(0).first(v,vdelta,fromgoal);
533 std::vector<dReal> vtemp0, vtemp1; vtemp0.reserve(nMaxDOFForGroup); vtemp1.reserve(nMaxDOFForGroup);
534 std::vector<dReal>::const_iterator itdelta = vdelta.begin();
535 std::vector<dReal>::iterator itdest = v.begin();
536 FOREACHC(itfn, vfunctions) {
537 vtemp0.resize(itfn->second);
538 std::copy(itdest,itdest+itfn->second,vtemp0.begin());
539 vtemp1.resize(itfn->second);
540 std::copy(itdelta,itdelta+itfn->second,vtemp1.begin());
541 if( !itfn->first(vtemp0,vtemp1,fromgoal) ) {
544 std::copy(vtemp0.begin(),vtemp0.end(),itdest);
545 itdest += itfn->second;
546 itdelta += itfn->second;
554 using namespace planningutils;
556 std::vector< std::pair<DiffStateFn, int> > diffstatefns(spec.
_vgroups.size());
557 std::vector< std::pair<DistMetricFn, int> > distmetricfns(spec.
_vgroups.size());
558 std::vector< std::pair<SampleFn, int> > samplefns(spec.
_vgroups.size());
559 std::vector< std::pair<SampleNeighFn, int> > sampleneighfns(spec.
_vgroups.size());
560 std::vector< std::pair<SetStateFn, int> > setstatefns(spec.
_vgroups.size());
561 std::vector< std::pair<GetStateFn, int> > getstatefns(spec.
_vgroups.size());
562 std::vector< std::pair<NeighStateFn, int> > neighstatefns(spec.
_vgroups.size());
563 std::vector<dReal> vConfigLowerLimit(spec.
GetDOF()), vConfigUpperLimit(spec.
GetDOF()), vConfigVelocityLimit(spec.
GetDOF()), vConfigAccelerationLimit(spec.
GetDOF()), vConfigResolution(spec.
GetDOF()), v0, v1;
564 std::list<KinBodyPtr> listCheckCollisions;
566 stringstream ss, ssout;
568 int nMaxDOFForGroup = 0;
569 std::vector< std::pair<int, int> > vgroupoffsets(spec.
_vgroups.size());
570 for(
size_t igroup = 0; igroup < spec.
_vgroups.size(); ++igroup) {
571 vgroupoffsets[igroup].first = spec.
_vgroups[igroup].offset;
572 vgroupoffsets[igroup].second = igroup;
573 nMaxDOFForGroup = max(nMaxDOFForGroup,spec.
_vgroups[igroup].dof);
575 std::sort(vgroupoffsets.begin(),vgroupoffsets.end());
576 for(
size_t igroup = 0; igroup < spec.
_vgroups.size(); ++igroup) {
578 int isavegroup = vgroupoffsets[igroup].second;
579 if( g.
name.size() >= 12 && g.
name.substr(0,12) ==
"joint_values" ) {
580 ss.clear(); ss.str(g.
name.substr(12));
583 KinBodyPtr pbody = penv->GetKinBody(bodyname);
585 std::vector<int> dofindices((istream_iterator<int>(ss)), istream_iterator<int>());
586 if( dofindices.size() == 0 ) {
589 std::vector<dReal> vweights2;
590 pbody->GetDOFWeights(vweights2, dofindices);
591 FOREACH(itf,vweights2) {
595 diffstatefns[isavegroup].second = g.
dof;
597 distmetricfns[isavegroup].second = g.
dof;
601 ss.clear(); ss.str(
""); ss <<
"SetDOFs ";
602 FOREACHC(itindex,dofindices) {
603 ss << *itindex <<
" ";
605 if( !pconfigsampler->SendCommand(ssout,ss) ) {
609 boost::shared_ptr<SimpleNeighborhoodSampler> defaultsamplefn(
new SimpleNeighborhoodSampler(pconfigsampler,distmetricfns[isavegroup].first));
610 samplefns[isavegroup].first = boost::bind(&SimpleNeighborhoodSampler::Sample,defaultsamplefn,_1);
611 samplefns[isavegroup].second = g.
dof;
612 sampleneighfns[isavegroup].first = boost::bind(&SimpleNeighborhoodSampler::Sample,defaultsamplefn,_1,_2,_3);
613 sampleneighfns[isavegroup].second = g.
dof;
616 setstatefns[isavegroup].second = g.
dof;
618 getstatefns[isavegroup].second = g.
dof;
619 neighstatefns[isavegroup].first =
addstates;
620 neighstatefns[isavegroup].second = g.
dof;
621 pbody->GetDOFLimits(v0,v1,dofindices);
622 std::copy(v0.begin(),v0.end(), vConfigLowerLimit.begin()+g.
offset);
623 std::copy(v1.begin(),v1.end(), vConfigUpperLimit.begin()+g.
offset);
624 pbody->GetDOFVelocityLimits(v0,dofindices);
625 std::copy(v0.begin(),v0.end(), vConfigVelocityLimit.begin()+g.
offset);
626 pbody->GetDOFAccelerationLimits(v0,dofindices);
627 std::copy(v0.begin(),v0.end(), vConfigAccelerationLimit.begin()+g.
offset);
628 pbody->GetDOFResolutions(v0,dofindices);
629 std::copy(v0.begin(),v0.end(),vConfigResolution.begin()+g.
offset);
630 if( find(listCheckCollisions.begin(),listCheckCollisions.end(),pbody) == listCheckCollisions.end() ) {
631 listCheckCollisions.push_back(pbody);
649 _sampleneighfn = boost::bind(
_CallSampleNeighFns,sampleneighfns, distmetricfns, spec.
GetDOF(), nMaxDOFForGroup, _1, _2, _3);
653 boost::shared_ptr<LineCollisionConstraint> pcollision(
new LineCollisionConstraint(listCheckCollisions,
true));
654 _checkpathconstraintsfn = boost::bind(&LineCollisionConstraint::Check,pcollision,
PlannerParametersWeakPtr(shared_parameters()), _1, _2, _3, _4);
655 _vConfigLowerLimit.swap(vConfigLowerLimit);
656 _vConfigUpperLimit.swap(vConfigUpperLimit);
657 _vConfigVelocityLimit.swap(vConfigVelocityLimit);
658 _vConfigAccelerationLimit.swap(vConfigAccelerationLimit);
659 _vConfigResolution.swap(vConfigResolution);
660 _configurationspecification = spec;
661 _getstatefn(vinitialconfig);
671 if( _vConfigVelocityLimit.size() > 0 ) {
674 if( _vConfigAccelerationLimit.size() > 0 ) {
679 OPENRAVE_ASSERT_OP(_nMaxIterations,>=,0);
682 vector<dReal> vstate;
683 if( !!_getstatefn ) {
685 OPENRAVE_ASSERT_OP(vstate.size(),==,(size_t)GetDOF());
687 if( !!_setstatefn ) {
697 if( !!_distmetricfn ) {
698 dReal dist = _distmetricfn(vstate,vstate);
701 if( !!_checkpathconstraintsfn ) {
704 if( !!_neighstatefn && vstate.size() > 0 ) {
705 vector<dReal> vstate2 = vstate;
706 vector<dReal> vzeros(vstate.size());
707 _neighstatefn(vstate2,vzeros,0);
708 dReal dist = _distmetricfn(vstate,vstate2);
711 if( !!_diffstatefn && vstate.size() > 0 ) {
712 vector<dReal> vstate2=vstate;
713 _diffstatefn(vstate2,vstate);
714 OPENRAVE_ASSERT_OP(vstate2.size(),==,(size_t)GetDOF());
730 planner->__listRegisteredCallbacks.erase(_iterator);
747 RAVELOG_WARN(str(boost::format(
"using default planner parameters structure to de-serialize parameters data inside %s, information might be lost!! Please define a InitPlan(robot,stream) function!\n")%
GetXMLId()));
749 isParameters >> *localparams;
750 localparams->Validate();
757 pdata->_iterator = __listRegisteredCallbacks.insert(__listRegisteredCallbacks.end(),pdata);
775 list<UserDataPtr> listhandles;
776 FOREACHC(it,__listRegisteredCallbacks) {
779 listhandles.push_back(planner->RegisterPlanCallback(pitdata->_callbackfn));
785 params->_sExtraParameters +=
GetParameters()->_sPostProcessingParameters;
786 params->_sPostProcessingPlanner =
"";
787 params->_sPostProcessingParameters =
"";
788 params->_nMaxIterations = 0;
789 if( planner->InitPlan(probot, params) ) {
790 return planner->PlanPath(ptraj);
799 FOREACHC(it,__listRegisteredCallbacks) {