19 using namespace OpenRAVE;
22 namespace cppexamples {
28 string scenefilename =
"data/diffdrive_arm.env.xml";
29 penv->Load(scenefilename);
33 penv->GetPhysicsEngine()->SetGravity(
Vector(0,0,-9.8));
35 vector<RobotBasePtr> vrobots;
36 penv->GetRobots(vrobots);
40 vector<int> wheelindices, restindices;
44 EnvironmentMutex::scoped_lock lock(penv->GetMutex());
47 vector<int> dofindices(probot->GetDOF());
48 for(
int i = 0; i < probot->GetDOF(); ++i) {
51 probot->SetController(multi,dofindices,1);
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);
60 for(
int i = 0; i < (*itjoint)->GetDOF(); ++i) {
61 restindices.push_back((*itjoint)->GetDOFIndex()+i);
66 if(wheelindices.size() > 0 ) {
68 multi->AttachController(wheelcontroller,wheelindices,0);
71 if( restindices.size() > 0 ) {
73 multi->AttachController(armcontroller,restindices,0);
76 RAVELOG_WARN(
"robot needs to have wheels and arm for demo to work\n");
82 EnvironmentMutex::scoped_lock lock(penv->GetMutex());
84 if( !!armcontroller ) {
87 probot->SetActiveDOFs(restindices);
91 probot->GetActiveDOFValues(q);
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) {
102 probot->SetActiveDOFValues(q);
103 if( probot->CheckSelfCollision() ) {
108 std::copy(q.begin(),q.end(),vdata.begin());
109 vdata.at(timeoffset) = 2;
110 traj->Insert(1,vdata);
112 armcontroller->SetPath(traj);
115 if( !!wheelcontroller ) {
116 stringstream sout,ss; ss <<
"setvelocity ";
117 for(
size_t i = 0; i < wheelindices.size(); ++i) {
120 if( !wheelcontroller->SendCommand(sout,ss) ) {
127 if( !armcontroller ) {
128 boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
131 while(!armcontroller->IsDone() && IsOk()) {
132 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
141 int main(
int argc,
char ** argv)
144 return example.
main(argc,argv);