parabolicsmoother.cpp | parabolicsmoother.cpp | |||
---|---|---|---|---|
skipping to change at line 203 | skipping to change at line 203 | |||
ConfigurationSpecification::ConvertData(ittargetdata,ne wspec,vconfig.begin(),velspec,1,GetEnv(),false); | ConfigurationSpecification::ConvertData(ittargetdata,ne wspec,vconfig.begin(),velspec,1,GetEnv(),false); | |||
*(ittargetdata+timeoffset) = vswitchtimes[i]-prevtime; | *(ittargetdata+timeoffset) = vswitchtimes[i]-prevtime; | |||
*(ittargetdata+waypointoffset) = dReal(i+1==vswitchtime s.size()); | *(ittargetdata+waypointoffset) = dReal(i+1==vswitchtime s.size()); | |||
ittargetdata += newspec.GetDOF(); | ittargetdata += newspec.GetDOF(); | |||
prevtime = vswitchtimes[i]; | prevtime = vswitchtimes[i]; | |||
} | } | |||
ptraj->Insert(ptraj->GetNumWaypoints(),vtrajpoints); | ptraj->Insert(ptraj->GetNumWaypoints(),vtrajpoints); | |||
} | } | |||
BOOST_ASSERT(RaveFabs(dynamicpath.GetTotalTime()-ptraj->GetDura tion())<0.001); | BOOST_ASSERT(RaveFabs(dynamicpath.GetTotalTime()-ptraj->GetDura tion())<0.001); | |||
RAVELOG_DEBUG(str(boost::format("after shortcutting %d times: p ath waypoints=%d, traj waypoints=%d, length=%fs")%numshortcuts%dynamicpath. ramps.size()%ptraj->GetNumWaypoints()%dynamicpath.GetTotalTime())); | RAVELOG_DEBUG(str(boost::format("after shortcutting %d times: p ath waypoints=%d, traj waypoints=%d, traj time=%fs")%numshortcuts%dynamicpa th.ramps.size()%ptraj->GetNumWaypoints()%dynamicpath.GetTotalTime())); | |||
} | } | |||
catch (const openrave_exception& ex) { | catch (const std::exception& ex) { | |||
string filename = str(boost::format("%s/failedsmoothing%d.xml") %RaveGetHomeDirectory()%(RaveRandomInt()%1000)); | string filename = str(boost::format("%s/failedsmoothing%d.xml") %RaveGetHomeDirectory()%(RaveRandomInt()%1000)); | |||
RAVELOG_WARN(str(boost::format("parabolic planner failed: %s, w riting original trajectory to %s")%ex.what()%filename)); | RAVELOG_WARN(str(boost::format("parabolic planner failed: %s, w riting original trajectory to %s")%ex.what()%filename)); | |||
ofstream f(filename.c_str()); | ofstream f(filename.c_str()); | |||
f << std::setprecision(std::numeric_limits<dReal>::digits10+1); | f << std::setprecision(std::numeric_limits<dReal>::digits10+1); | |||
ptraj->serialize(f); | ptraj->serialize(f); | |||
return PS_Failed; | return PS_Failed; | |||
} | } | |||
RAVELOG_DEBUG(str(boost::format("path optimizing - computation time =%fs\n")%(0.001f*(float)(GetMilliTime()-basetime)))); | RAVELOG_DEBUG(str(boost::format("path optimizing - computation time =%fs\n")%(0.001f*(float)(GetMilliTime()-basetime)))); | |||
return PS_HasSolution; | return PS_HasSolution; | |||
} | } | |||
End of changes. 2 change blocks. | ||||
2 lines changed or deleted | 2 lines changed or added | |||
This html diff was produced by rfcdiff 1.41. The latest version is available from http://tools.ietf.org/tools/rfcdiff/ |