Shows how to set two controllers for a robot using the MultiController class. The differential base moves with velocity control while the arm moves with position control.
#include <vector>
#include <cstring>
#include <sstream>
using namespace OpenRAVE;
using namespace std;
namespace cppexamples {
class MultiControlExample : public OpenRAVEExample
{
public:
virtual void demothread(int argc, char ** argv) {
string scenefilename = "data/diffdrive_arm.env.xml";
penv->Load(scenefilename);
penv->GetPhysicsEngine()->SetGravity(
Vector(0,0,-9.8));
vector<RobotBasePtr> vrobots;
penv->GetRobots(vrobots);
std::vector<dReal> q;
vector<int> wheelindices, restindices;
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
vector<int> dofindices(probot->GetDOF());
for(int i = 0; i < probot->GetDOF(); ++i) {
dofindices[i] = i;
}
probot->SetController(multi,dofindices,1);
for(std::vector<KinBody::JointPtr>::const_iterator itjoint = probot->GetJoints().begin(); itjoint != probot->GetJoints().end(); ++itjoint) {
if( (*itjoint)->GetName().find("wheel") != string::npos ) {
for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
wheelindices.push_back((*itjoint)->GetDOFIndex()+i);
}
}
else {
for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
restindices.push_back((*itjoint)->GetDOFIndex()+i);
}
}
}
if(wheelindices.size() > 0 ) {
multi->AttachController(wheelcontroller,wheelindices,0);
}
if( restindices.size() > 0 ) {
multi->AttachController(armcontroller,restindices,0);
}
else {
RAVELOG_WARN(
"robot needs to have wheels and arm for demo to work\n");
}
}
while(IsOk()) {
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
if( !!armcontroller ) {
probot->SetActiveDOFs(restindices);
traj->Init(spec);
probot->GetActiveDOFValues(q);
vector<dReal> vdata(spec.
GetDOF(),0);
std::copy(q.begin(),q.end(),vdata.begin());
traj->Insert(0,vdata);
for(int i = 0; i < 4; ++i) {
}
{
probot->SetActiveDOFValues(q);
if( probot->CheckSelfCollision() ) {
continue;
}
}
std::copy(q.begin(),q.end(),vdata.begin());
vdata.at(timeoffset) = 2;
traj->Insert(1,vdata);
armcontroller->SetPath(traj);
}
if( !!wheelcontroller ) {
stringstream sout,ss; ss << "setvelocity ";
for(size_t i = 0; i < wheelindices.size(); ++i) {
}
if( !wheelcontroller->SendCommand(sout,ss) ) {
}
}
}
if( !armcontroller ) {
boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
}
else {
while(!armcontroller->IsDone() && IsOk()) {
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
}
}
}
}
};
}
int main(
int argc,
char ** argv)
{
return example.
main(argc,argv);
}