openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
orplanning_multirobot.cpp
Go to the documentation of this file.
1 
8 #include <openrave-core.h>
9 #include <vector>
10 #include <sstream>
11 #include <boost/format.hpp>
12 
13 #include "orexample.h"
14 
15 using namespace OpenRAVE;
16 using namespace std;
17 
18 namespace cppexamples {
19 
22 {
23 public:
24  virtual void demothread(int argc, char ** argv) {
25  // create a scene with two robots
26  string robotfilename = "robots/barrettwam.robot.xml";
28  RobotBasePtr probot1 = penv->ReadRobotURI(RobotBasePtr(), robotfilename);
29  penv->Add(probot1,true);
30  RobotBasePtr probot2 = penv->ReadRobotURI(RobotBasePtr(), robotfilename);
31  penv->Add(probot2,true);
32  Transform trobot2 = probot2->GetTransform(); trobot2.trans.y += 0.5;
33  probot2->SetTransform(trobot2);
34 
35  std::vector<RobotBasePtr> vrobots(2);
36  vrobots[0] = probot1;
37  vrobots[1] = probot2;
38 
39  // create the configuration space using the manipulator indices
40  ConfigurationSpecification spec = probot1->GetActiveManipulator()->GetArmConfigurationSpecification() + probot2->GetActiveManipulator()->GetArmConfigurationSpecification();
42  params->SetConfigurationSpecification(penv,spec); // set the joint configuration
43  params->_nMaxIterations = 4000; // max iterations before failure
44  params->Validate();
45 
46  // create the planner parameters
47  PlannerBasePtr planner = RaveCreatePlanner(penv,"birrt");
48 
49  while(IsOk()) {
50  list<GraphHandlePtr> listgraphs;
51  {
52  EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
53 
54  params->_getstatefn(params->vinitialconfig);
55  params->vgoalconfig.resize(params->GetDOF());
56 
57  // find a set of free joint values for the robot
58  {
59  while(1) {
60  for(int i = 0; i < params->GetDOF(); ++i) {
61  params->vgoalconfig[i] = params->_vConfigLowerLimit[i] + (params->_vConfigUpperLimit[i]-params->_vConfigLowerLimit[i])*RaveRandomFloat();
62  }
63  params->_setstatefn(params->vgoalconfig);
64  if( params->_checkpathconstraintsfn(params->vgoalconfig,params->vgoalconfig,IT_OpenStart,PlannerBase::ConfigurationListPtr()) ) {
65  break;
66  }
67  }
68  // restore robot state
69  params->_setstatefn(params->vinitialconfig);
70  }
71 
72  RAVELOG_INFO("starting to plan\n");
73  if( !planner->InitPlan(RobotBasePtr(),params) ) {
74  continue;
75  }
76 
77  // create a new output trajectory
78  TrajectoryBasePtr ptraj = RaveCreateTrajectory(penv,"");
79  if( !planner->PlanPath(ptraj) ) {
80  RAVELOG_WARN("plan failed, trying again\n");
81  continue;
82  }
83 
84  // draw the end effector of the trajectory
85  listgraphs.clear();
86  for(std::vector<RobotBasePtr>::iterator itrobot = vrobots.begin(); itrobot != vrobots.end(); ++itrobot) {
87  RobotBase::RobotStateSaver saver(*itrobot); // save the state of the robot since will be setting joint values
88  RobotBase::ManipulatorPtr manip = (*itrobot)->GetActiveManipulator();
89  vector<RaveVector<float> > vpoints;
90  vector<dReal> vtrajdata;
91  for(dReal ftime = 0; ftime <= ptraj->GetDuration(); ftime += 0.01) {
92  ptraj->Sample(vtrajdata,ftime,manip->GetArmConfigurationSpecification());
93  (*itrobot)->SetDOFValues(vtrajdata,true,manip->GetArmIndices());
94  vpoints.push_back(manip->GetEndEffectorTransform().trans);
95  }
96  listgraphs.push_back(penv->drawlinestrip(&vpoints[0].x,vpoints.size(),sizeof(vpoints[0]),1.0f));
97  }
98 
99  // send the trajectory to each robot
100  probot1->GetController()->SetPath(ptraj);
101  probot2->GetController()->SetPath(ptraj);
102  }
103 
104  // unlock the environment and wait for the robot to finish
105  while(IsOk()) {
106  if( probot1->GetController()->IsDone() || probot2->GetController()->IsDone() ) {
107  break;
108  }
109  boost::this_thread::sleep(boost::posix_time::milliseconds(1));
110  }
111  }
112  }
113 };
114 
115 } // end namespace cppexamples
116 
117 int main(int argc, char ** argv)
118 {
120  return example.main(argc,argv);
121 }
122