Shows how to plan for two different robots simultaneously and control them simultaneously.
#include <vector>
#include <sstream>
#include <boost/format.hpp>
using namespace OpenRAVE;
using namespace std;
namespace cppexamples {
class PlanningPlannerExample : public OpenRAVEExample
{
public:
virtual void demothread(int argc, char ** argv) {
string robotfilename = "robots/barrettwam.robot.xml";
penv->Add(probot1,true);
penv->Add(probot2,true);
Transform trobot2 = probot2->GetTransform(); trobot2.trans.y += 0.5;
probot2->SetTransform(trobot2);
std::vector<RobotBasePtr> vrobots(2);
vrobots[0] = probot1;
vrobots[1] = probot2;
ConfigurationSpecification spec = probot1->GetActiveManipulator()->GetArmConfigurationSpecification() + probot2->GetActiveManipulator()->GetArmConfigurationSpecification();
params->SetConfigurationSpecification(penv,spec);
params->_nMaxIterations = 4000;
params->Validate();
while(IsOk()) {
list<GraphHandlePtr> listgraphs;
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
params->_getstatefn(params->vinitialconfig);
params->vgoalconfig.resize(params->GetDOF());
{
while(1) {
for(int i = 0; i < params->GetDOF(); ++i) {
params->vgoalconfig[i] = params->_vConfigLowerLimit[i] + (params->_vConfigUpperLimit[i]-params->_vConfigLowerLimit[i])*
RaveRandomFloat();
}
params->_setstatefn(params->vgoalconfig);
break;
}
}
params->_setstatefn(params->vinitialconfig);
}
continue;
}
if( !planner->PlanPath(ptraj) ) {
continue;
}
listgraphs.clear();
for(std::vector<RobotBasePtr>::iterator itrobot = vrobots.begin(); itrobot != vrobots.end(); ++itrobot) {
vector<RaveVector<float> > vpoints;
vector<dReal> vtrajdata;
for(
dReal ftime = 0; ftime <= ptraj->GetDuration(); ftime += 0.01) {
ptraj->Sample(vtrajdata,ftime,manip->GetArmConfigurationSpecification());
(*itrobot)->SetDOFValues(vtrajdata,true,manip->GetArmIndices());
vpoints.push_back(manip->GetEndEffectorTransform().trans);
}
listgraphs.push_back(penv->drawlinestrip(&vpoints[0].x,vpoints.size(),sizeof(vpoints[0]),1.0f));
}
probot1->GetController()->SetPath(ptraj);
probot2->GetController()->SetPath(ptraj);
}
while(IsOk()) {
if( probot1->GetController()->IsDone() || probot2->GetController()->IsDone() ) {
break;
}
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
}
}
}
};
}
int main(
int argc,
char ** argv)
{
return example.
main(argc,argv);
}