openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
orplanning_module.cpp
Go to the documentation of this file.
1 
9 #include <openrave-core.h>
10 #include <vector>
11 #include <sstream>
12 
13 #include "orexample.h"
14 
15 using namespace OpenRAVE;
16 using namespace std;
17 
18 namespace cppexamples {
19 
21 {
22 public:
23  virtual void demothread(int argc, char ** argv) {
24 
25  string scenefilename = "data/wamtest1.env.xml";
26  penv->Load(scenefilename);
27 
28  vector<RobotBasePtr> vrobots;
29  penv->GetRobots(vrobots);
30  RobotBasePtr probot = vrobots.at(0);
31  vector<dReal> vlower,vupper,v(probot->GetDOF());
32  probot->GetDOFLimits(vlower,vupper);
33 
34  // set all dofs as active
35  vector<int> vindices(probot->GetDOF());
36  for(size_t i = 0; i < vindices.size(); ++i) {
37  vindices[i] = i;
38  }
39  probot->SetActiveDOFs(vindices);
40 
41  ModuleBasePtr pbasemanip = RaveCreateModule(penv,"basemanipulation"); // create the module
42  penv->Add(pbasemanip,true,probot->GetName()); // load the module
43 
44  while(IsOk()) {
45  {
46  EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
47 
48  // find a set of free joint values for the robot
49  {
50  RobotBase::RobotStateSaver saver(probot); // save the state
51  while(1) {
52  for(size_t i = 0; i < vlower.size(); ++i) {
53  v[i] = vlower[i] + (vupper[i]-vlower[i])*RaveRandomFloat();
54  }
55  probot->SetActiveDOFValues(v);
56  if( !penv->CheckCollision(probot) && !probot->CheckSelfCollision() ) {
57  break;
58  }
59  }
60  // robot state is restored
61  }
62 
63  stringstream cmdin,cmdout;
64  cmdin << "MoveActiveJoints goal ";
65  for(size_t i = 0; i < v.size(); ++i) {
66  cmdin << v[i] << " ";
67  }
68 
69  // start the planner and run the robot
70  RAVELOG_INFO("%s\n",cmdin.str().c_str());
71  if( !pbasemanip->SendCommand(cmdout,cmdin) ) {
72  continue;
73  }
74  }
75 
76  // unlock the environment and wait for the robot to finish
77  while(!probot->GetController()->IsDone() && IsOk()) {
78  boost::this_thread::sleep(boost::posix_time::milliseconds(1));
79  }
80  }
81  }
82 };
83 
84 } // end namespace cppexamples
85 
86 int main(int argc, char ** argv)
87 {
89  return example.main(argc,argv);
90 }