12 #include <boost/format.hpp>
16 using namespace OpenRAVE;
19 namespace cppexamples {
21 class PlanningPlannerExample :
public OpenRAVEExample
32 string scenefilename =
"data/hanoi_complex2.env.xml";
34 penv->Load(scenefilename);
36 vector<RobotBasePtr> vrobots;
37 penv->GetRobots(vrobots);
41 for(
size_t i = 1; i < probot->GetManipulators().size(); ++i) {
42 if( pmanip->GetArmIndices().size() < probot->GetManipulators()[i]->GetArmIndices().size() ) {
43 pmanip = probot->GetManipulators()[i];
46 RAVELOG_INFO(str(boost::format(
"planning with manipulator %s\n")%pmanip->GetName()));
52 UserDataPtr handle = planner->RegisterPlanCallback(boost::bind(&PlanningPlannerExample::PlanCallback,
this,_1));
57 EnvironmentMutex::scoped_lock lock(penv->GetMutex());
59 probot->SetActiveDOFs(pmanip->GetArmIndices());
62 params->_nMaxIterations = 4000;
63 params->SetRobotActiveJoints(probot);
64 params->vgoalconfig.resize(probot->GetActiveDOF());
66 vector<dReal> vlower,vupper;
67 probot->GetActiveDOFLimits(vlower,vupper);
73 for(
size_t i = 0; i < vlower.size(); ++i) {
74 params->vgoalconfig[i] = vlower[i] + (vupper[i]-vlower[i])*
RaveRandomFloat();
76 probot->SetActiveDOFValues(params->vgoalconfig);
77 if( !penv->CheckCollision(probot) && !probot->CheckSelfCollision() ) {
85 probot->GetActiveDOFValues(params->vinitialconfig);
86 if( !planner->InitPlan(probot,params) ) {
92 if( !planner->PlanPath(ptraj) ) {
100 vector<RaveVector<float> > vpoints;
101 vector<dReal> vtrajdata;
102 for(
dReal ftime = 0; ftime <= ptraj->GetDuration(); ftime += 0.01) {
103 ptraj->Sample(vtrajdata,ftime,probot->GetActiveConfigurationSpecification());
104 probot->SetActiveDOFValues(vtrajdata);
105 vpoints.push_back(pmanip->GetEndEffectorTransform().trans);
107 pgraph = penv->drawlinestrip(&vpoints[0].x,vpoints.size(),
sizeof(vpoints[0]),1.0f);
111 probot->GetController()->SetPath(ptraj);
115 while(!probot->GetController()->IsDone() && IsOk()) {
116 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
124 int main(
int argc,
char ** argv)
127 return example.
main(argc,argv);