openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
planner.cpp
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2012 Rosen Diankov (rosen.diankov@gmail.com)
3 //
4 // This file is part of OpenRAVE.
5 // OpenRAVE is free software: you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation, either version 3 of the License, or
8 // at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
17 #include "libopenrave.h"
18 
19 #include <openrave/planningutils.h>
20 
21 namespace OpenRAVE {
22 
23 static std::string s_linearsmoother = "linearsmoother"; //"shortcut_linear";
24 
25 std::istream& operator>>(std::istream& I, PlannerBase::PlannerParameters& pp)
26 {
27  if( !!I) {
28  stringbuf buf;
29  stringstream::streampos pos = I.tellg();
30  I.get(buf, 0); // get all the data, yes this is inefficient, not sure if there anyway to search in streams
31 
32  string pbuf = buf.str();
33  const char* p = strcasestr(pbuf.c_str(), "</PlannerParameters>");
34  int ppsize=-1;
35  if( p != NULL ) {
36  I.clear();
37  ppsize=(p-pbuf.c_str())+20;
38  I.seekg((size_t)pos+ppsize);
39  }
40  else {
41  throw OPENRAVE_EXCEPTION_FORMAT("error, failed to find </PlannerParameters> in %s",buf.str(),ORE_InvalidArguments);
42  }
43  pp._plannerparametersdepth = 0;
45  }
46 
47  return I;
48 }
49 
50 void subtractstates(std::vector<dReal>& q1, const std::vector<dReal>& q2)
51 {
52  BOOST_ASSERT(q1.size()==q2.size());
53  for(size_t i = 0; i < q1.size(); ++i) {
54  q1[i] -= q2[i];
55  }
56 }
57 
58 bool addstates(std::vector<dReal>& q, const std::vector<dReal>& qdelta, int fromgoal)
59 {
60  BOOST_ASSERT(q.size()==qdelta.size());
61  for(size_t i = 0; i < q.size(); ++i) {
62  q[i] += qdelta[i];
63  }
64  return true;
65 }
66 
68 {
69  BOOST_ASSERT(!!_params->_setstatefn);
70  _params->_getstatefn(_values);
71  OPENRAVE_ASSERT_OP((int)_values.size(),==,_params->GetDOF());
72 }
73 
75 {
76  _Restore();
77 }
78 
80 {
81  _Restore();
82 }
83 
84 void PlannerBase::PlannerParameters::StateSaver::_Restore()
85 {
86  _params->_setstatefn(_values);
87 }
88 
90 {
93  //_sPostProcessingParameters ="<_nmaxiterations>100</_nmaxiterations><_postprocessing planner=\"lineartrajectoryretimer\"></_postprocessing>";
94  _sPostProcessingParameters ="<_nmaxiterations>20</_nmaxiterations><_postprocessing planner=\"parabolicsmoother\"><_nmaxiterations>100</_nmaxiterations></_postprocessing>";
95  _vXMLParameters.reserve(20);
96  _vXMLParameters.push_back("configuration");
97  _vXMLParameters.push_back("_vinitialconfig");
98  _vXMLParameters.push_back("_vgoalconfig");
99  _vXMLParameters.push_back("_vconfiglowerlimit");
100  _vXMLParameters.push_back("_vconfigupperlimit");
101  _vXMLParameters.push_back("_vconfigvelocitylimit");
102  _vXMLParameters.push_back("_vconfigaccelerationlimit");
103  _vXMLParameters.push_back("_vconfigresolution");
104  _vXMLParameters.push_back("_nmaxiterations");
105  _vXMLParameters.push_back("_fsteplength");
106  _vXMLParameters.push_back("_postprocessing");
107 }
108 
110 {
111  BOOST_ASSERT(0);
112 }
113 
115 {
116  // reset
117  _costfn = r._costfn;
118  _goalfn = r._goalfn;
119  _distmetricfn = r._distmetricfn;
120  _checkpathconstraintsfn = r._checkpathconstraintsfn;
121  _samplefn = r._samplefn;
122  _sampleneighfn = r._sampleneighfn;
123  _samplegoalfn = r._samplegoalfn;
124  _sampleinitialfn = r._sampleinitialfn;
125  _setstatefn = r._setstatefn;
126  _getstatefn = r._getstatefn;
127  _diffstatefn = r._diffstatefn;
128  _neighstatefn = r._neighstatefn;
129 
130  vinitialconfig.resize(0);
131  vgoalconfig.resize(0);
132  _configurationspecification = ConfigurationSpecification();
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);
141  _nMaxIterations = 0;
142  _fStepLength = 0.04f;
143  _plannerparametersdepth = 0;
144 
145  // transfer data
146  std::stringstream ss;
147  ss << std::setprecision(std::numeric_limits<dReal>::digits10+1);
148  ss << r;
149  ss >> *this;
150  return *this;
151 }
152 
153 void PlannerBase::PlannerParameters::copy(boost::shared_ptr<PlannerParameters const> r)
154 {
155  *this = *r;
156 }
157 
158 bool PlannerBase::PlannerParameters::serialize(std::ostream& O, int options) const
159 {
160  O << _configurationspecification << endl;
161  O << "<_vinitialconfig>";
162  FOREACHC(it, vinitialconfig) {
163  O << *it << " ";
164  }
165  O << "</_vinitialconfig>" << endl;
166  O << "<_vgoalconfig>";
167  FOREACHC(it, vgoalconfig) {
168  O << *it << " ";
169  }
170  O << "</_vgoalconfig>" << endl;
171  O << "<_vconfiglowerlimit>";
172  FOREACHC(it, _vConfigLowerLimit) {
173  O << *it << " ";
174  }
175  O << "</_vconfiglowerlimit>" << endl;
176  O << "<_vconfigupperlimit>";
177  FOREACHC(it, _vConfigUpperLimit) {
178  O << *it << " ";
179  }
180  O << "</_vconfigupperlimit>" << endl;
181  O << "<_vconfigvelocitylimit>";
182  FOREACHC(it, _vConfigVelocityLimit) {
183  O << *it << " ";
184  }
185  O << "</_vconfigvelocitylimit>" << endl;
186  O << "<_vconfigaccelerationlimit>";
187  FOREACHC(it, _vConfigAccelerationLimit) {
188  O << *it << " ";
189  }
190  O << "</_vconfigaccelerationlimit>" << endl;
191  O << "<_vconfigresolution>";
192  FOREACHC(it, _vConfigResolution) {
193  O << *it << " ";
194  }
195  O << "</_vconfigresolution>" << endl;
196 
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;
202  }
203  return !!O;
204 }
205 
207 {
208  _ss.str(""); // have to clear the string
209  if( !!__pcurreader ) {
210  if( __pcurreader->startElement(name, atts) == PE_Support ) {
211  return PE_Support;
212  }
213  return PE_Ignore;
214  }
215 
216  if( __processingtag.size() > 0 ) {
217  return PE_Ignore;
218  }
219  if( name=="plannerparameters" ) {
220  _plannerparametersdepth++;
221  return PE_Support;
222  }
223 
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;
231  }
232  }
233  __pcurreader.reset(new DummyXMLReader(name,GetXMLId(),_sslocal));
234  return PE_Support;
235  }
236 
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 << "\" ";
242  }
243  *_sslocal << ">" << endl;
244  __pcurreader.reset(new DummyXMLReader(name,GetXMLId(),_sslocal));
245  return PE_Support;
246  }
247 
248  if( name == "configuration" ) {
249  __pcurreader.reset(new ConfigurationSpecification::Reader(_configurationspecification));
250  return PE_Support;
251  }
252 
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;
256  return PE_Support;
257  }
258  return PE_Pass;
259 }
260 
261 bool PlannerBase::PlannerParameters::endElement(const std::string& name)
262 {
263  if( !!__pcurreader ) {
264  if( __pcurreader->endElement(name) ) {
265  boost::shared_ptr<DummyXMLReader> pdummy = boost::dynamic_pointer_cast<DummyXMLReader>(__pcurreader);
266  if( !!pdummy ) {
267  if( pdummy->GetFieldName() == "_postprocessing" ) {
268  _sPostProcessingParameters = _sslocal->str();
269  _sslocal.reset();
270  }
271  else {
272  *_sslocal << "</" << name << ">" << endl;
273  _sExtraParameters += _sslocal->str();
274  _sslocal.reset();
275  }
276  }
277  __pcurreader.reset();
278  }
279  }
280  else if( name == "plannerparameters" ) {
281  return --_plannerparametersdepth < 0;
282  }
283  else if( __processingtag.size() > 0 ) {
284  if( name == "_vinitialconfig") {
285  vinitialconfig = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
286  }
287  else if( name == "_vgoalconfig") {
288  vgoalconfig = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
289  }
290  else if( name == "_vconfiglowerlimit") {
291  _vConfigLowerLimit = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
292  }
293  else if( name == "_vconfigupperlimit") {
294  _vConfigUpperLimit = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
295  }
296  else if( name == "_vconfigresolution") {
297  _vConfigResolution = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
298  }
299  else if( name == "_vconfigvelocitylimit") {
300  _vConfigVelocityLimit = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
301  }
302  else if( name == "_vconfigaccelerationlimit") {
303  _vConfigAccelerationLimit = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
304  }
305  else if( name == "_nmaxiterations") {
306  _ss >> _nMaxIterations;
307  }
308  else if( name == "_fsteplength") {
309  _ss >> _fStepLength;
310  }
311  if( name !=__processingtag ) {
312  RAVELOG_WARN(str(boost::format("invalid tag %s!=%s\n")%name%__processingtag));
313  }
314  __processingtag = "";
315  return false;
316  }
317 
318  return false;
319 }
320 
322 {
323  if( !!__pcurreader ) {
324  __pcurreader->characters(ch);
325  }
326  else {
327  _ss.clear();
328  _ss << ch;
329  }
330 }
331 
332 std::ostream& operator<<(std::ostream& O, const PlannerBase::PlannerParameters& v)
333 {
334  O << "<" << v.GetXMLId() << ">" << endl;
335  v.serialize(O);
336  O << "</" << v.GetXMLId() << ">" << endl;
337  return O;
338 }
339 
341 {
342  // check if any of the links affected by the dofs beside the base link are static
343  FOREACHC(itlink, robot->GetLinks()) {
344  if( (*itlink)->IsStatic() ) {
345  FOREACHC(itdof, robot->GetActiveDOFIndices()) {
346  KinBody::JointPtr pjoint = robot->GetJointFromDOFIndex(*itdof);
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);
348  }
349  }
350  }
351 
352  using namespace planningutils;
353  _distmetricfn = boost::bind(&SimpleDistanceMetric::Eval,boost::shared_ptr<SimpleDistanceMetric>(new SimpleDistanceMetric(robot)),_1,_2);
354  SpaceSamplerBasePtr pconfigsampler = RaveCreateSpaceSampler(robot->GetEnv(),str(boost::format("robotconfiguration %s")%robot->GetName()));
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);
358  _setstatefn = boost::bind(&RobotBase::SetActiveDOFValues,robot,_1,false);
359  _getstatefn = boost::bind(&RobotBase::GetActiveDOFValues,robot,_1);
360  _diffstatefn = boost::bind(&RobotBase::SubtractActiveDOFValues,robot,_1,_2);
361  _neighstatefn = addstates; // probably ok...
362 
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);
366 
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();
373 }
374 
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)
376 {
377  if( vfunctions.size() == 1 ) {
378  vfunctions.at(0).first(v0,v1);
379  }
380  else {
381  OPENRAVE_ASSERT_OP((int)v0.size(),==,nDOF);
382  OPENRAVE_ASSERT_OP((int)v1.size(),==,nDOF);
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);
392  // copy result back to itsource0
393  std::copy(vtemp0.begin(),vtemp0.end(),itsource0);
394  itsource0 += itfn->second;
395  itsource1 += itfn->second;
396  }
397  }
398 }
399 
402 dReal _EvalJointDOFDistanceMetric(const PlannerBase::PlannerParameters::DiffStateFn& difffn, const std::vector<dReal>&c0, const std::vector<dReal>&c1, const std::vector<dReal>& vweights2)
403 {
404  std::vector<dReal> c = c0;
405  difffn(c,c1);
406  dReal dist = 0;
407  for(size_t i=0; i < c.size(); i++) {
408  dist += vweights2.at(i)*c.at(i)*c.at(i);
409  }
410  return RaveSqrt(dist);
411 }
412 
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)
414 {
415  if( vfunctions.size() == 1 ) {
416  return vfunctions.at(0).first(v0, v1);
417  }
418  else {
419  OPENRAVE_ASSERT_OP((int)v0.size(),==,nDOF);
420  OPENRAVE_ASSERT_OP((int)v1.size(),==,nDOF);
421  std::vector<dReal> vtemp0, vtemp1; vtemp0.reserve(nMaxDOFForGroup); vtemp1.reserve(nMaxDOFForGroup);
422  std::vector<dReal>::const_iterator itsource0 = v0.begin(), itsource1 = v1.begin();
423  dReal f = 0;
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;
432  }
433  return f;
434  }
435 }
436 
437 bool _CallSampleFns(const std::vector< std::pair<PlannerBase::PlannerParameters::SampleFn, int> >& vfunctions, int nDOF, int nMaxDOFForGroup, std::vector<dReal>& v)
438 {
439  if( vfunctions.size() == 1 ) {
440  return vfunctions.at(0).first(v);
441  }
442  else {
443  std::vector<dReal> vtemp; vtemp.reserve(nMaxDOFForGroup);
444  v.resize(nDOF);
445  std::vector<dReal>::iterator itdest = v.begin();
446  FOREACHC(itfn, vfunctions) {
447  if( !itfn->first(vtemp) ) {
448  return false;
449  }
450  std::copy(vtemp.begin(),vtemp.end(),itdest);
451  itdest += itfn->second;
452  }
453  return true;
454  }
455 }
456 
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)
458 {
459  if( vfunctions.size() == 1 ) {
460  return vfunctions.at(0).first(v,vCurSample,fRadius);
461  }
462  else {
463  OPENRAVE_ASSERT_OP((int)vCurSample.size(),==,nDOF);
464  std::vector<dReal> vtemp0, vtemp1; vtemp0.reserve(nMaxDOFForGroup); vtemp1.reserve(nMaxDOFForGroup);
465  std::vector<dReal>::const_iterator itsample = vCurSample.begin();
466  v.resize(nDOF);
467  std::vector<dReal>::iterator itdest = v.begin();
468  int ifn = 0;
469  FOREACHC(itfn, vfunctions) {
470  vtemp1.resize(itfn->second);
471  if( fRadius <= 0 ) {
472  std::copy(itsample,itsample+itfn->second,itdest);
473  }
474  else {
475  std::copy(itsample,itsample+itfn->second,vtemp1.begin());
476  if( !itfn->first(vtemp0,vtemp1,fRadius) ) {
477  return false;
478  }
479  fRadius -= vdistfunctions[ifn].first(vtemp0,vtemp1);
480  std::copy(vtemp0.begin(),vtemp0.end(),itdest);
481  }
482  itdest += itfn->second;
483  itsample += itfn->second;
484  ++ifn;
485  }
486  return true;
487  }
488 }
489 
490 void CallSetStateFns(const std::vector< std::pair<PlannerBase::PlannerParameters::SetStateFn, int> >& vfunctions, int nDOF, int nMaxDOFForGroup, const std::vector<dReal>& v)
491 {
492  if( vfunctions.size() == 1 ) {
493  return vfunctions.at(0).first(v);
494  }
495  else {
496  std::vector<dReal> vtemp; vtemp.reserve(nMaxDOFForGroup);
497  OPENRAVE_ASSERT_OP((int)v.size(),==,nDOF);
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());
502  itfn->first(vtemp);
503  itsrc += itfn->second;
504  }
505  }
506 }
507 
508 void CallGetStateFns(const std::vector< std::pair<PlannerBase::PlannerParameters::GetStateFn, int> >& vfunctions, int nDOF, int nMaxDOFForGroup, std::vector<dReal>& v)
509 {
510  if( vfunctions.size() == 1 ) {
511  vfunctions.at(0).first(v);
512  }
513  else {
514  std::vector<dReal> vtemp; vtemp.reserve(nMaxDOFForGroup);
515  v.resize(nDOF);
516  std::vector<dReal>::iterator itdest = v.begin();
517  FOREACHC(itfn, vfunctions) {
518  itfn->first(vtemp);
519  std::copy(vtemp.begin(),vtemp.end(),itdest);
520  itdest += itfn->second;
521  }
522  }
523 }
524 
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)
526 {
527  if( vfunctions.size() == 1 ) {
528  return vfunctions.at(0).first(v,vdelta,fromgoal);
529  }
530  else {
531  OPENRAVE_ASSERT_OP((int)vdelta.size(),==,nDOF);
532  OPENRAVE_ASSERT_OP((int)v.size(),==,nDOF);
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) ) {
542  return false;
543  }
544  std::copy(vtemp0.begin(),vtemp0.end(),itdest);
545  itdest += itfn->second;
546  itdelta += itfn->second;
547  }
548  return true;
549  }
550 }
551 
553 {
554  using namespace planningutils;
555  spec.Validate();
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;
565  string bodyname;
566  stringstream ss, ssout;
567  // order the groups depending on offset
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);
574  }
575  std::sort(vgroupoffsets.begin(),vgroupoffsets.end());
576  for(size_t igroup = 0; igroup < spec._vgroups.size(); ++igroup) {
577  const ConfigurationSpecification::Group& g = spec._vgroups[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));
581  ss >> bodyname;
582  BOOST_ASSERT(!!ss);
583  KinBodyPtr pbody = penv->GetKinBody(bodyname);
584  OPENRAVE_ASSERT_FORMAT(!!pbody,"body %s not found",bodyname,ORE_InvalidArguments);
585  std::vector<int> dofindices((istream_iterator<int>(ss)), istream_iterator<int>());
586  if( dofindices.size() == 0 ) {
587  OPENRAVE_ASSERT_OP((int)dofindices.size(),==,pbody->GetDOF());
588  }
589  std::vector<dReal> vweights2;
590  pbody->GetDOFWeights(vweights2, dofindices);
591  FOREACH(itf,vweights2) {
592  *itf *= *itf;
593  }
594  diffstatefns[isavegroup].first = boost::bind(&KinBody::SubtractDOFValues, pbody, _1, _2, dofindices);
595  diffstatefns[isavegroup].second = g.dof;
596  distmetricfns[isavegroup].first = boost::bind(_EvalJointDOFDistanceMetric, diffstatefns[isavegroup].first, _1, _2, vweights2);
597  distmetricfns[isavegroup].second = g.dof;
598 
599  SpaceSamplerBasePtr pconfigsampler = RaveCreateSpaceSampler(penv,str(boost::format("bodyconfiguration %s")%pbody->GetName()));
600  {
601  ss.clear(); ss.str(""); ss << "SetDOFs ";
602  FOREACHC(itindex,dofindices) {
603  ss << *itindex << " ";
604  }
605  if( !pconfigsampler->SendCommand(ssout,ss) ) {
606  throw OPENRAVE_EXCEPTION_FORMAT("failed to set body %s configuration to %s",pbody->GetName()%ss.str(), ORE_Assert);
607  }
608  }
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;
614  void (KinBody::*setdofvaluesptr)(const std::vector<dReal>&, uint32_t, const std::vector<int>&) = &KinBody::SetDOFValues;
615  setstatefns[isavegroup].first = boost::bind(setdofvaluesptr, pbody, _1, KinBody::CLA_CheckLimits, dofindices);
616  setstatefns[isavegroup].second = g.dof;
617  getstatefns[isavegroup].first = boost::bind(&KinBody::GetDOFValues, pbody, _1, dofindices);
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);
632  }
633  }
634 // else if( g.name.size() >= 16 && g.name.substr(0,16) == "joint_velocities" ) {
635 // }
636 // else if( g.name.size() >= 16 && g.name.substr(0,16) == "affine_transform" ) {
637 // }
638 // else if( g.name.size() >= 17 && g.name.substr(0,17) == "affine_velocities" ) {
639 // }
640 // else if( g.name.size() >= 4 && g.name.substr(0,4) == "grab" ) {
641 // }
642  else {
643  throw OPENRAVE_EXCEPTION_FORMAT("group %s not supported for for planner parameters configuration",g.name,ORE_InvalidArguments);
644  }
645  }
646  _diffstatefn = boost::bind(_CallDiffStateFns,diffstatefns, spec.GetDOF(), nMaxDOFForGroup, _1, _2);
647  _distmetricfn = boost::bind(_CallDistMetricFns,distmetricfns, spec.GetDOF(), nMaxDOFForGroup, _1, _2);
648  _samplefn = boost::bind(_CallSampleFns,samplefns, spec.GetDOF(), nMaxDOFForGroup, _1);
649  _sampleneighfn = boost::bind(_CallSampleNeighFns,sampleneighfns, distmetricfns, spec.GetDOF(), nMaxDOFForGroup, _1, _2, _3);
650  _setstatefn = boost::bind(CallSetStateFns,setstatefns, spec.GetDOF(), nMaxDOFForGroup, _1);
651  _getstatefn = boost::bind(CallGetStateFns,getstatefns, spec.GetDOF(), nMaxDOFForGroup, _1);
652  _neighstatefn = boost::bind(_CallNeighStateFns,neighstatefns, 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);
662 }
663 
665 {
666  OPENRAVE_ASSERT_OP(_configurationspecification.GetDOF(),==,GetDOF());
667  OPENRAVE_ASSERT_OP(vinitialconfig.size()%GetDOF(),==,0);
668  OPENRAVE_ASSERT_OP(vgoalconfig.size()%GetDOF(),==,0);
669  OPENRAVE_ASSERT_OP(_vConfigLowerLimit.size(),==,(size_t)GetDOF());
670  OPENRAVE_ASSERT_OP(_vConfigUpperLimit.size(),==,(size_t)GetDOF());
671  if( _vConfigVelocityLimit.size() > 0 ) {
672  OPENRAVE_ASSERT_OP(_vConfigVelocityLimit.size(),==,(size_t)GetDOF());
673  }
674  if( _vConfigAccelerationLimit.size() > 0 ) {
675  OPENRAVE_ASSERT_OP(_vConfigAccelerationLimit.size(),==,(size_t)GetDOF());
676  }
677  OPENRAVE_ASSERT_OP(_vConfigResolution.size(),==,(size_t)GetDOF());
678  OPENRAVE_ASSERT_OP(_fStepLength,>=,0); // == 0 is valid for auto-steps
679  OPENRAVE_ASSERT_OP(_nMaxIterations,>=,0); // == 0 is valid for auto-iterations
680 
681  // check all stateless functions, which means ie anything but configuration samplers
682  vector<dReal> vstate;
683  if( !!_getstatefn ) {
684  _getstatefn(vstate);
685  OPENRAVE_ASSERT_OP(vstate.size(),==,(size_t)GetDOF());
686  }
687  if( !!_setstatefn ) {
688  // need to save/restore state before calling this function?
689  //_setstatefn();
690  }
691  if( !!_costfn ) {
692  _costfn(vstate);
693  }
694  if( !!_goalfn ) {
695  _goalfn(vstate);
696  }
697  if( !!_distmetricfn ) {
698  dReal dist = _distmetricfn(vstate,vstate);
699  OPENRAVE_ASSERT_OP(dist,<=,10*g_fEpsilon);
700  }
701  if( !!_checkpathconstraintsfn ) {
702  _checkpathconstraintsfn(vstate,vstate,IT_OpenStart,ConfigurationListPtr());
703  }
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);
709  OPENRAVE_ASSERT_OP(dist,<=,10*g_fEpsilon);
710  }
711  if( !!_diffstatefn && vstate.size() > 0 ) {
712  vector<dReal> vstate2=vstate;
713  _diffstatefn(vstate2,vstate);
714  OPENRAVE_ASSERT_OP(vstate2.size(),==,(size_t)GetDOF());
715  }
716 }
717 
719 {
720 }
721 
722 class CustomPlannerCallbackData : public boost::enable_shared_from_this<CustomPlannerCallbackData>, public UserData
723 {
724 public:
725  CustomPlannerCallbackData(const PlannerBase::PlanCallbackFn& callbackfn, PlannerBasePtr planner) : _callbackfn(callbackfn), _plannerweak(planner) {
726  }
728  PlannerBasePtr planner = _plannerweak.lock();
729  if( !!planner ) {
730  planner->__listRegisteredCallbacks.erase(_iterator);
731  }
732  }
733 
736  std::list<UserDataWeakPtr>::iterator _iterator;
737 };
738 
739 typedef boost::shared_ptr<CustomPlannerCallbackData> CustomPlannerCallbackDataPtr;
740 
742 {
743 }
744 
745 bool PlannerBase::InitPlan(RobotBasePtr pbase, std::istream& isParameters)
746 {
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()));
748  boost::shared_ptr<PlannerParameters> localparams(new PlannerParameters());
749  isParameters >> *localparams;
750  localparams->Validate();
751  return InitPlan(pbase,localparams);
752 }
753 
755 {
756  CustomPlannerCallbackDataPtr pdata(new CustomPlannerCallbackData(callbackfn,shared_planner()));
757  pdata->_iterator = __listRegisteredCallbacks.insert(__listRegisteredCallbacks.end(),pdata);
758  return pdata;
759 }
760 
762 {
763  if( GetParameters()->_sPostProcessingPlanner.size() == 0 ) {
764  return PS_HasSolution;
765  }
766  PlannerBasePtr planner = RaveCreatePlanner(GetEnv(), GetParameters()->_sPostProcessingPlanner);
767  if( !planner ) {
769  if( !planner ) {
770  return PS_Failed;
771  }
772  }
773 
774  // transfer the callbacks?
775  list<UserDataPtr> listhandles;
776  FOREACHC(it,__listRegisteredCallbacks) {
777  CustomPlannerCallbackDataPtr pitdata = boost::dynamic_pointer_cast<CustomPlannerCallbackData>(it->lock());
778  if( !!pitdata) {
779  listhandles.push_back(planner->RegisterPlanCallback(pitdata->_callbackfn));
780  }
781  }
782 
784  params->copy(GetParameters());
785  params->_sExtraParameters += GetParameters()->_sPostProcessingParameters;
786  params->_sPostProcessingPlanner = "";
787  params->_sPostProcessingParameters = "";
788  params->_nMaxIterations = 0; // have to reset since path optimizers also use it and new parameters could be in extra parameters
789  if( planner->InitPlan(probot, params) ) {
790  return planner->PlanPath(ptraj);
791  }
792 
793  // do not fall back to a default linear smoother like in the past! that makes behavior unpredictable
794  return PS_Failed;
795 }
796 
798 {
799  FOREACHC(it,__listRegisteredCallbacks) {
800  CustomPlannerCallbackDataPtr pitdata = boost::dynamic_pointer_cast<CustomPlannerCallbackData>(it->lock());
801  if( !!pitdata) {
802  PlannerAction ret = pitdata->_callbackfn(progress);
803  if( ret != PA_None ) {
804  return ret;
805  }
806  }
807  }
808  return PA_None;
809 }
810 
811 }