openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
orplanning_planner.cpp
Go to the documentation of this file.
1 
9 #include <openrave-core.h>
10 #include <vector>
11 #include <sstream>
12 #include <boost/format.hpp>
13 
14 #include "orexample.h"
15 
16 using namespace OpenRAVE;
17 using namespace std;
18 
19 namespace cppexamples {
20 
21 class PlanningPlannerExample : public OpenRAVEExample
22 {
23 public:
24 
25  PlannerAction PlanCallback(const PlannerBase::PlannerProgress& progress)
26  {
27  // plan callback
28  return PA_None;
29  }
30 
31  virtual void demothread(int argc, char ** argv) {
32  string scenefilename = "data/hanoi_complex2.env.xml";
34  penv->Load(scenefilename);
35 
36  vector<RobotBasePtr> vrobots;
37  penv->GetRobots(vrobots);
38  RobotBasePtr probot = vrobots.at(0);
39  // find the longest manipulator chain to move
40  RobotBase::ManipulatorPtr pmanip = probot->GetManipulators().at(0);
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];
44  }
45  }
46  RAVELOG_INFO(str(boost::format("planning with manipulator %s\n")%pmanip->GetName()));
47 
48  // create the planner parameters
49  PlannerBasePtr planner = RaveCreatePlanner(penv,"birrt");
50 
51  // register an optional function to be called for every planner iteration
52  UserDataPtr handle = planner->RegisterPlanCallback(boost::bind(&PlanningPlannerExample::PlanCallback,this,_1));
53 
54  while(IsOk()) {
55  GraphHandlePtr pgraph;
56  {
57  EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
58 
59  probot->SetActiveDOFs(pmanip->GetArmIndices());
60 
62  params->_nMaxIterations = 4000; // max iterations before failure
63  params->SetRobotActiveJoints(probot); // set planning configuration space to current active dofs
64  params->vgoalconfig.resize(probot->GetActiveDOF());
65 
66  vector<dReal> vlower,vupper;
67  probot->GetActiveDOFLimits(vlower,vupper);
68 
69  // find a set of free joint values for the robot
70  {
71  RobotBase::RobotStateSaver saver(probot); // save the state
72  while(1) {
73  for(size_t i = 0; i < vlower.size(); ++i) {
74  params->vgoalconfig[i] = vlower[i] + (vupper[i]-vlower[i])*RaveRandomFloat();
75  }
76  probot->SetActiveDOFValues(params->vgoalconfig);
77  if( !penv->CheckCollision(probot) && !probot->CheckSelfCollision() ) {
78  break;
79  }
80  }
81  // robot state is restored
82  }
83 
84  RAVELOG_INFO("starting to plan\n");
85  probot->GetActiveDOFValues(params->vinitialconfig);
86  if( !planner->InitPlan(probot,params) ) {
87  continue;
88  }
89 
90  // create a new output trajectory
91  TrajectoryBasePtr ptraj = RaveCreateTrajectory(penv,"");
92  if( !planner->PlanPath(ptraj) ) {
93  RAVELOG_WARN("plan failed, trying again\n");
94  continue;
95  }
96 
97  // draw the end effector of the trajectory
98  {
99  RobotBase::RobotStateSaver saver(probot); // save the state of the robot since will be setting joint values
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);
106  }
107  pgraph = penv->drawlinestrip(&vpoints[0].x,vpoints.size(),sizeof(vpoints[0]),1.0f);
108  }
109 
110  // send the trajectory to the robot
111  probot->GetController()->SetPath(ptraj);
112  }
113 
114  // unlock the environment and wait for the robot to finish
115  while(!probot->GetController()->IsDone() && IsOk()) {
116  boost::this_thread::sleep(boost::posix_time::milliseconds(1));
117  }
118  }
119  }
120 };
121 
122 } // end namespace cppexamples
123 
124 int main(int argc, char ** argv)
125 {
127  return example.main(argc,argv);
128 }
129 
130