17 #include "libopenrave.h"
18 #include <boost/lexical_cast.hpp>
23 namespace planningutils {
28 vector<dReal> curdof, newdof, deltadof, deltadof2;
29 robot->GetActiveDOFValues(curdof);
30 newdof.resize(curdof.size());
31 deltadof.resize(curdof.size(),0);
34 bool bCollision =
false;
35 bool bConstraint = !!neighstatefn;
38 boost::array<dReal,3> perturbations = { { 0,1e-5f,-1e-5f}};
39 FOREACH(itperturbation,perturbations) {
41 FOREACH(it,deltadof) {
42 *it = *itperturbation;
45 if( !neighstatefn(newdof,deltadof,0) ) {
51 for(
size_t i = 0; i < newdof.size(); ++i) {
52 newdof[i] = curdof[i]+*itperturbation;
57 if(robot->CheckSelfCollision(preport)) {
69 if( !bCollision || fRand <= 0 ) {
75 deltadof2.resize(curdof.size(),0);
76 for(
int iter = 0; iter < nMaxIterations; ++iter) {
77 for(
size_t j = 0; j < newdof.size(); ++j) {
81 bool bConstraintFailed =
false;
82 FOREACH(itperturbation,perturbations) {
83 for(
size_t j = 0; j < deltadof.size(); ++j) {
84 deltadof2[j] = deltadof[j] + *itperturbation;
89 if( !neighstatefn(newdof,deltadof2,0) ) {
90 if( *itperturbation != 0 ) {
91 RAVELOG_DEBUG(str(boost::format(
"constraint function failed, pert=%e\n")%*itperturbation));
93 bConstraintFailed =
true;
98 for(
size_t j = 0; j < deltadof.size(); ++j) {
99 newdof[j] = curdof[j] + deltadof2[j];
103 if(robot->CheckSelfCollision() || robot->GetEnv()->CheckCollision(
KinBodyConstPtr(robot)) ) {
108 if( !bCollision && !bConstraintFailed ) {
113 if( !neighstatefn(newdof,deltadof,0) ) {
114 RAVELOG_WARN(
"neighstatefn failed, but previously succeeded\n");
119 for(
size_t j = 0; j < deltadof.size(); ++j) {
120 newdof[j] = curdof[j] + deltadof[j];
136 Transform transorig = pbody->GetTransform();
140 if( iter > nMaxIterations ) {
142 pbody->SetTransform(transorig);
145 if( iter > 0 && fJitter <= 0 ) {
147 pbody->SetTransform(transorig);
151 pbody->SetTransform(transnew);
178 dReal fresolutionmean = 0;
180 fresolutionmean += *it;
182 fresolutionmean /=
_parameters->_vConfigResolution.size();
184 dReal fthresh = 5e-5f;
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) ) {
197 for(
size_t i = 0; i <
_parameters->_vConfigVelocityLimit.size(); ++i) {
205 BOOST_ASSERT(vdata.size() == newq.size());
208 for(
size_t i = 0; i < vdiff.size(); ++i) {
223 if( !!
_parameters->_checkpathconstraintsfn && trajectory->GetNumWaypoints() >= 2 ) {
225 if( trajectory->GetDuration() > 0 && samplingstep > 0 ) {
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 ) {
233 trajectory->Sample(vdata,ftime+samplingstep,
_parameters->_configurationspecification);
234 trajectory->Sample(vdatavel,ftime+samplingstep,velspec);
237 for(
size_t i = 0; i <
_parameters->_vConfigVelocityLimit.size(); ++i) {
238 dReal velthresh =
_parameters->_vConfigVelocityLimit.at(i)*samplingstep+fthresh;
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);
252 vector<dReal> vtemp = *itprevconfig;
253 if( !
_parameters->_neighstatefn(vtemp,deltaq,0) ) {
261 itprevconfig=itcurconfig;
264 vprevdatavel=vdatavel;
268 for(
size_t i = 0; i < trajectory->GetNumWaypoints(); ++i) {
269 trajectory->GetWaypoint(i,vdata,
_parameters->_configurationspecification);
281 ofstream f(filename.c_str());
282 f << std::setprecision(std::numeric_limits<dReal>::digits10+1);
283 trajectory->serialize(f);
293 EnvironmentMutex::scoped_lock lockenv(trajectory->GetEnv()->GetMutex());
296 newparams->SetConfigurationSpecification(trajectory->GetEnv(), trajectory->GetConfigurationSpecification().GetTimeDerivativeSpecification(0));
297 parameters = newparams;
305 if( traj->GetNumWaypoints() == 1 ) {
310 traj->GetWaypoints(0,traj->GetNumWaypoints(),data,spec);
312 traj->Insert(0,data);
317 EnvironmentMutex::scoped_lock lockenv(env->GetMutex());
321 params->SetRobotActiveJoints(probot);
322 FOREACH(it,params->_vConfigVelocityLimit) {
325 FOREACH(it,params->_vConfigAccelerationLimit) {
326 *it *= fmaxaccelmult;
329 params->_setstatefn.clear();
330 params->_checkpathconstraintsfn.clear();
333 params->_sPostProcessingPlanner =
"";
334 params->_hastimestamps = hastimestamps;
335 params->_sExtraParameters += plannerparameters;
336 if( !planner->InitPlan(probot,params) ) {
352 std::string plannername = _plannername.size() > 0 ? _plannername :
"parabolicsmoother";
354 EnvironmentMutex::scoped_lock lockenv(robot->GetEnv()->GetMutex());
357 params->SetRobotActiveJoints(
_robot);
358 params->_sPostProcessingPlanner =
"";
359 params->_hastimestamps =
false;
360 params->_sExtraParameters += plannerparameters;
361 if( !_planner->InitPlan(
_robot,params) ) {
369 if( traj->GetNumWaypoints() == 1 ) {
374 traj->GetWaypoints(0,traj->GetNumWaypoints(),data,spec);
376 traj->Insert(0,data);
394 std::string plannername = _plannername.size() > 0 ? _plannername :
"parabolicretimer";
396 EnvironmentMutex::scoped_lock lockenv(robot->GetEnv()->GetMutex());
399 params->SetRobotActiveJoints(
_robot);
400 params->_sPostProcessingPlanner =
"";
401 params->_hastimestamps =
false;
402 params->_setstatefn.clear();
403 params->_checkpathconstraintsfn.clear();
404 params->_sExtraParameters = plannerparameters;
405 if( !_planner->InitPlan(
_robot,params) ) {
413 if( traj->GetNumWaypoints() == 1 ) {
418 traj->GetWaypoints(0,traj->GetNumWaypoints(),data,spec);
420 traj->Insert(0,data);
425 if( parameters->_hastimestamps != hastimestamps ) {
437 if( traj->GetNumWaypoints() == 1 ) {
442 traj->GetWaypoints(0,traj->GetNumWaypoints(),data,spec);
444 traj->Insert(0,data);
448 EnvironmentMutex::scoped_lock lockenv(traj->GetEnv()->GetMutex());
451 params->SetConfigurationSpecification(traj->GetEnv(),traj->GetConfigurationSpecification().GetTimeDerivativeSpecification(0));
452 FOREACH(it,params->_vConfigVelocityLimit) {
455 FOREACH(it,params->_vConfigAccelerationLimit) {
456 *it *= fmaxaccelmult;
459 params->_setstatefn.clear();
460 params->_checkpathconstraintsfn.clear();
463 params->_sPostProcessingPlanner =
"";
464 params->_hastimestamps = hastimestamps;
465 params->_sExtraParameters += plannerparameters;
480 static void diffstatefn(std::vector<dReal>& q1,
const std::vector<dReal>& q2,
const std::vector<int> vrotaxes)
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() ) {
497 pbody->SetTransform(t);
512 return TransformDistance2(t0, t1, 0.3);
515 void _SetAffineState(
const std::vector<dReal>& v,
const std::list< boost::function<
void(std::vector<dReal>::const_iterator) > >& listsetfunctions)
517 FOREACHC(itfn,listsetfunctions) {
522 void _GetAffineState(std::vector<dReal>& v,
size_t expectedsize,
const std::list< boost::function<
void(std::vector<dReal>::iterator) > >& listgetfunctions)
524 v.resize(expectedsize);
525 FOREACHC(itfn,listgetfunctions) {
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)
533 FOREACHC(itfn,listdistfunctions) {
534 dist += (*itfn)(v0.begin(), v1.begin());
543 _savedvalues.resize(dof);
545 BOOST_ASSERT(!!_setfn);
548 _setfn(_savedvalues);
553 vector<dReal> _savedvalues;
559 if( traj->GetNumWaypoints() == 1 ) {
564 traj->GetWaypoints(0,traj->GetNumWaypoints(),data,spec);
566 traj->Insert(0,data);
570 EnvironmentMutex::scoped_lock lockenv(traj->GetEnv()->GetMutex());
572 if( newspec.GetDOF() != (int)maxvelocities.size() || newspec.GetDOF() != (int)maxaccelerations.size() ) {
579 params->_sPostProcessingPlanner =
"";
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;
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;
598 FOREACHC(itgroup,newspec._vgroups) {
599 if( itgroup->name.size() >= 16 && itgroup->name.substr(0,16) ==
"affine_transform" ) {
602 stringstream ss(itgroup->name.substr(16));
603 ss >> tempname >> affinedofs;
604 BOOST_ASSERT( !!ss );
605 KinBodyPtr pbody = traj->GetEnv()->GetKinBody(tempname);
611 ss >> vaxis.x >> vaxis.y >> vaxis.z;
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));
619 else if( itgroup->name.size() >= 14 && itgroup->name.substr(0,14) ==
"ikparam_values" ) {
621 stringstream ss(itgroup->name.substr(14));
637 boost::shared_ptr<PlannerStateSaver> statesaver;
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));
643 std::list<KinBodyPtr> listCheckCollisions; listCheckCollisions.push_back(robot);
646 statesaver.reset(
new PlannerStateSaver(newspec.GetDOF(), params->_setstatefn, params->_getstatefn));
650 params->_setstatefn.clear();
651 params->_getstatefn.clear();
652 params->_checkpathconstraintsfn.clear();
655 params->_diffstatefn = boost::bind(
diffstatefn,_1,_2,vrotaxes);
657 params->_hastimestamps = hastimestamps;
658 params->_multidofinterp = 2;
659 params->_sExtraParameters = plannerparameters;
671 if( plannername.size() > 0 ) {
682 if( plannername.size() == 0 ) {
683 if(
_plannername !=
string(
"parabolictrajectoryretimer") ) {
708 if( traj->GetNumWaypoints() == 1 ) {
713 traj->GetWaypoints(0,traj->GetNumWaypoints(),data,spec);
715 traj->Insert(0,data);
720 EnvironmentMutex::scoped_lock lockenv(env->GetMutex());
722 if( trajspec.GetDOF() != (int)maxvelocities.size() || trajspec.GetDOF() != (int)maxaccelerations.size() ) {
729 parameters->_sPostProcessingPlanner =
"";
730 parameters->_setstatefn.clear();
731 parameters->_getstatefn.clear();
732 parameters->_checkpathconstraintsfn.clear();
734 parameters->_multidofinterp = 2;
742 bool bInitPlan=
false;
743 if( bInitPlan || CompareRealVectors(parameters->_vConfigVelocityLimit, maxvelocities, g_fEpsilonLinear) != 0 ) {
747 if( bInitPlan || CompareRealVectors(parameters->_vConfigAccelerationLimit, maxaccelerations, g_fEpsilonLinear) != 0 ) {
748 parameters->_vConfigAccelerationLimit = maxaccelerations;
751 if( bInitPlan || parameters->_configurationspecification != trajspec ) {
752 parameters->_configurationspecification = trajspec;
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;
767 std::vector<int> vrotaxes;
769 FOREACHC(itgroup,trajspec._vgroups) {
770 if( itgroup->name.size() >= 16 && itgroup->name.substr(0,16) ==
"affine_transform" ) {
773 stringstream ss(itgroup->name.substr(16));
774 ss >> tempname >> affinedofs;
775 BOOST_ASSERT( !!ss );
782 ss >> vaxis.x >> vaxis.y >> vaxis.z;
786 else if( itgroup->name.size() >= 14 && itgroup->name.substr(0,14) ==
"ikparam_values" ) {
788 stringstream ss(itgroup->name.substr(14));
802 parameters->_diffstatefn = boost::bind(
diffstatefn,_1,_2,vrotaxes);
804 if( parameters->_hastimestamps != hastimestamps ) {
805 parameters->_hastimestamps = hastimestamps;
815 stringstream ss; ss << trajspec;
825 return _PlanActiveDOFTrajectory(traj,robot,
false,fmaxvelmult,fmaxaccelmult,plannername.size() > 0 ? plannername :
"parabolicsmoother",
true,plannerparameters);
830 return _PlanAffineTrajectory(traj, maxvelocities, maxaccelerations,
false, plannername.size() > 0 ? plannername :
"parabolicsmoother",
true, plannerparameters);
835 return _PlanTrajectory(traj,
false,fmaxvelmult,fmaxaccelmult,plannername.size() > 0 ? plannername :
"parabolicsmoother",
true,plannerparameters);
840 if( forceplanner.size() > 0 ) {
843 std::string interpolation;
845 FOREACHC(itgroup,traj->GetConfigurationSpecification()._vgroups) {
846 if( itgroup->name.size() >= 12 && itgroup->name.substr(0,12) ==
"joint_values" ) {
847 interpolation = itgroup->interpolation;
850 else if( itgroup->name.size() >= 16 && itgroup->name.substr(0,16) ==
"affine_transform" ) {
851 interpolation = itgroup->interpolation;
853 else if( itgroup->name.size() >= 14 && itgroup->name.substr(0,14) ==
"ikparam_values" ) {
854 interpolation = itgroup->interpolation;
858 if( interpolation ==
"quadratic" ) {
859 return "parabolictrajectoryretimer";
861 else if( interpolation ==
"linear" ) {
862 return "lineartrajectoryretimer";
865 return "lineartrajectoryretimer";
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);
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() ) {
897 if( itgroup->interpolation.size() > 0 ) {
898 it->interpolation = itgroup->interpolation;
899 interpolation = itgroup->interpolation;
902 newspec.AddDerivativeGroups(1,
false);
904 vector<dReal> vwaypointstart, vwaypointend, vtargetvalues;
905 if( waypointindex == 0 ) {
906 vwaypointstart.resize(newspec.GetDOF());
908 if( dofvalues.size() == dofvelocities.size() ) {
909 ConfigurationSpecification::ConvertData(vwaypointstart.begin(), newspec.ConvertToVelocitySpecification(), dofvelocities.begin(), robot->GetActiveConfigurationSpecification().ConvertToVelocitySpecification(), 1, traj->GetEnv(),
false);
911 traj->GetWaypoint(0,vwaypointend, newspec);
912 traj->GetWaypoint(0,vtargetvalues);
914 else if( waypointindex == (
int)traj->GetNumWaypoints() ) {
915 traj->GetWaypoint(waypointindex-1,vtargetvalues);
916 traj->GetWaypoint(waypointindex-1,vwaypointstart, newspec);
918 vwaypointend.resize(newspec.GetDOF());
920 if( dofvalues.size() == dofvelocities.size() ) {
921 ConfigurationSpecification::ConvertData(vwaypointend.begin(), newspec.ConvertToVelocitySpecification(), dofvelocities.begin(), robot->GetActiveConfigurationSpecification().ConvertToVelocitySpecification(), 1, traj->GetEnv(),
false);
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";
937 else if( interpolation.size() == 0 || interpolation ==
"quadratic" ) {
938 newplannername =
"parabolictrajectoryretimer";
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);
952 trajinitial->GetWaypoints(0,trajinitial->GetNumWaypoints(),vwaypointstart);
955 ConfigurationSpecification::ConvertData(vtargetvalues.begin(), traj->GetConfigurationSpecification(), vwaypointstart.begin(), trajinitial->GetConfigurationSpecification(), trajinitial->GetNumWaypoints(), traj->GetEnv(),
false);
957 if( waypointindex == 0 ) {
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);
969 vtargetvalues.erase(vtargetvalues.begin(), vtargetvalues.begin()+targetdof);
970 traj->Insert(waypointindex,vtargetvalues,
false);
976 if( index != (
int)traj->GetNumWaypoints() ) {
984 params->SetConfigurationSpecification(traj->GetEnv(),specpos);
985 FOREACH(it,params->_vConfigVelocityLimit) {
988 FOREACH(it,params->_vConfigAccelerationLimit) {
989 *it *= fmaxaccelmult;
992 params->_hasvelocities =
true;
993 params->_hastimestamps =
false;
995 EnvironmentMutex::scoped_lock lockenv(traj->GetEnv()->GetMutex());
1001 dReal fSamplingTime = 0.01;
1002 dReal fTimeBuffer = 0.01;
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());
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;
1026 for(
int iwaypoint = 0; iwaypoint < N; ++iwaypoint) {
1027 traj->GetWaypoint(N-1-iwaypoint,vwaypoint);
1028 dReal deltatime = 0;
1029 if( iTimeIndex >= 0 ) {
1031 deltatime = vwaypoint.at(iTimeIndex);
1032 vwaypoint.at(iTimeIndex) = 0;
1037 ptesttraj->Init(traj->GetConfigurationSpecification());
1038 ptesttraj->Insert(0,vwaypoint);
1039 ptesttraj->Insert(1,vstartdata,spectotal);
1043 dReal fNewDuration = fRemainingDuration+ptesttraj->GetDuration();
1044 if( fNewDuration < fBestDuration ) {
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);
1051 bInCollision =
true;
1054 vprevpoint = vwaypoint;
1056 if( !bInCollision ) {
1057 iBestInsertionWaypoint = N-1-iwaypoint;
1058 fBestDuration = fNewDuration;
1059 swap(pBestTrajectory,ptesttraj);
1060 if( iwaypoint > 0 && fBestDuration < fOriginalTime+fTimeBuffer ) {
1065 if( !!pBestTrajectory ) {
1073 if( !!pBestTrajectory ) {
1078 fRemainingDuration -= deltatime;
1081 if( !pBestTrajectory ) {
1084 if( fBestDuration > fOriginalTime+fTimeBuffer ) {
1085 RAVELOG_WARN(str(boost::format(
"new trajectory is greater than expected time %f > %f \n")%fBestDuration%fOriginalTime));
1088 traj->Remove(iBestInsertionWaypoint+1,traj->GetNumWaypoints());
1096 pBestTrajectory->GetWaypoints(1,pBestTrajectory->GetNumWaypoints(),vwaypoint,traj->GetConfigurationSpecification());
1097 traj->Insert(iBestInsertionWaypoint+1,vwaypoint);
1098 dReal fNewDuration = traj->GetDuration();
1104 if( traj->GetConfigurationSpecification() != spec ) {
1105 size_t numpoints = traj->GetConfigurationSpecification().GetDOF() > 0 ? traj->GetNumWaypoints() : 0;
1107 if( numpoints > 0 ) {
1108 traj->GetWaypoints(0,numpoints,data,spec);
1111 if( numpoints > 0 ) {
1112 traj->Insert(0,data);
1121 for(
int i = 1; i <= maxderiv; ++i) {
1124 std::vector<dReal> data;
1125 traj->GetWaypoints(0,traj->GetNumWaypoints(),data, newspec);
1126 if( data.size() == 0 ) {
1127 traj->Init(newspec);
1130 int numpoints = (int)data.size()/newspec.
GetDOF();
1133 std::vector<ConfigurationSpecification::Group>::const_iterator itdeltatimegroup = newspec.
FindCompatibleGroup(
"deltatime");
1134 if(itdeltatimegroup == newspec.
_vgroups.end() ) {
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);
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);
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;
1167 traj->Init(newspec);
1168 traj->Insert(0,data);
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;
1187 else if( itgroup->name ==
"deltatime" ) {
1188 timeoffset = itgroup->offset;
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);
1204 targetdata.at(j) = 0;
1208 *(ittarget+j) = -*(itsource+j);
1212 *(ittarget+j) = *(itsource+j);
1216 if( timeoffset >= 0 ) {
1217 *(ittarget+timeoffset) = prevdeltatime;
1218 prevdeltatime = *(itsource+timeoffset);
1223 traj->Init(sourcetraj->GetConfigurationSpecification());
1224 traj->Insert(0,targetdata);
1225 traj->SetDescription(sourcetraj->GetDescription());
1232 if( listtrajectories.size() == 0 ) {
1235 if( listtrajectories.size() == 1 ) {
1236 presulttraj =
RaveCreateTrajectory(listtrajectories.front()->GetEnv(),listtrajectories.front()->GetXMLId());
1237 presulttraj->Clone(listtrajectories.front(),0);
1242 vector<dReal> vpointdata;
1243 vector<dReal> vtimes; vtimes.reserve(listtrajectories.front()->GetNumWaypoints());
1245 FOREACHC(ittraj,listtrajectories) {
1249 totaldof += trajspec.
GetDOF()-1;
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);
1264 vector<ConfigurationSpecification::Group>::const_iterator itwaypointgroup = spec.
FindCompatibleGroup(
"iswaypoint",
true);
1265 vector<dReal> vwaypoints;
1266 if( itwaypointgroup != spec.
_vgroups.end() ) {
1268 vwaypoints.resize(vtimes.size(),0);
1271 if( totaldof != spec.
GetDOF() ) {
1274 presulttraj =
RaveCreateTrajectory(listtrajectories.front()->GetEnv(),listtrajectories.front()->GetXMLId());
1275 presulttraj->Init(spec);
1277 if( vtimes.size() == 0 ) {
1282 vector<dReal> vtemp, vnewdata;
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;
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];
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];
1313 sdesc << (*ittraj)->GetDescription() << endl;
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];
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];
1326 presulttraj->Insert(0,vnewdata);
1327 presulttraj->SetDescription(sdesc.str());
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()) {
1341 itdh->joint = *itjoint;
1342 itdh->parentindex = -1;
1344 std::vector<KinBody::JointPtr> vparentjoints;
1345 pbody->GetChain(0,plink->GetIndex(),vparentjoints);
1346 if( vparentjoints.size() > 0 ) {
1348 int index = (int)vparentjoints.size()-1;
1349 while(index >= 0 && vparentjoints[index]->GetJointIndex() < 0 ) {
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();
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);
1366 itdh->alpha =
RaveAtan2(fsinalpha, vlocalaxis.z);
1370 vlocalx.x = vlocalanchor.x;
1371 vlocalx.y = vlocalanchor.y;
1372 vlocalx.normalize3();
1379 vlocalx /= fsinalpha;
1381 itdh->d = vlocalanchor.z;
1382 itdh->theta =
RaveAtan2(vlocalx.y,vlocalx.x);
1383 itdh->a = vlocalanchor.dot3(vlocalx);
1395 dReal xcomponent = (1-2*worldrotquat.z*worldrotquat.z-2*worldrotquat.w*worldrotquat.w);
1396 if( xcomponent < 0 ) {
1404 itdh->alpha = -itdh->alpha;
1409 itdh->alpha = -itdh->alpha;
1418 itdh->transform = tparent*Z_i*X_i;
1449 if( (*itbody)->CheckSelfCollision(
_report) ) {
1470 bool bCheckEnd=
false;
1473 start = 1; bCheckEnd =
false;
1476 start = 1; bCheckEnd =
true;
1479 start = 0; bCheckEnd =
false;
1482 start = 0; bCheckEnd =
true;
1491 params->_setstatefn(pQ1);
1495 if( robot->CheckSelfCollision(
_report) ) {
1502 params->_diffstatefn(
dQ,pQ0);
1503 int i, numSteps = 1;
1504 std::vector<dReal>::const_iterator itres = params->_vConfigResolution.begin();
1507 for (i = 0; i < params->GetDOF(); i++,itres++) {
1510 steps = (int)(fabs(
dQ[i]) / *itres);
1513 steps = (int)(fabs(
dQ[i]) * 100);
1515 totalsteps += steps;
1516 if (steps > numSteps) {
1520 if( totalsteps == 0 && start > 0) {
1521 if( bCheckEnd && !!pvCheckedConfigurations ) {
1522 pvCheckedConfigurations->push_back(pQ1);
1528 params->_setstatefn(pQ0);
1532 if( robot->CheckSelfCollision(
_report) ) {
1535 if( !!pvCheckedConfigurations ) {
1536 pvCheckedConfigurations->push_back(pQ0);
1542 for(std::vector<dReal>::iterator it =
dQ.begin(); it !=
dQ.end(); ++it) {
1549 for (i = 0; i < params->GetDOF(); i++) {
1558 for (
int f = start; f < numSteps; f++) {
1563 if( robot->CheckSelfCollision() ) {
1566 if( !!params->_getstatefn ) {
1569 if( !!pvCheckedConfigurations ) {
1577 if( bCheckEnd && !!pvCheckedConfigurations ) {
1578 pvCheckedConfigurations->push_back(pQ1);
1593 bool bCheckEnd=
false;
1596 start = 1; bCheckEnd =
false;
1599 start = 1; bCheckEnd =
true;
1602 start = 0; bCheckEnd =
false;
1605 start = 0; bCheckEnd =
true;
1614 params->_setstatefn(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());
1628 for (i = 0; i < params->GetDOF(); i++,itres++) {
1631 steps = (int)(fabs(
dQ[i]) / *itres);
1634 steps = (int)(fabs(
dQ[i]) * 100);
1636 totalsteps += steps;
1637 if (steps > numSteps) {
1641 if( totalsteps == 0 && start > 0 ) {
1646 params->_setstatefn(pQ0);
1655 for(std::vector<dReal>::iterator it =
dQ.begin(); it !=
dQ.end(); ++it) {
1662 for (i = 0; i < params->GetDOF(); i++) {
1672 for (
int f = start; f < numSteps; f++) {
1678 if( !!params->_getstatefn ) {
1681 if( !!pvCheckedConfigurations ) {
1691 if( bCheckEnd && !!pvCheckedConfigurations ) {
1692 pvCheckedConfigurations->push_back(pQ1);
1700 for(std::vector<dReal>::iterator it =
weights2.begin(); it !=
weights2.end(); ++it) {
1707 std::vector<dReal> c = c0;
1708 _robot->SubtractActiveDOFValues(c,c1);
1710 for(
int i=0; i <
_robot->GetActiveDOF(); i++) {
1711 dist +=
weights2.at(i)*c.at(i)*c.at(i);
1723 size_t dof = vCurSample.size();
1724 BOOST_ASSERT(dof==vNewSample.size() && &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];
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];
1738 for (
size_t i = 0; i < dof; i++) {
1739 vNewSample[i] = 0.833333333333333*vNewSample[i]-0.16666666666666669*vCurSample[i];
1764 return samples.size()>0;
1774 for(std::list<IkParameterization>::const_iterator it = listparameterizations.begin(); it != listparameterizations.end(); ++it) {
1782 pmanip->GetIkSolver()->GetFreeParameters(
_vfreestart);
1796 vgoal.swap(ikreturn->_vsolution);
1802 std::vector<dReal> vindex;
1814 return ikreturnlocal;
1822 int isampleindex = (int)(vindex.at(0)*
_listsamples.size());
1823 std::list<SampleInfo>::iterator itsample =
_listsamples.begin();
1824 advance(itsample,isampleindex);
1826 bool bCheckEndEffector = itsample->_ikparam.GetType() ==
IKP_Transform6D || (int)
_pmanip->GetArmIndices().size() <= itsample->_ikparam.GetDOF();
1829 if( itsample->_numleft ==
_nummaxsamples && bCheckEndEffector ) {
1832 bool bcollision=
true;
1840 for(
int iaxis = 2; iaxis >= 0; --iaxis) {
1841 tjitter.trans =
Vector();
1843 for(
int iiter = 2; iiter < 10; ++iiter) {
1847 tjitter.trans[iaxis] = -tjitter.trans[iaxis];
1851 if( !
_pmanip->CheckEndEffectorCollision(ikparamjittered,
_report) ) {
1852 ikparam = ikparamjittered;
1857 catch(
const std::exception& ex) {
1868 int nMaxIterations = 10;
1869 std::vector<dReal> xyzsamples(3);
1870 for(
int iiter = 0; iiter < nMaxIterations; ++iiter) {
1872 tjitter.trans =
Vector(xyzsamples[0]-0.5f, xyzsamples[1]-0.5f, xyzsamples[2]-0.5f) * (
_fjittermaxdist*2);
1875 if( !
_pmanip->CheckEndEffectorCollision(ikparamjittered,
_report) ) {
1876 ikparam = ikparamjittered;
1881 catch(
const std::exception& ex) {
1894 catch(
const std::exception& ex) {
1896 RAVELOG_WARN(str(boost::format(
"CheckEndEffectorCollision threw exception: %s")%ex.what()));
1900 RAVELOG_VERBOSE(str(boost::format(
"sampleiksolutions failed to solve ik: %s.\n")%ex.what()));
1907 std::vector<dReal> vfree;
1908 int orgindex = itsample->_orgindex;
1909 if(
_pmanip->GetIkSolver()->GetNumFreeParameters() > 0 ) {
1912 if( !itsample->_psampler ) {
1914 itsample->_psampler->SetSpaceDOF(
_pmanip->GetIkSolver()->GetNumFreeParameters());
1916 itsample->_psampler->SampleSequence(vfree,1);
1919 if( vfree[i] < 0 ) {
1922 if( vfree[i] > 1 ) {
1928 _pmanip->GetIkSolver()->GetFreeParameters(vfree);
1944 return ikreturnlocal;
1960 samples.push_back(ikreturn);
1961 if( maxsamples > 0 && (
int)samples.size() >= maxsamples ) {
1964 RAVELOG_VERBOSE(str(boost::format(
"computed %d samples")%samples.size()));
1966 if( maxchecksamples > 0 && numchecked >= maxchecksamples ) {
1970 return samples.size()>0;