openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
ormulticontrol.cpp
Go to the documentation of this file.
1 
11 #include <openrave-core.h>
12 #include <openrave/planningutils.h>
13 #include <vector>
14 #include <cstring>
15 #include <sstream>
16 
17 #include "orexample.h"
18 
19 using namespace OpenRAVE;
20 using namespace std;
21 
22 namespace cppexamples {
23 
25 {
26 public:
27  virtual void demothread(int argc, char ** argv) {
28  string scenefilename = "data/diffdrive_arm.env.xml";
29  penv->Load(scenefilename);
30 
31  // attach a physics engine
32  penv->SetPhysicsEngine(RaveCreatePhysicsEngine(penv,"ode"));
33  penv->GetPhysicsEngine()->SetGravity(Vector(0,0,-9.8));
34 
35  vector<RobotBasePtr> vrobots;
36  penv->GetRobots(vrobots);
37  RobotBasePtr probot = vrobots.at(0);
38  std::vector<dReal> q;
39 
40  vector<int> wheelindices, restindices;
41  ControllerBasePtr wheelcontroller, armcontroller;
42  // create the controllers, make sure to lock environment!
43  {
44  EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
45 
46  MultiControllerPtr multi(new MultiController(penv));
47  vector<int> dofindices(probot->GetDOF());
48  for(int i = 0; i < probot->GetDOF(); ++i) {
49  dofindices[i] = i;
50  }
51  probot->SetController(multi,dofindices,1); // control everything
52  // set the velocity controller on all joints that have 'wheel' in their description
53  for(std::vector<KinBody::JointPtr>::const_iterator itjoint = probot->GetJoints().begin(); itjoint != probot->GetJoints().end(); ++itjoint) {
54  if( (*itjoint)->GetName().find("wheel") != string::npos ) {
55  for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
56  wheelindices.push_back((*itjoint)->GetDOFIndex()+i);
57  }
58  }
59  else {
60  for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
61  restindices.push_back((*itjoint)->GetDOFIndex()+i);
62  }
63  }
64  }
65 
66  if(wheelindices.size() > 0 ) {
67  wheelcontroller = RaveCreateController(penv,"odevelocity");
68  multi->AttachController(wheelcontroller,wheelindices,0);
69  }
70 
71  if( restindices.size() > 0 ) {
72  armcontroller = RaveCreateController(penv,"idealcontroller");
73  multi->AttachController(armcontroller,restindices,0);
74  }
75  else {
76  RAVELOG_WARN("robot needs to have wheels and arm for demo to work\n");
77  }
78  }
79 
80  while(IsOk()) {
81  {
82  EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
83 
84  if( !!armcontroller ) {
85  // set a trajectory on the arm and velocity on the wheels
87  probot->SetActiveDOFs(restindices);
88  ConfigurationSpecification spec = probot->GetActiveConfigurationSpecification();
89  int timeoffset = spec.AddDeltaTimeGroup();
90  traj->Init(spec);
91  probot->GetActiveDOFValues(q); // get current values
92  vector<dReal> vdata(spec.GetDOF(),0);
93  std::copy(q.begin(),q.end(),vdata.begin());
94  traj->Insert(0,vdata);
95  for(int i = 0; i < 4; ++i) {
96  q.at(RaveRandomInt()%restindices.size()) += RaveRandomFloat()-0.5; // move a random axis
97  }
98 
99  // check for collisions
100  {
101  RobotBase::RobotStateSaver saver(probot); // add a state saver so robot is not moved permenantly
102  probot->SetActiveDOFValues(q);
103  if( probot->CheckSelfCollision() ) { // don't check env collisions since we have physics enabled
104  continue; // robot in collision at final point, so reject
105  }
106  }
107 
108  std::copy(q.begin(),q.end(),vdata.begin());
109  vdata.at(timeoffset) = 2; // trajectory takes 2s
110  traj->Insert(1,vdata);
112  armcontroller->SetPath(traj);
113  }
114 
115  if( !!wheelcontroller ) {
116  stringstream sout,ss; ss << "setvelocity ";
117  for(size_t i = 0; i < wheelindices.size(); ++i) {
118  ss << 2*(RaveRandomFloat()-0.5) << " ";
119  }
120  if( !wheelcontroller->SendCommand(sout,ss) ) {
121  RAVELOG_WARN("failed to send velocity command\n");
122  }
123  }
124  }
125 
126  // unlock the environment and wait for the arm controller to finish (wheel controller will never finish)
127  if( !armcontroller ) {
128  boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
129  }
130  else {
131  while(!armcontroller->IsDone() && IsOk()) {
132  boost::this_thread::sleep(boost::posix_time::milliseconds(1));
133  }
134  }
135  }
136  }
137 };
138 
139 } // end namespace cppexamples
140 
141 int main(int argc, char ** argv)
142 {
144  return example.main(argc,argv);
145 }