openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
planningutils.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 #include <boost/lexical_cast.hpp>
19 #include <openrave/planningutils.h>
21 
22 namespace OpenRAVE {
23 namespace planningutils {
24 
25 int JitterActiveDOF(RobotBasePtr robot,int nMaxIterations,dReal fRand,const PlannerBase::PlannerParameters::NeighStateFn& neighstatefn)
26 {
27  RAVELOG_VERBOSE("starting jitter active dof...\n");
28  vector<dReal> curdof, newdof, deltadof, deltadof2;
29  robot->GetActiveDOFValues(curdof);
30  newdof.resize(curdof.size());
31  deltadof.resize(curdof.size(),0);
32  CollisionReport report;
33  CollisionReportPtr preport(&report,utils::null_deleter());
34  bool bCollision = false;
35  bool bConstraint = !!neighstatefn;
36 
37  // have to test with perturbations since very small changes in angles can produce collision inconsistencies
38  boost::array<dReal,3> perturbations = { { 0,1e-5f,-1e-5f}};
39  FOREACH(itperturbation,perturbations) {
40  if( bConstraint ) {
41  FOREACH(it,deltadof) {
42  *it = *itperturbation;
43  }
44  newdof = curdof;
45  if( !neighstatefn(newdof,deltadof,0) ) {
46  robot->SetActiveDOFValues(curdof,KinBody::CLA_CheckLimitsSilent);
47  return -1;
48  }
49  }
50  else {
51  for(size_t i = 0; i < newdof.size(); ++i) {
52  newdof[i] = curdof[i]+*itperturbation;
53  }
54  }
55  robot->SetActiveDOFValues(newdof,KinBody::CLA_CheckLimitsSilent);
56 
57  if(robot->CheckSelfCollision(preport)) {
58  bCollision = true;
59  RAVELOG_DEBUG(str(boost::format("JitterActiveDOFs: self collision: %s!\n")%report.__str__()));
60  break;
61  }
62  if( robot->GetEnv()->CheckCollision(KinBodyConstPtr(robot),preport) ) {
63  bCollision = true;
64  RAVELOG_DEBUG(str(boost::format("JitterActiveDOFs: collision: %s!\n")%report.__str__()));
65  break;
66  }
67  }
68 
69  if( !bCollision || fRand <= 0 ) {
70  // have to restore to initial non-perturbed configuration!
71  robot->SetActiveDOFValues(curdof,KinBody::CLA_Nothing);
72  return -1;
73  }
74 
75  deltadof2.resize(curdof.size(),0);
76  for(int iter = 0; iter < nMaxIterations; ++iter) {
77  for(size_t j = 0; j < newdof.size(); ++j) {
78  deltadof[j] = fRand * (RaveRandomFloat()-0.5f);
79  }
80  bCollision = false;
81  bool bConstraintFailed = false;
82  FOREACH(itperturbation,perturbations) {
83  for(size_t j = 0; j < deltadof.size(); ++j) {
84  deltadof2[j] = deltadof[j] + *itperturbation;
85  }
86  if( bConstraint ) {
87  newdof = curdof;
88  robot->SetActiveDOFValues(newdof,KinBody::CLA_CheckLimitsSilent);
89  if( !neighstatefn(newdof,deltadof2,0) ) {
90  if( *itperturbation != 0 ) {
91  RAVELOG_DEBUG(str(boost::format("constraint function failed, pert=%e\n")%*itperturbation));
92  }
93  bConstraintFailed = true;
94  break;
95  }
96  }
97  else {
98  for(size_t j = 0; j < deltadof.size(); ++j) {
99  newdof[j] = curdof[j] + deltadof2[j];
100  }
101  }
102  robot->SetActiveDOFValues(newdof,KinBody::CLA_CheckLimitsSilent);
103  if(robot->CheckSelfCollision() || robot->GetEnv()->CheckCollision(KinBodyConstPtr(robot)) ) {
104  bCollision = true;
105  break;
106  }
107  }
108  if( !bCollision && !bConstraintFailed ) {
109  // have to restore to non-perturbed configuration!
110  if( bConstraint ) {
111  newdof = curdof;
112  robot->SetActiveDOFValues(newdof,KinBody::CLA_Nothing);
113  if( !neighstatefn(newdof,deltadof,0) ) {
114  RAVELOG_WARN("neighstatefn failed, but previously succeeded\n");
115  continue;
116  }
117  }
118  else {
119  for(size_t j = 0; j < deltadof.size(); ++j) {
120  newdof[j] = curdof[j] + deltadof[j];
121  }
122  }
123  robot->SetActiveDOFValues(newdof,KinBody::CLA_CheckLimitsSilent);
124  return 1;
125  }
126  }
127 
128  return 0;
129 }
130 
131 bool JitterTransform(KinBodyPtr pbody, float fJitter, int nMaxIterations)
132 {
133  RAVELOG_VERBOSE("starting jitter transform...\n");
134 
135  // randomly add small offset to the body until it stops being in collision
136  Transform transorig = pbody->GetTransform();
137  Transform transnew = transorig;
138  int iter = 0;
139  while(pbody->GetEnv()->CheckCollision(KinBodyConstPtr(pbody)) ) {
140  if( iter > nMaxIterations ) {
141  // have to restore to initial non-perturbed transform!
142  pbody->SetTransform(transorig);
143  return false;
144  }
145  if( iter > 0 && fJitter <= 0 ) {
146  // have to restore to initial non-perturbed transform!
147  pbody->SetTransform(transorig);
148  return false;
149  }
150  transnew.trans = transorig.trans + 2.0 * fJitter * Vector(RaveRandomFloat()-0.5f, RaveRandomFloat()-0.5f, RaveRandomFloat()-0.5f);
151  pbody->SetTransform(transnew);
152  ++iter;
153  }
154 
155  return true;
156 }
157 
159 {
160 public:
163  }
164 
166  OPENRAVE_ASSERT_FORMAT0(!!_parameters,"need planner parameters to verify trajectory",ORE_InvalidArguments);
167  OPENRAVE_ASSERT_OP_FORMAT0((int)_parameters->_vConfigLowerLimit.size(), ==, _parameters->GetDOF(), "unexpected size",ORE_InvalidState);
168  OPENRAVE_ASSERT_OP_FORMAT0((int)_parameters->_vConfigUpperLimit.size(), ==, _parameters->GetDOF(), "unexpected size",ORE_InvalidState);
169  OPENRAVE_ASSERT_OP_FORMAT0((int)_parameters->_vConfigResolution.size(), ==, _parameters->GetDOF(), "unexpected size",ORE_InvalidState);
170  }
171 
172  void VerifyTrajectory(TrajectoryBaseConstPtr trajectory, dReal samplingstep)
173  {
174  OPENRAVE_ASSERT_FORMAT0(!!trajectory,"need valid trajectory",ORE_InvalidArguments);
175 
176  ConfigurationSpecification velspec = _parameters->_configurationspecification.ConvertToVelocitySpecification();
177 
178  dReal fresolutionmean = 0;
179  FOREACHC(it,_parameters->_vConfigResolution) {
180  fresolutionmean += *it;
181  }
182  fresolutionmean /= _parameters->_vConfigResolution.size();
183 
184  dReal fthresh = 5e-5f;
185  vector<dReal> deltaq(_parameters->GetDOF(),0);
186  std::vector<dReal> vdata, vdatavel, vdiff;
187  for(size_t ipoint = 0; ipoint < trajectory->GetNumWaypoints(); ++ipoint) {
188  trajectory->GetWaypoint(ipoint,vdata,_parameters->_configurationspecification);
189  trajectory->GetWaypoint(ipoint,vdatavel,velspec);
190  BOOST_ASSERT((int)vdata.size()==_parameters->GetDOF());
191  BOOST_ASSERT((int)vdatavel.size()==_parameters->GetDOF());
192  for(size_t i = 0; i < vdata.size(); ++i) {
193  if( !(vdata[i] >= _parameters->_vConfigLowerLimit[i]-fthresh) || !(vdata[i] <= _parameters->_vConfigUpperLimit[i]+fthresh) ) {
194  throw OPENRAVE_EXCEPTION_FORMAT("limits exceeded configuration %d dof %d: %f in [%f,%f]", ipoint%i%vdata[i]%_parameters->_vConfigLowerLimit[i]%_parameters->_vConfigUpperLimit[i], ORE_InconsistentConstraints);
195  }
196  }
197  for(size_t i = 0; i < _parameters->_vConfigVelocityLimit.size(); ++i) {
198  if( !(RaveFabs(vdatavel.at(i)) <= _parameters->_vConfigVelocityLimit[i]+fthresh) ) { // !(x<=y) necessary for catching NaNs
199  throw OPENRAVE_EXCEPTION_FORMAT("velocity exceeded configuration %d dof %d: %f>%f", ipoint%i%RaveFabs(vdatavel.at(i))%_parameters->_vConfigVelocityLimit[i], ORE_InconsistentConstraints);
200  }
201  }
202  _parameters->_setstatefn(vdata);
203  vector<dReal> newq;
204  _parameters->_getstatefn(newq);
205  BOOST_ASSERT(vdata.size() == newq.size());
206  vdiff = newq;
207  _parameters->_diffstatefn(vdiff,vdata);
208  for(size_t i = 0; i < vdiff.size(); ++i) {
209  if( !(RaveFabs(vdiff.at(i)) <= 0.001 * _parameters->_vConfigResolution[i]) ) {
210  throw OPENRAVE_EXCEPTION_FORMAT("setstate/getstate inconsistent configuration %d dof %d: %f != %f, wrote trajectory to %s",ipoint%i%vdata.at(i)%newq.at(i)%DumpTrajectory(trajectory),ORE_InconsistentConstraints);
211  }
212  }
213  if( !!_parameters->_neighstatefn ) {
214  newq = vdata;
215  if( !_parameters->_neighstatefn(newq,vdiff,0) ) {
216  throw OPENRAVE_EXCEPTION_FORMAT("neighstatefn is rejecting configuration %d, wrote trajectory %s",ipoint%DumpTrajectory(trajectory),ORE_InconsistentConstraints);
217  }
218  dReal fdist = _parameters->_distmetricfn(newq,vdata);
219  OPENRAVE_ASSERT_OP_FORMAT(fdist,<=,0.01 * fresolutionmean, "neighstatefn is rejecting configuration %d, wrote trajectory %s",ipoint%DumpTrajectory(trajectory),ORE_InconsistentConstraints);
220  }
221  }
222 
223  if( !!_parameters->_checkpathconstraintsfn && trajectory->GetNumWaypoints() >= 2 ) {
224 
225  if( trajectory->GetDuration() > 0 && samplingstep > 0 ) {
226  // use sampling and check segment constraints
227  std::vector<dReal> vprevdata, vprevdatavel;
229  trajectory->Sample(vprevdata,0,_parameters->_configurationspecification);
230  trajectory->Sample(vprevdatavel,0,velspec);
231  for(dReal ftime = 0; ftime < trajectory->GetDuration(); ftime += samplingstep ) {
232  configs->clear();
233  trajectory->Sample(vdata,ftime+samplingstep,_parameters->_configurationspecification);
234  trajectory->Sample(vdatavel,ftime+samplingstep,velspec);
235  vdiff = vdata;
236  _parameters->_diffstatefn(vdiff,vprevdata);
237  for(size_t i = 0; i < _parameters->_vConfigVelocityLimit.size(); ++i) {
238  dReal velthresh = _parameters->_vConfigVelocityLimit.at(i)*samplingstep+fthresh;
239  OPENRAVE_ASSERT_OP_FORMAT(RaveFabs(vdiff.at(i)), <=, velthresh, "time %fs-%fs, dof %d traveled %f, but maxvelocity only allows %f, wrote trajectory to %s",ftime%(ftime+samplingstep)%i%RaveFabs(vdiff.at(i))%velthresh%DumpTrajectory(trajectory),ORE_InconsistentConstraints);
240  }
241  if( !_parameters->_checkpathconstraintsfn(vprevdata,vdata,IT_Closed, configs) ) {
242  throw OPENRAVE_EXCEPTION_FORMAT("time %fs-%fs, checkpathconstraintsfn failed, wrote trajectory to %s",ftime%(ftime+samplingstep)%DumpTrajectory(trajectory),ORE_InconsistentConstraints);
243  }
244  PlannerBase::ConfigurationList::iterator itprevconfig = configs->begin();
245  PlannerBase::ConfigurationList::iterator itcurconfig = ++configs->begin();
246  for(; itcurconfig != configs->end(); ++itcurconfig) {
247  BOOST_ASSERT( (int)itcurconfig->size() == _parameters->GetDOF());
248  for(size_t i = 0; i < itcurconfig->size(); ++i) {
249  deltaq.at(i) = itcurconfig->at(i) - itprevconfig->at(i);
250  }
251  _parameters->_setstatefn(*itprevconfig);
252  vector<dReal> vtemp = *itprevconfig;
253  if( !_parameters->_neighstatefn(vtemp,deltaq,0) ) {
254  throw OPENRAVE_EXCEPTION_FORMAT("time %fs-%fs, neighstatefn is rejecting configurations from checkpathconstraintsfn, wrote trajectory to %s",ftime%(ftime+samplingstep)%DumpTrajectory(trajectory),ORE_InconsistentConstraints);
255  }
256  else {
257  dReal fprevdist = _parameters->_distmetricfn(*itprevconfig,vtemp);
258  dReal fcurdist = _parameters->_distmetricfn(*itcurconfig,vtemp);
259  OPENRAVE_ASSERT_OP_FORMAT(fprevdist, >, fcurdist, "time %fs-%fs, neightstatefn returned a configuration closer to the previous configuration %f than the expected current %f, wrote trajectory to %s",ftime%(ftime+samplingstep)%fprevdist%fcurdist%DumpTrajectory(trajectory), ORE_InconsistentConstraints);
260  }
261  itprevconfig=itcurconfig;
262  }
263  vprevdata=vdata;
264  vprevdatavel=vdatavel;
265  }
266  }
267  else {
268  for(size_t i = 0; i < trajectory->GetNumWaypoints(); ++i) {
269  trajectory->GetWaypoint(i,vdata,_parameters->_configurationspecification);
270  if( !_parameters->_checkpathconstraintsfn(vdata,vdata,IT_OpenStart, PlannerBase::ConfigurationListPtr()) ) {
271  throw OPENRAVE_EXCEPTION_FORMAT("checkpathconstraintsfn failed at %d, wrote trajectory to %s",i%DumpTrajectory(trajectory),ORE_InconsistentConstraints);
272  }
273  }
274  }
275  }
276  }
277 
279  {
280  string filename = str(boost::format("%s/failedtrajectory%d.xml")%RaveGetHomeDirectory()%(RaveRandomInt()%1000));
281  ofstream f(filename.c_str());
282  f << std::setprecision(std::numeric_limits<dReal>::digits10+1);
283  trajectory->serialize(f);
284  return filename;
285  }
286 
287 protected:
289 };
290 
292 {
293  EnvironmentMutex::scoped_lock lockenv(trajectory->GetEnv()->GetMutex());
294  if( !parameters ) {
296  newparams->SetConfigurationSpecification(trajectory->GetEnv(), trajectory->GetConfigurationSpecification().GetTimeDerivativeSpecification(0));
297  parameters = newparams;
298  }
299  TrajectoryVerifier v(parameters);
300  v.VerifyTrajectory(trajectory,samplingstep);
301 }
302 
303 PlannerStatus _PlanActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBasePtr probot, bool hastimestamps, dReal fmaxvelmult, dReal fmaxaccelmult, const std::string& plannername, bool bsmooth, const std::string& plannerparameters)
304 {
305  if( traj->GetNumWaypoints() == 1 ) {
306  // don't need velocities, but should at least add a time group
307  ConfigurationSpecification spec = traj->GetConfigurationSpecification();
308  spec.AddDeltaTimeGroup();
309  vector<dReal> data;
310  traj->GetWaypoints(0,traj->GetNumWaypoints(),data,spec);
311  traj->Init(spec);
312  traj->Insert(0,data);
313  return PS_HasSolution;
314  }
315 
316  EnvironmentBasePtr env = traj->GetEnv();
317  EnvironmentMutex::scoped_lock lockenv(env->GetMutex());
318  CollisionOptionsStateSaver optionstate(env->GetCollisionChecker(),env->GetCollisionChecker()->GetCollisionOptions()|CO_ActiveDOFs,false);
319  PlannerBasePtr planner = RaveCreatePlanner(env,plannername.size() > 0 ? plannername : string("parabolicsmoother"));
321  params->SetRobotActiveJoints(probot);
322  FOREACH(it,params->_vConfigVelocityLimit) {
323  *it *= fmaxvelmult;
324  }
325  FOREACH(it,params->_vConfigAccelerationLimit) {
326  *it *= fmaxaccelmult;
327  }
328  if( !bsmooth ) {
329  params->_setstatefn.clear();
330  params->_checkpathconstraintsfn.clear();
331  }
332 
333  params->_sPostProcessingPlanner = ""; // have to turn off the second post processing stage
334  params->_hastimestamps = hastimestamps;
335  params->_sExtraParameters += plannerparameters;
336  if( !planner->InitPlan(probot,params) ) {
337  return PS_Failed;
338  }
339  if( planner->PlanPath(traj) != PS_HasSolution ) {
340  return PS_Failed;
341  }
342 
343  if( bsmooth && (RaveGetDebugLevel() & Level_VerifyPlans) ) {
344  RobotBase::RobotStateSaver saver(probot);
345  planningutils::VerifyTrajectory(params,traj);
346  }
347  return PS_HasSolution;
348 }
349 
350 ActiveDOFTrajectorySmoother::ActiveDOFTrajectorySmoother(RobotBasePtr robot, const std::string& _plannername, const std::string& plannerparameters)
351 {
352  std::string plannername = _plannername.size() > 0 ? _plannername : "parabolicsmoother";
353  _robot = robot;
354  EnvironmentMutex::scoped_lock lockenv(robot->GetEnv()->GetMutex());
355  _planner = RaveCreatePlanner(robot->GetEnv(),plannername);
357  params->SetRobotActiveJoints(_robot);
358  params->_sPostProcessingPlanner = ""; // have to turn off the second post processing stage
359  params->_hastimestamps = false;
360  params->_sExtraParameters += plannerparameters;
361  if( !_planner->InitPlan(_robot,params) ) {
362  throw OPENRAVE_EXCEPTION_FORMAT("failed to init planner %s with robot %s", plannername%_robot->GetName(), ORE_InvalidArguments);
363  }
364  _parameters=params; // necessary because SetRobotActiveJoints builds functions that hold weak_ptr to the parameters
365 }
366 
368 {
369  if( traj->GetNumWaypoints() == 1 ) {
370  // don't need velocities, but should at least add a time group
371  ConfigurationSpecification spec = traj->GetConfigurationSpecification();
372  spec.AddDeltaTimeGroup();
373  vector<dReal> data;
374  traj->GetWaypoints(0,traj->GetNumWaypoints(),data,spec);
375  traj->Init(spec);
376  traj->Insert(0,data);
377  return PS_HasSolution;
378  }
379 
380  EnvironmentBasePtr env = traj->GetEnv();
381  CollisionOptionsStateSaver optionstate(env->GetCollisionChecker(),env->GetCollisionChecker()->GetCollisionOptions()|CO_ActiveDOFs,false);
382  PlannerStatus status = _planner->PlanPath(traj);
383  if( status & PS_HasSolution ) {
386  planningutils::VerifyTrajectory(_planner->GetParameters(),traj);
387  }
388  }
389  return status;
390 }
391 
392 ActiveDOFTrajectoryRetimer::ActiveDOFTrajectoryRetimer(RobotBasePtr robot, const std::string& _plannername, const std::string& plannerparameters)
393 {
394  std::string plannername = _plannername.size() > 0 ? _plannername : "parabolicretimer";
395  _robot = robot;
396  EnvironmentMutex::scoped_lock lockenv(robot->GetEnv()->GetMutex());
397  _planner = RaveCreatePlanner(robot->GetEnv(),plannername);
399  params->SetRobotActiveJoints(_robot);
400  params->_sPostProcessingPlanner = ""; // have to turn off the second post processing stage
401  params->_hastimestamps = false;
402  params->_setstatefn.clear();
403  params->_checkpathconstraintsfn.clear();
404  params->_sExtraParameters = plannerparameters;
405  if( !_planner->InitPlan(_robot,params) ) {
406  throw OPENRAVE_EXCEPTION_FORMAT("failed to init planner %s with robot %s", plannername%_robot->GetName(), ORE_InvalidArguments);
407  }
408  _parameters=params; // necessary because SetRobotActiveJoints builds functions that hold weak_ptr to the parameters
409 }
410 
412 {
413  if( traj->GetNumWaypoints() == 1 ) {
414  // don't need velocities, but should at least add a time group
415  ConfigurationSpecification spec = traj->GetConfigurationSpecification();
416  spec.AddDeltaTimeGroup();
417  vector<dReal> data;
418  traj->GetWaypoints(0,traj->GetNumWaypoints(),data,spec);
419  traj->Init(spec);
420  traj->Insert(0,data);
421  return PS_HasSolution;
422  }
423 
424  TrajectoryTimingParametersPtr parameters = boost::dynamic_pointer_cast<TrajectoryTimingParameters>(_parameters);
425  if( parameters->_hastimestamps != hastimestamps ) {
426  parameters->_hastimestamps = hastimestamps;
427  if( !_planner->InitPlan(_robot,parameters) ) {
428  throw OPENRAVE_EXCEPTION_FORMAT("failed to init planner %s with robot %s", _planner->GetXMLId()%_robot->GetName(), ORE_InvalidArguments);
429  }
430  }
431 
432  return _planner->PlanPath(traj);
433 }
434 
435 PlannerStatus _PlanTrajectory(TrajectoryBasePtr traj, bool hastimestamps, dReal fmaxvelmult, dReal fmaxaccelmult, const std::string& plannername, bool bsmooth, const std::string& plannerparameters)
436 {
437  if( traj->GetNumWaypoints() == 1 ) {
438  // don't need velocities, but should at least add a time group
439  ConfigurationSpecification spec = traj->GetConfigurationSpecification();
440  spec.AddDeltaTimeGroup();
441  vector<dReal> data;
442  traj->GetWaypoints(0,traj->GetNumWaypoints(),data,spec);
443  traj->Init(spec);
444  traj->Insert(0,data);
445  return PS_HasSolution;
446  }
447 
448  EnvironmentMutex::scoped_lock lockenv(traj->GetEnv()->GetMutex());
449  PlannerBasePtr planner = RaveCreatePlanner(traj->GetEnv(),plannername.size() > 0 ? plannername : string("parabolicsmoother"));
451  params->SetConfigurationSpecification(traj->GetEnv(),traj->GetConfigurationSpecification().GetTimeDerivativeSpecification(0));
452  FOREACH(it,params->_vConfigVelocityLimit) {
453  *it *= fmaxvelmult;
454  }
455  FOREACH(it,params->_vConfigAccelerationLimit) {
456  *it *= fmaxaccelmult;
457  }
458  if( !bsmooth ) {
459  params->_setstatefn.clear();
460  params->_checkpathconstraintsfn.clear();
461  }
462 
463  params->_sPostProcessingPlanner = ""; // have to turn off the second post processing stage
464  params->_hastimestamps = hastimestamps;
465  params->_sExtraParameters += plannerparameters;
466  if( !planner->InitPlan(RobotBasePtr(),params) ) {
467  return PS_Failed;
468  }
469  if( planner->PlanPath(traj) != PS_HasSolution ) {
470  return PS_Failed;
471  }
472 
473  if( bsmooth && (RaveGetDebugLevel() & Level_VerifyPlans) ) {
475  planningutils::VerifyTrajectory(params,traj);
476  }
477  return PS_HasSolution;
478 }
479 
480 static void diffstatefn(std::vector<dReal>& q1, const std::vector<dReal>& q2, const std::vector<int> vrotaxes)
481 {
482  BOOST_ASSERT(q1.size()==q2.size());
483  for(size_t i = 0; i < q1.size(); ++i) {
484  if( find(vrotaxes.begin(),vrotaxes.end(),i) != vrotaxes.end() ) {
485  q1[i] = utils::SubtractCircularAngle(q1[i],q2[i]);
486  }
487  else {
488  q1[i] -= q2[i];
489  }
490  }
491 }
492 
493 static void _SetTransformBody(std::vector<dReal>::const_iterator itvalues, KinBodyPtr pbody, int index, int affinedofs,const Vector& vActvAffineRotationAxis)
494 {
495  Transform t = pbody->GetTransform();
496  RaveGetTransformFromAffineDOFValues(t,itvalues+index,affinedofs,vActvAffineRotationAxis);
497  pbody->SetTransform(t);
498 }
499 
500 static void _GetTransformBody(std::vector<dReal>::iterator itvalues, KinBodyPtr pbody, int index, int affinedofs,const Vector& vActvAffineRotationAxis)
501 {
502  Transform t = pbody->GetTransform();
503  RaveGetAffineDOFValuesFromTransform(itvalues+index,t, affinedofs,vActvAffineRotationAxis);
504 }
505 
506 static dReal _ComputeTransformBodyDistance(std::vector<dReal>::const_iterator itvalues0, std::vector<dReal>::const_iterator itvalues1, KinBodyPtr pbody, int index, int affinedofs,const Vector& vActvAffineRotationAxis)
507 {
508  Transform t0 = pbody->GetTransform();
509  Transform t1 = t0;
510  RaveGetTransformFromAffineDOFValues(t0,itvalues0+index,affinedofs,vActvAffineRotationAxis);
511  RaveGetTransformFromAffineDOFValues(t1,itvalues1+index,affinedofs,vActvAffineRotationAxis);
512  return TransformDistance2(t0, t1, 0.3);
513 }
514 
515 void _SetAffineState(const std::vector<dReal>& v, const std::list< boost::function< void(std::vector<dReal>::const_iterator) > >& listsetfunctions)
516 {
517  FOREACHC(itfn,listsetfunctions) {
518  (*itfn)(v.begin());
519  }
520 }
521 
522 void _GetAffineState(std::vector<dReal>& v, size_t expectedsize, const std::list< boost::function< void(std::vector<dReal>::iterator) > >& listgetfunctions)
523 {
524  v.resize(expectedsize);
525  FOREACHC(itfn,listgetfunctions) {
526  (*itfn)(v.begin());
527  }
528 }
529 
530 dReal _ComputeAffineDistanceMetric(const std::vector<dReal>& v0, const std::vector<dReal>& v1, const std::list< boost::function< dReal(std::vector<dReal>::const_iterator, std::vector<dReal>::const_iterator) > >& listdistfunctions)
531 {
532  dReal dist = 0;
533  FOREACHC(itfn,listdistfunctions) {
534  dist += (*itfn)(v0.begin(), v1.begin());
535  }
536  return dist;
537 }
538 
540 {
541 public:
543  _savedvalues.resize(dof);
544  getfn(_savedvalues);
545  BOOST_ASSERT(!!_setfn);
546  }
547  virtual ~PlannerStateSaver() {
548  _setfn(_savedvalues);
549  }
550 
551 private:
553  vector<dReal> _savedvalues;
554 };
555 
556 // this function is very messed up...?
557 static PlannerStatus _PlanAffineTrajectory(TrajectoryBasePtr traj, const std::vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, bool hastimestamps, const std::string& plannername, bool bsmooth, const std::string& plannerparameters)
558 {
559  if( traj->GetNumWaypoints() == 1 ) {
560  // don't need retiming, but should at least add a time group
561  ConfigurationSpecification spec = traj->GetConfigurationSpecification();
562  spec.AddDeltaTimeGroup();
563  vector<dReal> data;
564  traj->GetWaypoints(0,traj->GetNumWaypoints(),data,spec);
565  traj->Init(spec);
566  traj->Insert(0,data);
567  return PS_HasSolution;
568  }
569 
570  EnvironmentMutex::scoped_lock lockenv(traj->GetEnv()->GetMutex());
571  ConfigurationSpecification newspec = traj->GetConfigurationSpecification().GetTimeDerivativeSpecification(0);
572  if( newspec.GetDOF() != (int)maxvelocities.size() || newspec.GetDOF() != (int)maxaccelerations.size() ) {
573  throw OPENRAVE_EXCEPTION_FORMAT("traj values (%d) do not match maxvelocity size (%d) or maxaccelerations size (%d)",newspec.GetDOF()%maxvelocities.size()%maxaccelerations.size(), ORE_InvalidArguments);
574  }
575  // don't need to convert since the planner does that automatically
576  //ConvertTrajectorySpecification(traj,newspec);
577  PlannerBasePtr planner = RaveCreatePlanner(traj->GetEnv(),plannername.size() > 0 ? plannername : string("parabolicsmoother"));
579  params->_sPostProcessingPlanner = ""; // have to turn off the second post processing stage
580  params->_vConfigVelocityLimit = maxvelocities;
581  params->_vConfigAccelerationLimit = maxaccelerations;
582  params->_configurationspecification = newspec;
583  params->_vConfigLowerLimit.resize(newspec.GetDOF());
584  params->_vConfigUpperLimit.resize(newspec.GetDOF());
585  params->_vConfigResolution.resize(newspec.GetDOF());
586  for(size_t i = 0; i < params->_vConfigLowerLimit.size(); ++i) {
587  params->_vConfigLowerLimit[i] = -1e6;
588  params->_vConfigUpperLimit[i] = 1e6;
589  params->_vConfigResolution[i] = 0.01;
590  }
591 
592  std::list< boost::function<void(std::vector<dReal>::const_iterator) > > listsetfunctions;
593  std::list< boost::function<void(std::vector<dReal>::iterator) > > listgetfunctions;
594  std::list< boost::function<dReal(std::vector<dReal>::const_iterator, std::vector<dReal>::const_iterator) > > listdistfunctions;
595  std::vector<int> vrotaxes;
596  // analyze the configuration for identified dimensions
597  KinBodyPtr robot;
598  FOREACHC(itgroup,newspec._vgroups) {
599  if( itgroup->name.size() >= 16 && itgroup->name.substr(0,16) == "affine_transform" ) {
600  string tempname;
601  int affinedofs=0;
602  stringstream ss(itgroup->name.substr(16));
603  ss >> tempname >> affinedofs;
604  BOOST_ASSERT( !!ss );
605  KinBodyPtr pbody = traj->GetEnv()->GetKinBody(tempname);
606  // body doesn't hav eto be set
607  if( !!pbody ) {
608  Vector vaxis(0,0,1);
609  if( affinedofs & DOF_RotationAxis ) {
610  vrotaxes.push_back(itgroup->offset+RaveGetIndexFromAffineDOF(affinedofs,DOF_RotationAxis));
611  ss >> vaxis.x >> vaxis.y >> vaxis.z;
612  }
613  robot = pbody;
614  listsetfunctions.push_back(boost::bind(_SetTransformBody,_1,pbody,itgroup->offset,affinedofs,vaxis));
615  listgetfunctions.push_back(boost::bind(_GetTransformBody,_1,pbody,itgroup->offset,affinedofs,vaxis));
616  listdistfunctions.push_back(boost::bind(_ComputeTransformBodyDistance, _1, _2, pbody, itgroup->offset,affinedofs,vaxis));
617  }
618  }
619  else if( itgroup->name.size() >= 14 && itgroup->name.substr(0,14) == "ikparam_values" ) {
620  int iktypeint = 0;
621  stringstream ss(itgroup->name.substr(14));
622  ss >> iktypeint;
623  if( !!ss ) {
624  IkParameterizationType iktype=static_cast<IkParameterizationType>(iktypeint);
625  switch(iktype) {
626  case IKP_TranslationXYOrientation3D: vrotaxes.push_back(itgroup->offset+2); break;
627  default:
628  break;
629  }
630  }
631  if( bsmooth ) {
632  RAVELOG_VERBOSE("cannot smooth state for IK configurations\n");
633  }
634  }
635  }
636 
637  boost::shared_ptr<PlannerStateSaver> statesaver;
638  if( bsmooth ) {
639  if( listsetfunctions.size() > 0 ) {
640  params->_setstatefn = boost::bind(_SetAffineState,_1,boost::ref(listsetfunctions));
641  params->_getstatefn = boost::bind(_GetAffineState,_1,params->GetDOF(), boost::ref(listgetfunctions));
642  params->_distmetricfn = boost::bind(_ComputeAffineDistanceMetric,_1,_2,boost::ref(listdistfunctions));
643  std::list<KinBodyPtr> listCheckCollisions; listCheckCollisions.push_back(robot);
644  boost::shared_ptr<LineCollisionConstraint> pcollision(new LineCollisionConstraint(listCheckCollisions,true));
645  params->_checkpathconstraintsfn = boost::bind(&LineCollisionConstraint::Check,pcollision,PlannerBase::PlannerParametersWeakPtr(params), _1, _2, _3, _4);
646  statesaver.reset(new PlannerStateSaver(newspec.GetDOF(), params->_setstatefn, params->_getstatefn));
647  }
648  }
649  else {
650  params->_setstatefn.clear();
651  params->_getstatefn.clear();
652  params->_checkpathconstraintsfn.clear();
653  }
654 
655  params->_diffstatefn = boost::bind(diffstatefn,_1,_2,vrotaxes);
656 
657  params->_hastimestamps = hastimestamps;
658  params->_multidofinterp = 2; // always force switch points to be the same
659  params->_sExtraParameters = plannerparameters;
660  if( !planner->InitPlan(RobotBasePtr(),params) ) {
661  return PS_Failed;
662  }
663  if( planner->PlanPath(traj) != PS_HasSolution ) {
664  return PS_Failed;
665  }
666  return PS_HasSolution;
667 }
668 
669 AffineTrajectoryRetimer::AffineTrajectoryRetimer(const std::string& plannername, const std::string& plannerparameters)
670 {
671  if( plannername.size() > 0 ) {
672  _plannername = plannername;
673  }
674  else {
675  _plannername = "parabolictrajectoryretimer";
676  }
677  _extraparameters = plannerparameters;
678 }
679 
680 void AffineTrajectoryRetimer::SetPlanner(const std::string& plannername, const std::string& plannerparameters)
681 {
682  if( plannername.size() == 0 ) {
683  if( _plannername != string("parabolictrajectoryretimer") ) {
684  _plannername = "parabolictrajectoryretimer";
685  _planner.reset();
686  }
687  }
688  else if( _plannername != plannername ) {
689  _plannername = plannername;
690  _planner.reset();
691  }
692 
693  if( _extraparameters != plannerparameters ) {
694  _extraparameters = plannerparameters;
695  if( !!_parameters ) {
696  _parameters->_sExtraParameters = _extraparameters;
697  if( !!_planner ) {
698  if( !_planner->InitPlan(RobotBasePtr(), _parameters) ) {
699  throw OPENRAVE_EXCEPTION_FORMAT("failed to init planner %s", _plannername, ORE_InvalidArguments);
700  }
701  }
702  }
703  }
704 }
705 
706 PlannerStatus AffineTrajectoryRetimer::PlanPath(TrajectoryBasePtr traj, const std::vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, bool hastimestamps)
707 {
708  if( traj->GetNumWaypoints() == 1 ) {
709  // don't need retiming, but should at least add a time group
710  ConfigurationSpecification spec = traj->GetConfigurationSpecification();
711  spec.AddDeltaTimeGroup();
712  vector<dReal> data;
713  traj->GetWaypoints(0,traj->GetNumWaypoints(),data,spec);
714  traj->Init(spec);
715  traj->Insert(0,data);
716  return PS_HasSolution;
717  }
718 
719  EnvironmentBasePtr env = traj->GetEnv();
720  EnvironmentMutex::scoped_lock lockenv(env->GetMutex());
721  ConfigurationSpecification trajspec = traj->GetConfigurationSpecification().GetTimeDerivativeSpecification(0);
722  if( trajspec.GetDOF() != (int)maxvelocities.size() || trajspec.GetDOF() != (int)maxaccelerations.size() ) {
723  throw OPENRAVE_EXCEPTION_FORMAT("traj values (%d) do not match maxvelocity size (%d) or maxaccelerations size (%d)",trajspec.GetDOF()%maxvelocities.size()%maxaccelerations.size(), ORE_InvalidArguments);
724  }
725  //ConvertTrajectorySpecification(traj,trajspec);
727  if( !_parameters ) {
728  parameters.reset(new TrajectoryTimingParameters());
729  parameters->_sPostProcessingPlanner = ""; // have to turn off the second post processing stage
730  parameters->_setstatefn.clear();
731  parameters->_getstatefn.clear();
732  parameters->_checkpathconstraintsfn.clear();
733  parameters->_sExtraParameters += _extraparameters;
734  parameters->_multidofinterp = 2; // always force switch points to be the same
735  _parameters = parameters;
736  }
737  else {
738  parameters = boost::dynamic_pointer_cast<TrajectoryTimingParameters>(_parameters);
739  }
740 
741 
742  bool bInitPlan=false;
743  if( bInitPlan || CompareRealVectors(parameters->_vConfigVelocityLimit, maxvelocities, g_fEpsilonLinear) != 0 ) {
744  parameters->_vConfigVelocityLimit = maxvelocities;
745  bInitPlan = true;
746  }
747  if( bInitPlan || CompareRealVectors(parameters->_vConfigAccelerationLimit, maxaccelerations, g_fEpsilonLinear) != 0 ) {
748  parameters->_vConfigAccelerationLimit = maxaccelerations;
749  bInitPlan = true;
750  }
751  if( bInitPlan || parameters->_configurationspecification != trajspec ) {
752  parameters->_configurationspecification = trajspec;
753  bInitPlan = true;
754  }
755  if( bInitPlan || (int)parameters->_vConfigLowerLimit.size() != trajspec.GetDOF() ) {
756  parameters->_vConfigLowerLimit.resize(trajspec.GetDOF());
757  parameters->_vConfigUpperLimit.resize(trajspec.GetDOF());
758  parameters->_vConfigResolution.resize(trajspec.GetDOF());
759  for(size_t i = 0; i < parameters->_vConfigLowerLimit.size(); ++i) {
760  parameters->_vConfigLowerLimit[i] = -1e6;
761  parameters->_vConfigUpperLimit[i] = 1e6;
762  parameters->_vConfigResolution[i] = 0.01;
763  }
764  bInitPlan = true;
765  }
766 
767  std::vector<int> vrotaxes;
768  // analyze the configuration for identified dimensions
769  FOREACHC(itgroup,trajspec._vgroups) {
770  if( itgroup->name.size() >= 16 && itgroup->name.substr(0,16) == "affine_transform" ) {
771  string tempname;
772  int affinedofs=0;
773  stringstream ss(itgroup->name.substr(16));
774  ss >> tempname >> affinedofs;
775  BOOST_ASSERT( !!ss );
776  KinBodyPtr pbody = env->GetKinBody(tempname);
777  // body doesn't hav eto be set
778  if( !!pbody ) {
779  Vector vaxis(0,0,1);
780  if( affinedofs & DOF_RotationAxis ) {
781  vrotaxes.push_back(itgroup->offset+RaveGetIndexFromAffineDOF(affinedofs,DOF_RotationAxis));
782  ss >> vaxis.x >> vaxis.y >> vaxis.z;
783  }
784  }
785  }
786  else if( itgroup->name.size() >= 14 && itgroup->name.substr(0,14) == "ikparam_values" ) {
787  int iktypeint = 0;
788  stringstream ss(itgroup->name.substr(14));
789  ss >> iktypeint;
790  if( !!ss ) {
791  IkParameterizationType iktype=static_cast<IkParameterizationType>(iktypeint);
792  switch(iktype) {
793  case IKP_TranslationXYOrientation3D: vrotaxes.push_back(itgroup->offset+2); break;
794  default:
795  break;
796  }
797  }
798  }
799  }
800 
801  if( bInitPlan ) {
802  parameters->_diffstatefn = boost::bind(diffstatefn,_1,_2,vrotaxes);
803  }
804  if( parameters->_hastimestamps != hastimestamps ) {
805  parameters->_hastimestamps = hastimestamps;
806  bInitPlan = true;
807  }
808 
809  if( !_planner ) {
811  bInitPlan = true;
812  }
813  if( bInitPlan ) {
814  if( !_planner->InitPlan(RobotBasePtr(),parameters) ) {
815  stringstream ss; ss << trajspec;
816  throw OPENRAVE_EXCEPTION_FORMAT("failed to init planner %s with affine trajectory spec: %s", _plannername%ss.str(), ORE_InvalidArguments);
817  }
818  }
819 
820  return _planner->PlanPath(traj);
821 }
822 
823 PlannerStatus SmoothActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBasePtr robot, dReal fmaxvelmult, dReal fmaxaccelmult, const std::string& plannername, const std::string& plannerparameters)
824 {
825  return _PlanActiveDOFTrajectory(traj,robot,false,fmaxvelmult,fmaxaccelmult,plannername.size() > 0 ? plannername : "parabolicsmoother", true,plannerparameters);
826 }
827 
828 PlannerStatus SmoothAffineTrajectory(TrajectoryBasePtr traj, const std::vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, const std::string& plannername, const std::string& plannerparameters)
829 {
830  return _PlanAffineTrajectory(traj, maxvelocities, maxaccelerations, false, plannername.size() > 0 ? plannername : "parabolicsmoother", true, plannerparameters);
831 }
832 
833 PlannerStatus SmoothTrajectory(TrajectoryBasePtr traj, dReal fmaxvelmult, dReal fmaxaccelmult, const std::string& plannername, const std::string& plannerparameters)
834 {
835  return _PlanTrajectory(traj,false,fmaxvelmult,fmaxaccelmult,plannername.size() > 0 ? plannername : "parabolicsmoother", true,plannerparameters);
836 }
837 
838 static std::string GetPlannerFromInterpolation(TrajectoryBasePtr traj, const std::string& forceplanner=std::string())
839 {
840  if( forceplanner.size() > 0 ) {
841  return forceplanner;
842  }
843  std::string interpolation;
844  // check out the trajectory interpolation values and take it from there
845  FOREACHC(itgroup,traj->GetConfigurationSpecification()._vgroups) {
846  if( itgroup->name.size() >= 12 && itgroup->name.substr(0,12) == "joint_values" ) {
847  interpolation = itgroup->interpolation;
848  break;
849  }
850  else if( itgroup->name.size() >= 16 && itgroup->name.substr(0,16) == "affine_transform" ) {
851  interpolation = itgroup->interpolation;
852  }
853  else if( itgroup->name.size() >= 14 && itgroup->name.substr(0,14) == "ikparam_values" ) {
854  interpolation = itgroup->interpolation;
855  }
856  }
857 
858  if( interpolation == "quadratic" ) {
859  return "parabolictrajectoryretimer";
860  }
861  else if( interpolation == "linear" ) {
862  return "lineartrajectoryretimer";
863  }
864  else {
865  return "lineartrajectoryretimer";
866  }
867 }
868 
869 PlannerStatus RetimeActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBasePtr robot, bool hastimestamps, dReal fmaxvelmult, dReal fmaxaccelmult, const std::string& plannername, const std::string& plannerparameters)
870 {
871  return _PlanActiveDOFTrajectory(traj,robot,hastimestamps,fmaxvelmult,fmaxaccelmult,GetPlannerFromInterpolation(traj,plannername), false,plannerparameters);
872 }
873 
874 PlannerStatus RetimeAffineTrajectory(TrajectoryBasePtr traj, const std::vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, bool hastimestamps, const std::string& plannername, const std::string& plannerparameters)
875 {
876  return _PlanAffineTrajectory(traj, maxvelocities, maxaccelerations, hastimestamps, GetPlannerFromInterpolation(traj,plannername), false,plannerparameters);
877 }
878 
879 PlannerStatus RetimeTrajectory(TrajectoryBasePtr traj, bool hastimestamps, dReal fmaxvelmult, dReal fmaxaccelmult, const std::string& plannername, const std::string& plannerparameters)
880 {
881  return _PlanTrajectory(traj,hastimestamps,fmaxvelmult,fmaxaccelmult,GetPlannerFromInterpolation(traj,plannername), false,plannerparameters);
882 }
883 
884 void InsertActiveDOFWaypointWithRetiming(int waypointindex, const std::vector<dReal>& dofvalues, const std::vector<dReal>& dofvelocities, TrajectoryBasePtr traj, RobotBasePtr robot, dReal fmaxvelmult, dReal fmaxaccelmult, const std::string& plannername)
885 {
886  BOOST_ASSERT((int)dofvalues.size()==robot->GetActiveDOF());
887  BOOST_ASSERT(traj->GetEnv()==robot->GetEnv());
888  vector<dReal> v1pos(robot->GetActiveDOF(),0), v1vel(robot->GetActiveDOF(),0);
889  ConfigurationSpecification newspec = robot->GetActiveConfigurationSpecification();
890 
891  string interpolation = "";
892  FOREACH(it,newspec._vgroups) {
893  std::vector<ConfigurationSpecification::Group>::const_iterator itgroup = traj->GetConfigurationSpecification().FindCompatibleGroup(*it, false);
894  if( itgroup == traj->GetConfigurationSpecification()._vgroups.end() ) {
895  throw OPENRAVE_EXCEPTION_FORMAT("could not find group %s in trajectory",newspec._vgroups.at(0).name,ORE_InvalidArguments);
896  }
897  if( itgroup->interpolation.size() > 0 ) {
898  it->interpolation = itgroup->interpolation;
899  interpolation = itgroup->interpolation;
900  }
901  }
902  newspec.AddDerivativeGroups(1,false);
903 
904  vector<dReal> vwaypointstart, vwaypointend, vtargetvalues;
905  if( waypointindex == 0 ) {
906  vwaypointstart.resize(newspec.GetDOF());
907  ConfigurationSpecification::ConvertData(vwaypointstart.begin(), newspec, dofvalues.begin(), robot->GetActiveConfigurationSpecification(), 1, traj->GetEnv(), true);
908  if( dofvalues.size() == dofvelocities.size() ) {
909  ConfigurationSpecification::ConvertData(vwaypointstart.begin(), newspec.ConvertToVelocitySpecification(), dofvelocities.begin(), robot->GetActiveConfigurationSpecification().ConvertToVelocitySpecification(), 1, traj->GetEnv(), false);
910  }
911  traj->GetWaypoint(0,vwaypointend, newspec);
912  traj->GetWaypoint(0,vtargetvalues); // in target spec
913  }
914  else if( waypointindex == (int)traj->GetNumWaypoints() ) {
915  traj->GetWaypoint(waypointindex-1,vtargetvalues); // in target spec
916  traj->GetWaypoint(waypointindex-1,vwaypointstart, newspec);
917 
918  vwaypointend.resize(newspec.GetDOF());
919  ConfigurationSpecification::ConvertData(vwaypointend.begin(), newspec, dofvalues.begin(), robot->GetActiveConfigurationSpecification(), 1, traj->GetEnv(), true);
920  if( dofvalues.size() == dofvelocities.size() ) {
921  ConfigurationSpecification::ConvertData(vwaypointend.begin(), newspec.ConvertToVelocitySpecification(), dofvelocities.begin(), robot->GetActiveConfigurationSpecification().ConvertToVelocitySpecification(), 1, traj->GetEnv(), false);
922  }
923  }
924  else {
925  throw OPENRAVE_EXCEPTION_FORMAT0("do no support inserting waypoints in middle of trajectories yet",ORE_InvalidArguments);
926  }
927 
928  TrajectoryBasePtr trajinitial = RaveCreateTrajectory(traj->GetEnv(),traj->GetXMLId());
929  trajinitial->Init(newspec);
930  trajinitial->Insert(0,vwaypointstart);
931  trajinitial->Insert(1,vwaypointend);
932  std::string newplannername = plannername;
933  if( newplannername.size() == 0 ) {
934  if( interpolation == "linear" ) {
935  newplannername = "lineartrajectoryretimer";
936  }
937  else if( interpolation.size() == 0 || interpolation == "quadratic" ) {
938  newplannername = "parabolictrajectoryretimer";
939  }
940  else {
941  throw OPENRAVE_EXCEPTION_FORMAT("currently do not support retiming for %s interpolations",interpolation,ORE_InvalidArguments);
942  }
943  }
944  RetimeActiveDOFTrajectory(trajinitial,robot,false,fmaxvelmult,fmaxaccelmult,newplannername);
945 
946  // retiming is done, now merge the two trajectories
947  size_t targetdof = vtargetvalues.size();
948  vtargetvalues.resize(targetdof*trajinitial->GetNumWaypoints());
949  for(size_t i = targetdof; i < vtargetvalues.size(); i += targetdof) {
950  std::copy(vtargetvalues.begin(),vtargetvalues.begin()+targetdof,vtargetvalues.begin()+i);
951  }
952  trajinitial->GetWaypoints(0,trajinitial->GetNumWaypoints(),vwaypointstart);
953 
954  // copy to the target values while preserving other data
955  ConfigurationSpecification::ConvertData(vtargetvalues.begin(), traj->GetConfigurationSpecification(), vwaypointstart.begin(), trajinitial->GetConfigurationSpecification(), trajinitial->GetNumWaypoints(), traj->GetEnv(), false);
956 
957  if( waypointindex == 0 ) {
958  // have to insert the first N-1 and overwrite the Nth
959  vwaypointstart.resize(targetdof);
960  std::copy(vtargetvalues.begin()+vtargetvalues.size()-targetdof,vtargetvalues.end(),vwaypointstart.begin());
961  traj->Insert(waypointindex,vwaypointstart,true);
962  vtargetvalues.resize(vtargetvalues.size()-targetdof);
963  if( vtargetvalues.size() > 0 ) {
964  traj->Insert(waypointindex,vtargetvalues,false);
965  }
966  }
967  else {
968  // insert last N-1
969  vtargetvalues.erase(vtargetvalues.begin(), vtargetvalues.begin()+targetdof);
970  traj->Insert(waypointindex,vtargetvalues,false);
971  }
972 }
973 
974 void InsertWaypointWithSmoothing(int index, const std::vector<dReal>& dofvalues, const std::vector<dReal>& dofvelocities, TrajectoryBasePtr traj, dReal fmaxvelmult, dReal fmaxaccelmult, const std::string& plannername)
975 {
976  if( index != (int)traj->GetNumWaypoints() ) {
977  throw OPENRAVE_EXCEPTION_FORMAT0("InsertWaypointWithSmoothing only supports adding waypoints at the end",ORE_InvalidArguments);
978  }
979  OPENRAVE_ASSERT_OP(dofvalues.size(),==,dofvelocities.size());
980 
982  ConfigurationSpecification specpos = traj->GetConfigurationSpecification().GetTimeDerivativeSpecification(0);
983  OPENRAVE_ASSERT_OP(specpos.GetDOF(),==,(int)dofvalues.size());
984  params->SetConfigurationSpecification(traj->GetEnv(),specpos);
985  FOREACH(it,params->_vConfigVelocityLimit) {
986  *it *= fmaxvelmult;
987  }
988  FOREACH(it,params->_vConfigAccelerationLimit) {
989  *it *= fmaxaccelmult;
990  }
991 
992  params->_hasvelocities = true;
993  params->_hastimestamps = false;
994 
995  EnvironmentMutex::scoped_lock lockenv(traj->GetEnv()->GetMutex());
996  PlannerBasePtr planner = RaveCreatePlanner(traj->GetEnv(),plannername.size() > 0 ? plannername : string("parabolictrajectoryretimer"));
997  if( !planner->InitPlan(RobotBasePtr(),params) ) {
998  throw OPENRAVE_EXCEPTION_FORMAT0("failed to InitPlan",ORE_Failed);
999  }
1000 
1001  dReal fSamplingTime = 0.01; // for collision checking
1002  dReal fTimeBuffer = 0.01; // if new trajectory increases within this time limit, then it will be accepted
1003 
1004  ConfigurationSpecification spectotal = specpos;
1005  spectotal.AddDerivativeGroups(1,false);
1006  OPENRAVE_ASSERT_OP(spectotal.GetDOF(),==,2*(int)dofvalues.size());
1007  TrajectoryBasePtr ptesttraj;
1008  TrajectoryBasePtr pBestTrajectory;
1009  dReal fBestDuration = 1e30;
1010  int iBestInsertionWaypoint = -1;
1011  std::vector<dReal> vstartdata(spectotal.GetDOF(),0), vwaypoint, vprevpoint;
1012  std::copy(dofvalues.begin(),dofvalues.end(),vstartdata.begin());
1013  std::copy(dofvelocities.begin(),dofvelocities.end(),vstartdata.begin()+dofvalues.size());
1014  // look for the waypoints in reverse
1015  int N = (int)traj->GetNumWaypoints();
1016  dReal fOriginalTime = traj->GetDuration();
1017  dReal fRemainingDuration=traj->GetDuration();
1018  int iTimeIndex = -1;
1019  std::vector<ConfigurationSpecification::Group>::const_iterator itdeltatimegroup = traj->GetConfigurationSpecification().FindCompatibleGroup("deltatime");
1020  if( itdeltatimegroup != traj->GetConfigurationSpecification()._vgroups.end() ) {
1021  iTimeIndex = itdeltatimegroup->offset;
1022  }
1023 
1024  // since inserting from the back, go through each of the waypoints from reverse and record collision free segments
1025  // perhaps a faster method would be to delay the collision checking until all the possible segments are already checked...?
1026  for(int iwaypoint = 0; iwaypoint < N; ++iwaypoint) {
1027  traj->GetWaypoint(N-1-iwaypoint,vwaypoint);
1028  dReal deltatime = 0;
1029  if( iTimeIndex >= 0 ) {
1030  // reset the time in case the trajectory retimer does not reset it
1031  deltatime = vwaypoint.at(iTimeIndex);
1032  vwaypoint.at(iTimeIndex) = 0;
1033  }
1034  if( !ptesttraj ) {
1035  ptesttraj = RaveCreateTrajectory(traj->GetEnv(),traj->GetXMLId());
1036  }
1037  ptesttraj->Init(traj->GetConfigurationSpecification());
1038  ptesttraj->Insert(0,vwaypoint);
1039  ptesttraj->Insert(1,vstartdata,spectotal);
1040 
1041  if( planner->PlanPath(ptesttraj) & PS_HasSolution ) {
1042  // before checking, make sure it is better than we currently have
1043  dReal fNewDuration = fRemainingDuration+ptesttraj->GetDuration();
1044  if( fNewDuration < fBestDuration ) {
1045  // have to check for collision by sampling
1046  ptesttraj->Sample(vprevpoint,0,specpos);
1047  bool bInCollision = false;
1048  for(dReal t = fSamplingTime; t < ptesttraj->GetDuration(); t+=fSamplingTime) {
1049  ptesttraj->Sample(vwaypoint,t,specpos);
1050  if( !params->_checkpathconstraintsfn(vprevpoint,vwaypoint,IT_OpenStart, PlannerBase::ConfigurationListPtr()) ) {
1051  bInCollision = true;
1052  break;
1053  }
1054  vprevpoint = vwaypoint;
1055  }
1056  if( !bInCollision ) {
1057  iBestInsertionWaypoint = N-1-iwaypoint;
1058  fBestDuration = fNewDuration;
1059  swap(pBestTrajectory,ptesttraj);
1060  if( iwaypoint > 0 && fBestDuration < fOriginalTime+fTimeBuffer ) {
1061  break;
1062  }
1063  }
1064  else {
1065  if( !!pBestTrajectory ) {
1066  // already have a good trajectory, and chances are that anything after this waypoint will also be in collision, so exit
1067  break;
1068  }
1069  }
1070  }
1071  else {
1072  // if it isn't better and we already have a best trajectory, then choose it. most likely things will get worse from now on...
1073  if( !!pBestTrajectory ) {
1074  break;
1075  }
1076  }
1077  }
1078  fRemainingDuration -= deltatime;
1079  }
1080 
1081  if( !pBestTrajectory ) {
1082  throw OPENRAVE_EXCEPTION_FORMAT0("failed to find connecting trajectory",ORE_Assert);
1083  }
1084  if( fBestDuration > fOriginalTime+fTimeBuffer ) {
1085  RAVELOG_WARN(str(boost::format("new trajectory is greater than expected time %f > %f \n")%fBestDuration%fOriginalTime));
1086  }
1087  // splice in the new trajectory. pBestTrajectory's first waypoint matches traj's iBestInsertionWaypoint
1088  traj->Remove(iBestInsertionWaypoint+1,traj->GetNumWaypoints());
1089 // traj->GetWaypoint(iBestInsertionWaypoint,vprevpoint);
1090 // pBestTrajectory->GetWaypoint(0,vwaypoint,traj->GetConfigurationSpecification());
1091 // for(size_t i = 0; i < vprevpoint.size(); ++i) {
1092 // if( RaveFabs(vprevpoint[i]-vwaypoint.at(i)) > 0.0001 ) {
1093 // RAVELOG_WARN(str(boost::format("start points differ at %d: %e != %e")%i%vprevpoint[i]%vwaypoint[i]));
1094 // }
1095 // }
1096  pBestTrajectory->GetWaypoints(1,pBestTrajectory->GetNumWaypoints(),vwaypoint,traj->GetConfigurationSpecification());
1097  traj->Insert(iBestInsertionWaypoint+1,vwaypoint);
1098  dReal fNewDuration = traj->GetDuration();
1099  OPENRAVE_ASSERT_OP( RaveFabs(fNewDuration-fBestDuration), <=, 0.001 );
1100 }
1101 
1103 {
1104  if( traj->GetConfigurationSpecification() != spec ) {
1105  size_t numpoints = traj->GetConfigurationSpecification().GetDOF() > 0 ? traj->GetNumWaypoints() : 0;
1106  vector<dReal> data;
1107  if( numpoints > 0 ) {
1108  traj->GetWaypoints(0,numpoints,data,spec);
1109  }
1110  traj->Init(spec);
1111  if( numpoints > 0 ) {
1112  traj->Insert(0,data);
1113  }
1114  }
1115 }
1116 
1118 {
1119  OPENRAVE_ASSERT_OP(maxderiv,>,0);
1120  ConfigurationSpecification newspec = traj->GetConfigurationSpecification();
1121  for(int i = 1; i <= maxderiv; ++i) {
1122  newspec.AddDerivativeGroups(i);
1123  }
1124  std::vector<dReal> data;
1125  traj->GetWaypoints(0,traj->GetNumWaypoints(),data, newspec);
1126  if( data.size() == 0 ) {
1127  traj->Init(newspec);
1128  return;
1129  }
1130  int numpoints = (int)data.size()/newspec.GetDOF();
1131  ConfigurationSpecification velspec = newspec.GetTimeDerivativeSpecification(maxderiv-1);
1132  ConfigurationSpecification accelspec = newspec.GetTimeDerivativeSpecification(maxderiv);
1133  std::vector<ConfigurationSpecification::Group>::const_iterator itdeltatimegroup = newspec.FindCompatibleGroup("deltatime");
1134  if(itdeltatimegroup == newspec._vgroups.end() ) {
1135  throw OPENRAVE_EXCEPTION_FORMAT0("trajectory does not seem to have time stamps, so derivatives cannot be computed", ORE_InvalidArguments);
1136  }
1137  OPENRAVE_ASSERT_OP(velspec.GetDOF(), ==, accelspec.GetDOF());
1138  int offset = 0;
1139  dReal nextdeltatime=0;
1140  for(int ipoint = 0; ipoint < numpoints; ++ipoint, offset += newspec.GetDOF()) {
1141  if( ipoint < numpoints-1 ) {
1142  nextdeltatime = data.at(offset+newspec.GetDOF()+itdeltatimegroup->offset);
1143  }
1144  else {
1145  nextdeltatime = 0;
1146  }
1147  for(size_t igroup = 0; igroup < velspec._vgroups.size(); ++igroup) {
1148  std::vector<ConfigurationSpecification::Group>::const_iterator itvel = newspec.FindCompatibleGroup(velspec._vgroups.at(igroup),true);
1149  std::vector<ConfigurationSpecification::Group>::const_iterator itaccel = newspec.FindCompatibleGroup(accelspec._vgroups.at(igroup),true);
1150  OPENRAVE_ASSERT_OP(itvel->dof,==,itaccel->dof);
1151  if( ipoint < numpoints-1 ) {
1152  for(int i = 1; i < itvel->dof; ++i) {
1153  if( nextdeltatime > 0 ) {
1154  data.at(offset+itaccel->offset+i) = (data.at(offset+newspec.GetDOF()+itvel->offset+i)-data.at(offset+itvel->offset+i))/nextdeltatime;
1155  }
1156  else {
1157  // time is 0 so leave empty?
1158  }
1159  }
1160  }
1161  else {
1162  // what to do with the last point?
1163  }
1164  }
1165  }
1166 
1167  traj->Init(newspec);
1168  traj->Insert(0,data);
1169 }
1170 
1172 {
1173  vector<dReal> sourcedata;
1174  size_t numpoints = sourcetraj->GetNumWaypoints();
1175  int dof = sourcetraj->GetConfigurationSpecification().GetDOF();
1176  vector<uint8_t> velocitydofs(dof,0);
1177  int timeoffset = -1;
1178  vector<uint8_t> velocitynextinterp(dof,0);
1179  FOREACHC(itgroup, sourcetraj->GetConfigurationSpecification()._vgroups) {
1180  if( itgroup->name.find("_velocities") != string::npos ) {
1181  bool bnext = itgroup->interpolation == "next";
1182  for(int i = 0; i < itgroup->dof; ++i) {
1183  velocitydofs.at(itgroup->offset+i) = 1;
1184  velocitynextinterp.at(itgroup->offset+i) = bnext;
1185  }
1186  }
1187  else if( itgroup->name == "deltatime" ) {
1188  timeoffset = itgroup->offset;
1189  }
1190  }
1191  sourcetraj->GetWaypoints(0,numpoints,sourcedata);
1192  vector<dReal> targetdata(sourcedata.size());
1193  dReal prevdeltatime = 0;
1194  for(size_t i = 0; i < numpoints; ++i) {
1195  vector<dReal>::iterator ittarget = targetdata.begin()+i*dof;
1196  vector<dReal>::iterator itsource = sourcedata.begin()+(numpoints-i-1)*dof;
1197  for(int j = 0; j < dof; ++j) {
1198  if( velocitydofs[j] ) {
1199  if( velocitynextinterp[j] ) {
1200  if( i < numpoints-1 ) {
1201  *(ittarget+j+dof) = -*(itsource+j);
1202  }
1203  else {
1204  targetdata.at(j) = 0;
1205  }
1206  }
1207  else {
1208  *(ittarget+j) = -*(itsource+j);
1209  }
1210  }
1211  else {
1212  *(ittarget+j) = *(itsource+j);
1213  }
1214  }
1215 
1216  if( timeoffset >= 0 ) {
1217  *(ittarget+timeoffset) = prevdeltatime;
1218  prevdeltatime = *(itsource+timeoffset);
1219  }
1220  }
1221 
1222  TrajectoryBasePtr traj = RaveCreateTrajectory(sourcetraj->GetEnv(),sourcetraj->GetXMLId());
1223  traj->Init(sourcetraj->GetConfigurationSpecification());
1224  traj->Insert(0,targetdata);
1225  traj->SetDescription(sourcetraj->GetDescription());
1226  return traj;
1227 }
1228 
1229 TrajectoryBasePtr MergeTrajectories(const std::list<TrajectoryBaseConstPtr>& listtrajectories)
1230 {
1231  TrajectoryBasePtr presulttraj;
1232  if( listtrajectories.size() == 0 ) {
1233  return presulttraj;
1234  }
1235  if( listtrajectories.size() == 1 ) {
1236  presulttraj = RaveCreateTrajectory(listtrajectories.front()->GetEnv(),listtrajectories.front()->GetXMLId());
1237  presulttraj->Clone(listtrajectories.front(),0);
1238  return presulttraj;
1239  }
1240 
1242  vector<dReal> vpointdata;
1243  vector<dReal> vtimes; vtimes.reserve(listtrajectories.front()->GetNumWaypoints());
1244  int totaldof = 1; // for delta time
1245  FOREACHC(ittraj,listtrajectories) {
1246  const ConfigurationSpecification& trajspec = (*ittraj)->GetConfigurationSpecification();
1247  ConfigurationSpecification::Group gtime = trajspec.GetGroupFromName("deltatime");
1248  spec += trajspec;
1249  totaldof += trajspec.GetDOF()-1;
1250  if( trajspec.FindCompatibleGroup("iswaypoint",true) != trajspec._vgroups.end() ) {
1251  totaldof -= 1;
1252  }
1253  dReal curtime = 0;
1254  for(size_t ipoint = 0; ipoint < (*ittraj)->GetNumWaypoints(); ++ipoint) {
1255  (*ittraj)->GetWaypoint(ipoint,vpointdata);
1256  curtime += vpointdata.at(gtime.offset);
1257  vector<dReal>::iterator it = lower_bound(vtimes.begin(),vtimes.end(),curtime);
1258  if( *it != curtime ) {
1259  vtimes.insert(it,curtime);
1260  }
1261  }
1262  }
1263 
1264  vector<ConfigurationSpecification::Group>::const_iterator itwaypointgroup = spec.FindCompatibleGroup("iswaypoint",true);
1265  vector<dReal> vwaypoints;
1266  if( itwaypointgroup != spec._vgroups.end() ) {
1267  totaldof += 1;
1268  vwaypoints.resize(vtimes.size(),0);
1269  }
1270 
1271  if( totaldof != spec.GetDOF() ) {
1272  throw OPENRAVE_EXCEPTION_FORMAT("merged configuration needs to have %d DOF, currently has %d",totaldof%spec.GetDOF(),ORE_InvalidArguments);
1273  }
1274  presulttraj = RaveCreateTrajectory(listtrajectories.front()->GetEnv(),listtrajectories.front()->GetXMLId());
1275  presulttraj->Init(spec);
1276 
1277  if( vtimes.size() == 0 ) {
1278  return presulttraj;
1279  }
1280 
1281  // need to find all waypoints
1282  vector<dReal> vtemp, vnewdata;
1283  stringstream sdesc;
1284  int deltatimeoffset = spec.GetGroupFromName("deltatime").offset;
1285  FOREACHC(ittraj,listtrajectories) {
1286  vector<ConfigurationSpecification::Group>::const_iterator itwaypointgrouptraj = (*ittraj)->GetConfigurationSpecification().FindCompatibleGroup("iswaypoint",true);
1287  int waypointoffset = -1;
1288  if( itwaypointgrouptraj != (*ittraj)->GetConfigurationSpecification()._vgroups.end() ) {
1289  waypointoffset = itwaypointgrouptraj->offset;
1290  }
1291  if( vnewdata.size() == 0 ) {
1292  vnewdata.reserve(vtimes.size()*spec.GetDOF());
1293  for(size_t i = 0; i < vtimes.size(); ++i) {
1294  (*ittraj)->Sample(vtemp,vtimes[i],spec);
1295  vnewdata.insert(vnewdata.end(),vtemp.begin(),vtemp.end());
1296  if( waypointoffset >= 0 ) {
1297  vwaypoints[i] += vtemp[waypointoffset];
1298  }
1299  }
1300  }
1301  else {
1302  vpointdata.resize(0);
1303  for(size_t i = 0; i < vtimes.size(); ++i) {
1304  (*ittraj)->Sample(vtemp,vtimes[i]);
1305  vpointdata.insert(vpointdata.end(),vtemp.begin(),vtemp.end());
1306  if( waypointoffset >= 0 ) {
1307  vwaypoints[i] += vtemp[waypointoffset];
1308  }
1309  }
1310  ConfigurationSpecification::ConvertData(vnewdata.begin(),spec,vpointdata.begin(),(*ittraj)->GetConfigurationSpecification(),vtimes.size(),presulttraj->GetEnv(),false);
1311  }
1312 
1313  sdesc << (*ittraj)->GetDescription() << endl;
1314  }
1315 
1316  vnewdata.at(deltatimeoffset) = vtimes[0];
1317  for(size_t i = 1; i < vtimes.size(); ++i) {
1318  vnewdata.at(i*spec.GetDOF()+deltatimeoffset) = vtimes[i]-vtimes[i-1];
1319  }
1320  if( itwaypointgroup != spec._vgroups.end() ) {
1321  vnewdata.at(itwaypointgroup->offset) = vwaypoints[0];
1322  for(size_t i = 1; i < vtimes.size(); ++i) {
1323  vnewdata.at(i*spec.GetDOF()+itwaypointgroup->offset) = vwaypoints[i];
1324  }
1325  }
1326  presulttraj->Insert(0,vnewdata);
1327  presulttraj->SetDescription(sdesc.str());
1328  return presulttraj;
1329 }
1330 
1331 void GetDHParameters(std::vector<DHParameter>& vparameters, KinBodyConstPtr pbody)
1332 {
1333  EnvironmentMutex::scoped_lock lockenv(pbody->GetEnv()->GetMutex());
1334  Transform tbaseinv = pbody->GetTransform().inverse();
1335  vparameters.resize(pbody->GetDependencyOrderedJoints().size());
1336  std::vector<DHParameter>::iterator itdh = vparameters.begin();
1337  FOREACHC(itjoint,pbody->GetDependencyOrderedJoints()) {
1338  OPENRAVE_ASSERT_FORMAT((*itjoint)->GetType() == KinBody::JointHinge || (*itjoint)->GetType() == KinBody::JointSlider, "joint type 0x%x not supported for DH parameters", (*itjoint)->GetType(), ORE_Assert);
1339  KinBody::LinkConstPtr plink = (*itjoint)->GetHierarchyParentLink();
1340  Transform tparent, tparentinv;
1341  itdh->joint = *itjoint;
1342  itdh->parentindex = -1;
1343  if( !!plink ) {
1344  std::vector<KinBody::JointPtr> vparentjoints;
1345  pbody->GetChain(0,plink->GetIndex(),vparentjoints);
1346  if( vparentjoints.size() > 0 ) {
1347  // get the first non-passive joint
1348  int index = (int)vparentjoints.size()-1;
1349  while(index >= 0 && vparentjoints[index]->GetJointIndex() < 0 ) {
1350  index--;
1351  }
1352  if( index >= 0 ) {
1353  // search for it in pbody->GetDependencyOrderedJoints()
1354  std::vector<KinBody::JointPtr>::const_iterator itjoint = find(pbody->GetDependencyOrderedJoints().begin(),pbody->GetDependencyOrderedJoints().end(),vparentjoints[index]);
1355  BOOST_ASSERT( itjoint != pbody->GetDependencyOrderedJoints().end() );
1356  itdh->parentindex = itjoint - pbody->GetDependencyOrderedJoints().begin();
1357  tparent = vparameters.at(itdh->parentindex).transform;
1358  tparentinv = tparent.inverse();
1359  }
1360  }
1361  }
1362  Vector vlocalanchor = tparentinv*(tbaseinv*(*itjoint)->GetAnchor());
1363  Vector vlocalaxis = tparentinv.rotate(tbaseinv.rotate((*itjoint)->GetAxis(0)));
1364  Vector vlocalx(-vlocalaxis.y,vlocalaxis.x,0);
1365  dReal fsinalpha = RaveSqrt(vlocalx.lengthsqr3());
1366  itdh->alpha = RaveAtan2(fsinalpha, vlocalaxis.z);
1367  if( itdh->alpha < 10*g_fEpsilon ) {
1368  // axes are parallel
1369  if( vlocalanchor.lengthsqr2() > 10*g_fEpsilon*g_fEpsilon ) {
1370  vlocalx.x = vlocalanchor.x;
1371  vlocalx.y = vlocalanchor.y;
1372  vlocalx.normalize3();
1373  }
1374  else {
1375  vlocalx = Vector(1,0,0);
1376  }
1377  }
1378  else {
1379  vlocalx /= fsinalpha;
1380  }
1381  itdh->d = vlocalanchor.z;
1382  itdh->theta = RaveAtan2(vlocalx.y,vlocalx.x);
1383  itdh->a = vlocalanchor.dot3(vlocalx);
1384  Vector zquat = quatFromAxisAngle(Vector(0,0,1),itdh->theta);
1385  Vector xquat = quatFromAxisAngle(Vector(1,0,0),itdh->alpha);
1386  bool bflip = false;
1387 // if( itdh->a < -g_fEpsilon ) {
1388 // // cannot have -a since a lot of formats specify a as positive
1389 // bflip = true;
1390 // }
1391 // else {
1392  Vector worldrotquat = quatMultiply(itdh->transform.rot, quatMultiply(zquat, xquat));
1393  // always try to have the x-axis pointed positively towards x
1394  // (worldrotquat * (1,0,0))[0] < 0
1395  dReal xcomponent = (1-2*worldrotquat.z*worldrotquat.z-2*worldrotquat.w*worldrotquat.w);
1396  if( xcomponent < 0 ) {
1397  bflip = true;
1398  }
1399  // }
1400 
1401  if( bflip ) {
1402  // normalize if theta is closer to PI
1403  if( itdh->theta > g_fEpsilon ) { // this is a really weird condition but it looks like people prefer theta to be negative
1404  itdh->alpha = -itdh->alpha;
1405  itdh->theta -= PI;
1406  itdh->a = -itdh->a;
1407  }
1408  else {
1409  itdh->alpha = -itdh->alpha;
1410  itdh->theta += PI;
1411  itdh->a = -itdh->a;
1412  }
1413  zquat = quatFromAxisAngle(Vector(0,0,1),itdh->theta);
1414  xquat = quatFromAxisAngle(Vector(1,0,0),itdh->alpha);
1415  }
1416  Transform Z_i(zquat, Vector(0,0,itdh->d));
1417  Transform X_i(xquat, Vector(itdh->a,0,0));
1418  itdh->transform = tparent*Z_i*X_i;
1419  ++itdh;
1420  }
1421 }
1422 
1424 {
1425  _report.reset(new CollisionReport());
1426 }
1427 
1428 LineCollisionConstraint::LineCollisionConstraint(const std::list<KinBodyPtr>& listCheckCollisions, bool bCheckEnv) : _listCheckSelfCollisions(listCheckCollisions), _bCheckEnv(bCheckEnv)
1429 {
1430  _report.reset(new CollisionReport());
1431 }
1432 
1433 void LineCollisionConstraint::SetUserCheckFunction(const boost::function<bool() >& usercheckfn, bool bCallAfterCheckCollision)
1434 {
1435  _usercheckfns[bCallAfterCheckCollision] = usercheckfn;
1436 }
1437 
1439 {
1440  if( !!_usercheckfns[0] ) {
1441  if( !_usercheckfns[0]() ) {
1442  return false;
1443  }
1444  }
1445  FOREACHC(itbody, _listCheckSelfCollisions) {
1446  if( _bCheckEnv && (*itbody)->GetEnv()->CheckCollision(KinBodyConstPtr(*itbody),_report) ) {
1447  return false;
1448  }
1449  if( (*itbody)->CheckSelfCollision(_report) ) {
1450  return false;
1451  }
1452  }
1453  if( !!_usercheckfns[1] ) {
1454  if( !_usercheckfns[1]() ) {
1455  return false;
1456  }
1457  }
1458  return true;
1459 }
1460 
1461 bool LineCollisionConstraint::Check(PlannerBase::PlannerParametersWeakPtr _params, KinBodyPtr robot, const std::vector<dReal>& pQ0, const std::vector<dReal>& pQ1, IntervalType interval, PlannerBase::ConfigurationListPtr pvCheckedConfigurations)
1462 {
1463  // set the bounds based on the interval type
1464  PlannerBase::PlannerParametersPtr params = _params.lock();
1465  if( !params ) {
1466  RAVELOG_WARN("parameters have been destroyed!\n");
1467  return false;
1468  }
1469  int start=0;
1470  bool bCheckEnd=false;
1471  switch (interval) {
1472  case IT_Open:
1473  start = 1; bCheckEnd = false;
1474  break;
1475  case IT_OpenStart:
1476  start = 1; bCheckEnd = true;
1477  break;
1478  case IT_OpenEnd:
1479  start = 0; bCheckEnd = false;
1480  break;
1481  case IT_Closed:
1482  start = 0; bCheckEnd = true;
1483  break;
1484  default:
1485  BOOST_ASSERT(0);
1486  }
1487 
1488  // first make sure the end is free
1489  _vtempconfig.resize(params->GetDOF());
1490  if (bCheckEnd) {
1491  params->_setstatefn(pQ1);
1492  if (_bCheckEnv && robot->GetEnv()->CheckCollision(KinBodyConstPtr(robot),_report) ) {
1493  return false;
1494  }
1495  if( robot->CheckSelfCollision(_report) ) {
1496  return false;
1497  }
1498  }
1499 
1500  // compute the discretization
1501  dQ = pQ1;
1502  params->_diffstatefn(dQ,pQ0);
1503  int i, numSteps = 1;
1504  std::vector<dReal>::const_iterator itres = params->_vConfigResolution.begin();
1505  OPENRAVE_ASSERT_OP((int)params->_vConfigResolution.size(),==,params->GetDOF());
1506  int totalsteps = 0;
1507  for (i = 0; i < params->GetDOF(); i++,itres++) {
1508  int steps;
1509  if( *itres != 0 ) {
1510  steps = (int)(fabs(dQ[i]) / *itres);
1511  }
1512  else {
1513  steps = (int)(fabs(dQ[i]) * 100);
1514  }
1515  totalsteps += steps;
1516  if (steps > numSteps) {
1517  numSteps = steps;
1518  }
1519  }
1520  if( totalsteps == 0 && start > 0) {
1521  if( bCheckEnd && !!pvCheckedConfigurations ) {
1522  pvCheckedConfigurations->push_back(pQ1);
1523  }
1524  return true;
1525  }
1526 
1527  if (start == 0 ) {
1528  params->_setstatefn(pQ0);
1529  if (_bCheckEnv && robot->GetEnv()->CheckCollision(KinBodyConstPtr(robot),_report) ) {
1530  return false;
1531  }
1532  if( robot->CheckSelfCollision(_report) ) {
1533  return false;
1534  }
1535  if( !!pvCheckedConfigurations ) {
1536  pvCheckedConfigurations->push_back(pQ0);
1537  }
1538  start = 1;
1539  }
1540 
1541  dReal fisteps = dReal(1.0f)/numSteps;
1542  for(std::vector<dReal>::iterator it = dQ.begin(); it != dQ.end(); ++it) {
1543  *it *= fisteps;
1544  }
1545 
1546  // check for collision along the straight-line path
1547  // NOTE: this does not check the end config, and may or may
1548  // not check the start based on the value of 'start'
1549  for (i = 0; i < params->GetDOF(); i++) {
1550  _vtempconfig[i] = pQ0[i];
1551  }
1552  if( start > 0 ) {
1553  params->_setstatefn(_vtempconfig);
1554  if( !params->_neighstatefn(_vtempconfig, dQ,0) ) {
1555  return false;
1556  }
1557  }
1558  for (int f = start; f < numSteps; f++) {
1559  params->_setstatefn(_vtempconfig);
1560  if( _bCheckEnv && robot->GetEnv()->CheckCollision(KinBodyConstPtr(robot)) ) {
1561  return false;
1562  }
1563  if( robot->CheckSelfCollision() ) {
1564  return false;
1565  }
1566  if( !!params->_getstatefn ) {
1567  params->_getstatefn(_vtempconfig); // query again in order to get normalizations/joint limits
1568  }
1569  if( !!pvCheckedConfigurations ) {
1570  pvCheckedConfigurations->push_back(_vtempconfig);
1571  }
1572  if( !params->_neighstatefn(_vtempconfig,dQ,0) ) {
1573  return false;
1574  }
1575  }
1576 
1577  if( bCheckEnd && !!pvCheckedConfigurations ) {
1578  pvCheckedConfigurations->push_back(pQ1);
1579  }
1580  return true;
1581 }
1582 
1583 bool LineCollisionConstraint::Check(PlannerBase::PlannerParametersWeakPtr _params, const std::vector<dReal>& pQ0, const std::vector<dReal>& pQ1, IntervalType interval, PlannerBase::ConfigurationListPtr pvCheckedConfigurations)
1584 {
1585  // set the bounds based on the interval type
1586  PlannerBase::PlannerParametersPtr params = _params.lock();
1587  if( !params ) {
1588  RAVELOG_WARN("parameters have been destroyed!\n");
1589  return false;
1590  }
1591  BOOST_ASSERT(_listCheckSelfCollisions.size()>0);
1592  int start=0;
1593  bool bCheckEnd=false;
1594  switch (interval) {
1595  case IT_Open:
1596  start = 1; bCheckEnd = false;
1597  break;
1598  case IT_OpenStart:
1599  start = 1; bCheckEnd = true;
1600  break;
1601  case IT_OpenEnd:
1602  start = 0; bCheckEnd = false;
1603  break;
1604  case IT_Closed:
1605  start = 0; bCheckEnd = true;
1606  break;
1607  default:
1608  BOOST_ASSERT(0);
1609  }
1610 
1611  // first make sure the end is free
1612  _vtempconfig.resize(params->GetDOF());
1613  if (bCheckEnd) {
1614  params->_setstatefn(pQ1);
1615  if( !_CheckState() ) {
1616  RAVELOG_VERBOSE(str(boost::format("collision: %s")%_report->__str__()));
1617  return false;
1618  }
1619  }
1620 
1621  // compute the discretization
1622  dQ = pQ1;
1623  params->_diffstatefn(dQ,pQ0);
1624  int i, numSteps = 1;
1625  std::vector<dReal>::const_iterator itres = params->_vConfigResolution.begin();
1626  BOOST_ASSERT((int)params->_vConfigResolution.size()==params->GetDOF());
1627  int totalsteps = 0;
1628  for (i = 0; i < params->GetDOF(); i++,itres++) {
1629  int steps;
1630  if( *itres != 0 ) {
1631  steps = (int)(fabs(dQ[i]) / *itres);
1632  }
1633  else {
1634  steps = (int)(fabs(dQ[i]) * 100);
1635  }
1636  totalsteps += steps;
1637  if (steps > numSteps) {
1638  numSteps = steps;
1639  }
1640  }
1641  if( totalsteps == 0 && start > 0 ) {
1642  return true;
1643  }
1644 
1645  if (start == 0 ) {
1646  params->_setstatefn(pQ0);
1647  if( !_CheckState() ) {
1648  RAVELOG_VERBOSE(str(boost::format("collision: %s")%_report->__str__()));
1649  return false;
1650  }
1651  start = 1;
1652  }
1653 
1654  dReal fisteps = dReal(1.0f)/numSteps;
1655  for(std::vector<dReal>::iterator it = dQ.begin(); it != dQ.end(); ++it) {
1656  *it *= fisteps;
1657  }
1658 
1659  // check for collision along the straight-line path
1660  // NOTE: this does not check the end config, and may or may
1661  // not check the start based on the value of 'start'
1662  for (i = 0; i < params->GetDOF(); i++) {
1663  _vtempconfig[i] = pQ0[i];
1664  }
1665  if( start > 0 ) {
1666  params->_setstatefn(_vtempconfig);
1667  if( !params->_neighstatefn(_vtempconfig, dQ,0) ) {
1668  RAVELOG_VERBOSE(str(boost::format("collision: %s")%_report->__str__()));
1669  return false;
1670  }
1671  }
1672  for (int f = start; f < numSteps; f++) {
1673  params->_setstatefn(_vtempconfig);
1674  if( !_CheckState() ) {
1675  RAVELOG_VERBOSE(str(boost::format("collision: %s")%_report->__str__()));
1676  return false;
1677  }
1678  if( !!params->_getstatefn ) {
1679  params->_getstatefn(_vtempconfig); // query again in order to get normalizations/joint limits
1680  }
1681  if( !!pvCheckedConfigurations ) {
1682  pvCheckedConfigurations->push_back(_vtempconfig);
1683 
1684  }
1685  if( !params->_neighstatefn(_vtempconfig,dQ,0) ) {
1686  RAVELOG_VERBOSE(str(boost::format("collision: %s")%_report->__str__()));
1687  return false;
1688  }
1689  }
1690 
1691  if( bCheckEnd && !!pvCheckedConfigurations ) {
1692  pvCheckedConfigurations->push_back(pQ1);
1693  }
1694  return true;
1695 }
1696 
1698 {
1699  _robot->GetActiveDOFWeights(weights2);
1700  for(std::vector<dReal>::iterator it = weights2.begin(); it != weights2.end(); ++it) {
1701  *it *= *it;
1702  }
1703 }
1704 
1705 dReal SimpleDistanceMetric::Eval(const std::vector<dReal>& c0, const std::vector<dReal>& c1)
1706 {
1707  std::vector<dReal> c = c0;
1708  _robot->SubtractActiveDOFValues(c,c1);
1709  dReal dist = 0;
1710  for(int i=0; i < _robot->GetActiveDOF(); i++) {
1711  dist += weights2.at(i)*c.at(i)*c.at(i);
1712  }
1713  return RaveSqrt(dist);
1714 }
1715 
1716 SimpleNeighborhoodSampler::SimpleNeighborhoodSampler(SpaceSamplerBasePtr psampler, const boost::function<dReal(const std::vector<dReal>&, const std::vector<dReal>&)>& distmetricfn) : _psampler(psampler), _distmetricfn(distmetricfn)
1717 {
1718 }
1719 
1720 bool SimpleNeighborhoodSampler::Sample(std::vector<dReal>& vNewSample, const std::vector<dReal>& vCurSample, dReal fRadius)
1721 {
1722  _psampler->SampleSequence(vNewSample);
1723  size_t dof = vCurSample.size();
1724  BOOST_ASSERT(dof==vNewSample.size() && &vNewSample != &vCurSample);
1725  dReal fDist = _distmetricfn(vNewSample,vCurSample);
1726  while(fDist > fRadius) {
1727  for (size_t i = 0; i < dof; i++) {
1728  vNewSample[i] = 0.5f*vCurSample[i]+0.5f*vNewSample[i];
1729  }
1730  fDist = _distmetricfn(vNewSample,vCurSample);
1731  }
1732  for(int iter = 0; iter < 20; ++iter) {
1733  for (size_t i = 0; i < dof; i++) {
1734  vNewSample[i] = 1.2f*vNewSample[i]-0.2f*vCurSample[i];
1735  }
1736  if(_distmetricfn(vNewSample, vCurSample) > fRadius ) {
1737  // take the previous
1738  for (size_t i = 0; i < dof; i++) {
1739  vNewSample[i] = 0.833333333333333*vNewSample[i]-0.16666666666666669*vCurSample[i];
1740  }
1741  break;
1742  }
1743  }
1744 
1745  // vNewSample.resize(lower.size());
1746  // for (size_t i = 0; i < dof; i++) {
1747  // if( sample[i] < lower[i] ) {
1748  // vNewSample[i] = lower[i];
1749  // }
1750  // else if( sample[i] > upper[i] ) {
1751  // vNewSample[i] = upper[i];
1752  // }
1753  // else {
1754  // vNewSample[i] = sample[i];
1755  // }
1756  // }
1757 
1758  return true;
1759 }
1760 
1761 bool SimpleNeighborhoodSampler::Sample(std::vector<dReal>& samples)
1762 {
1763  _psampler->SampleSequence(samples,1,IT_Closed);
1764  return samples.size()>0;
1765 }
1766 
1767 ManipulatorIKGoalSampler::ManipulatorIKGoalSampler(RobotBase::ManipulatorConstPtr pmanip, const std::list<IkParameterization>& listparameterizations, int nummaxsamples, int nummaxtries, dReal fsampleprob, bool searchfreeparameters) : _pmanip(pmanip), _nummaxsamples(nummaxsamples), _nummaxtries(nummaxtries), _fsampleprob(fsampleprob), _searchfreeparameters(searchfreeparameters)
1768 {
1769  _tempikindex = -1;
1770  _fjittermaxdist = 0;
1771  _probot = _pmanip->GetRobot();
1772  _pindexsampler = RaveCreateSpaceSampler(_probot->GetEnv(),"mt19937");
1773  int orgindex = 0;
1774  for(std::list<IkParameterization>::const_iterator it = listparameterizations.begin(); it != listparameterizations.end(); ++it) {
1775  SampleInfo s;
1776  s._orgindex = orgindex++;
1777  s._ikparam = *it;
1779  _listsamples.push_back(s);
1780  }
1781  _report.reset(new CollisionReport());
1782  pmanip->GetIkSolver()->GetFreeParameters(_vfreestart);
1783  // the halton sequence centers around 0.5, so make it center around vfreestart
1784  for(std::vector<dReal>::iterator it = _vfreestart.begin(); it != _vfreestart.end(); ++it) {
1785  *it -= 0.5;
1786  }
1787 }
1788 
1789 bool ManipulatorIKGoalSampler::Sample(std::vector<dReal>& vgoal)
1790 {
1791  IkReturnPtr ikreturn = Sample();
1792  if( !ikreturn ) {
1793  vgoal.resize(0);
1794  return false;
1795  }
1796  vgoal.swap(ikreturn->_vsolution); // won't be using the ik return anymore
1797  return true;
1798 }
1799 
1801 {
1802  std::vector<dReal> vindex;
1803  _pindexsampler->SampleSequence(vindex,1,IT_OpenEnd);
1804  if( vindex.at(0) > _fsampleprob ) {
1805  return IkReturnPtr();
1806  }
1807  if( _vikreturns.size() > 0 ) {
1808  IkReturnPtr ikreturnlocal = _vikreturns.back();
1809  _vikreturns.pop_back();
1811  if( _vikreturns.size() == 0 ) {
1812  _tempikindex = -1;
1813  }
1814  return ikreturnlocal;
1815  }
1816 
1817  for(int itry = 0; itry < _nummaxtries; ++itry ) {
1818  if( _listsamples.size() == 0 ) {
1819  return IkReturnPtr();
1820  }
1821  _pindexsampler->SampleSequence(vindex,1,IT_OpenEnd);
1822  int isampleindex = (int)(vindex.at(0)*_listsamples.size());
1823  std::list<SampleInfo>::iterator itsample = _listsamples.begin();
1824  advance(itsample,isampleindex);
1825 
1826  bool bCheckEndEffector = itsample->_ikparam.GetType() == IKP_Transform6D || (int)_pmanip->GetArmIndices().size() <= itsample->_ikparam.GetDOF();
1827  // if first grasp, quickly prune grasp is end effector is in collision
1828  IkParameterization ikparam = itsample->_ikparam;
1829  if( itsample->_numleft == _nummaxsamples && bCheckEndEffector ) {
1830  try {
1831  if( _pmanip->CheckEndEffectorCollision(ikparam,_report) ) {
1832  bool bcollision=true;
1833  if( _fjittermaxdist > 0 ) {
1834  // try jittering the end effector out
1835  RAVELOG_VERBOSE("starting jitter transform...\n");
1836 
1837  // randomly add small offset to the ik until it stops being in collision
1838  Transform tjitter;
1839  // before random sampling, first try sampling along the axes. try order z,y,x since z is most likely gravity
1840  for(int iaxis = 2; iaxis >= 0; --iaxis) {
1841  tjitter.trans = Vector();
1842  dReal delta = 0.25;
1843  for(int iiter = 2; iiter < 10; ++iiter) {
1844  tjitter.trans[iaxis] = _fjittermaxdist*delta*(iiter/2);
1845  if( iiter & 1 ) {
1846  // revert sign
1847  tjitter.trans[iaxis] = -tjitter.trans[iaxis];
1848  }
1849  IkParameterization ikparamjittered = tjitter * ikparam;
1850  try {
1851  if( !_pmanip->CheckEndEffectorCollision(ikparamjittered,_report) ) {
1852  ikparam = ikparamjittered;
1853  bcollision = false;
1854  break;
1855  }
1856  }
1857  catch(const std::exception& ex) {
1858  // ignore most likely ik failed in CheckEndEffectorCollision
1859  }
1860  }
1861  if( !bcollision ) {
1862  break;
1863  }
1864  }
1865 
1866  if( bcollision ) {
1867  // try random samples, most likely will fail...
1868  int nMaxIterations = 10;
1869  std::vector<dReal> xyzsamples(3);
1870  for(int iiter = 0; iiter < nMaxIterations; ++iiter) {
1871  _pindexsampler->SampleSequence(xyzsamples,3,IT_Closed);
1872  tjitter.trans = Vector(xyzsamples[0]-0.5f, xyzsamples[1]-0.5f, xyzsamples[2]-0.5f) * (_fjittermaxdist*2);
1873  IkParameterization ikparamjittered = tjitter * ikparam;
1874  try {
1875  if( !_pmanip->CheckEndEffectorCollision(ikparamjittered,_report) ) {
1876  ikparam = ikparamjittered;
1877  bcollision = false;
1878  break;
1879  }
1880  }
1881  catch(const std::exception& ex) {
1882  // ignore most likely ik failed in CheckEndEffectorCollision
1883  }
1884  }
1885  }
1886  }
1887  if( bcollision ) {
1888  RAVELOG_VERBOSE(str(boost::format("sampleiksolutions gripper in collision: %s.\n")%_report->__str__()));
1889  _listsamples.erase(itsample);
1890  continue;
1891  }
1892  }
1893  }
1894  catch(const std::exception& ex) {
1895  if( itsample->_ikparam.GetType() == IKP_Transform6D ) {
1896  RAVELOG_WARN(str(boost::format("CheckEndEffectorCollision threw exception: %s")%ex.what()));
1897  }
1898  else {
1899  // most likely the ik couldn't get solved
1900  RAVELOG_VERBOSE(str(boost::format("sampleiksolutions failed to solve ik: %s.\n")%ex.what()));
1901  _listsamples.erase(itsample);
1902  continue;
1903  }
1904  }
1905  }
1906 
1907  std::vector<dReal> vfree;
1908  int orgindex = itsample->_orgindex;
1909  if( _pmanip->GetIkSolver()->GetNumFreeParameters() > 0 ) {
1910 
1911  if( _searchfreeparameters ) {
1912  if( !itsample->_psampler ) {
1913  itsample->_psampler = RaveCreateSpaceSampler(_probot->GetEnv(),"halton");
1914  itsample->_psampler->SetSpaceDOF(_pmanip->GetIkSolver()->GetNumFreeParameters());
1915  }
1916  itsample->_psampler->SampleSequence(vfree,1);
1917  for(size_t i = 0; i < _vfreestart.size(); ++i) {
1918  vfree.at(i) += _vfreestart[i];
1919  if( vfree[i] < 0 ) {
1920  vfree[i] += 1;
1921  }
1922  if( vfree[i] > 1 ) {
1923  vfree[i] -= 1;
1924  }
1925  }
1926  }
1927  else {
1928  _pmanip->GetIkSolver()->GetFreeParameters(vfree);
1929  }
1930  }
1931  bool bsuccess = _pmanip->FindIKSolutions(ikparam, vfree, IKFO_CheckEnvCollisions|(bCheckEndEffector ? IKFO_IgnoreEndEffectorCollisions : 0), _vikreturns);
1932  if( --itsample->_numleft <= 0 || vfree.size() == 0 || !_searchfreeparameters ) {
1933  _listsamples.erase(itsample);
1934  }
1935 
1936  if( bsuccess ) {
1937  _tempikindex = orgindex;
1938  _listreturnedsamples.push_back(orgindex);
1939  IkReturnPtr ikreturnlocal = _vikreturns.back();
1940  _vikreturns.pop_back();
1941  if( _vikreturns.size() == 0 ) {
1942  _tempikindex = -1;
1943  }
1944  return ikreturnlocal;
1945  }
1946  }
1947  return IkReturnPtr();
1948 }
1949 
1950 bool ManipulatorIKGoalSampler::SampleAll(std::list<IkReturnPtr>& samples, int maxsamples, int maxchecksamples)
1951 {
1952  // currently this is a very slow implementation...
1953  samples.clear();
1954  int numchecked=0;
1955  while(true) {
1956  IkReturnPtr ikreturn = Sample();
1957  if( !ikreturn ) {
1958  break;
1959  }
1960  samples.push_back(ikreturn);
1961  if( maxsamples > 0 && (int)samples.size() >= maxsamples ) {
1962  return true;
1963  }
1964  RAVELOG_VERBOSE(str(boost::format("computed %d samples")%samples.size()));
1965  numchecked += 1;
1966  if( maxchecksamples > 0 && numchecked >= maxchecksamples ) {
1967  break;
1968  }
1969  }
1970  return samples.size()>0;
1971 }
1972 
1974 {
1975  BOOST_ASSERT(index >= 0 && index < (int)_listreturnedsamples.size());
1976  std::list<int>::iterator it = _listreturnedsamples.begin();
1977  advance(it,index);
1978  return *it;
1979 }
1980 
1982 {
1983  _fsampleprob = fsampleprob;
1984 }
1985 
1987 {
1988  _fjittermaxdist = maxdist;
1989 }
1990 
1991 } // planningutils
1992 } // OpenRAVE