11 #include <boost/format.hpp>
15 using namespace OpenRAVE;
18 namespace cppexamples {
26 string robotfilename =
"robots/barrettwam.robot.xml";
29 penv->Add(probot1,
true);
31 penv->Add(probot2,
true);
32 Transform trobot2 = probot2->GetTransform(); trobot2.trans.y += 0.5;
33 probot2->SetTransform(trobot2);
35 std::vector<RobotBasePtr> vrobots(2);
40 ConfigurationSpecification spec = probot1->GetActiveManipulator()->GetArmConfigurationSpecification() + probot2->GetActiveManipulator()->GetArmConfigurationSpecification();
42 params->SetConfigurationSpecification(penv,spec);
43 params->_nMaxIterations = 4000;
50 list<GraphHandlePtr> listgraphs;
52 EnvironmentMutex::scoped_lock lock(penv->GetMutex());
54 params->_getstatefn(params->vinitialconfig);
55 params->vgoalconfig.resize(params->GetDOF());
60 for(
int i = 0; i < params->GetDOF(); ++i) {
61 params->vgoalconfig[i] = params->_vConfigLowerLimit[i] + (params->_vConfigUpperLimit[i]-params->_vConfigLowerLimit[i])*
RaveRandomFloat();
63 params->_setstatefn(params->vgoalconfig);
69 params->_setstatefn(params->vinitialconfig);
79 if( !planner->PlanPath(ptraj) ) {
86 for(std::vector<RobotBasePtr>::iterator itrobot = vrobots.begin(); itrobot != vrobots.end(); ++itrobot) {
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);
96 listgraphs.push_back(penv->drawlinestrip(&vpoints[0].x,vpoints.size(),
sizeof(vpoints[0]),1.0f));
100 probot1->GetController()->SetPath(ptraj);
101 probot2->GetController()->SetPath(ptraj);
106 if( probot1->GetController()->IsDone() || probot2->GetController()->IsDone() ) {
109 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
117 int main(
int argc,
char ** argv)
120 return example.
main(argc,argv);