openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
ortrajectory.cpp
Go to the documentation of this file.
1 
38 #include <openrave-core.h>
39 #include <vector>
40 #include <cstring>
41 #include <sstream>
42 #include <openrave/planningutils.h>
43 
44 #include "orexample.h"
45 
46 using namespace OpenRAVE;
47 using namespace std;
48 
49 namespace cppexamples {
50 
52 {
53 public:
54  virtual void demothread(int argc, char ** argv) {
55  string scenefilename = "data/lab1.env.xml";
56  penv->Load(scenefilename);
57  vector<RobotBasePtr> vrobots;
58  penv->GetRobots(vrobots);
59  RobotBasePtr probot = vrobots.at(0);
60  std::vector<dReal> q;
61 
62  while(IsOk()) {
63  {
64  EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
65 
67  traj->Init(probot->GetActiveConfigurationSpecification());
68  probot->GetActiveDOFValues(q); // get current values
69  traj->Insert(0,q);
70  q[RaveRandomInt()%probot->GetDOF()] += RaveRandomFloat()-0.5; // move a random axis
71 
72  // check for collisions
73  {
74  RobotBase::RobotStateSaver saver(probot); // add a state saver so robot is not moved permenantly
75  probot->SetDOFValues(q);
76  if( penv->CheckCollision(RobotBaseConstPtr(probot)) ) {
77  continue; // robot in collision at final point, so reject
78  }
79  }
80 
81  traj->Insert(1,q);
83  probot->GetController()->SetPath(traj);
84  // setting through the robot is also possible: probot->SetMotion(traj);
85  }
86  // unlock the environment and wait for the robot to finish
87  while(!probot->GetController()->IsDone() && IsOk()) {
88  boost::this_thread::sleep(boost::posix_time::milliseconds(1));
89  }
90  }
91  }
92 };
93 
94 } // end namespace cppexamples
95 
96 int main(int argc, char ** argv)
97 {
99  return example.main(argc,argv);
100 }