virtual void demothread(int argc, char ** argv) {
string scenefilename = "/home/yo/asibot/main/app/ravebot/models/asibot_kitchen_wheelchair.env.xml";
penv->Load(scenefilename);
vector<RobotBasePtr> vrobots;
penv->GetRobots(vrobots);
KinBodyPtr objPtr = penv->GetKinBody("asibot");
if(objPtr) {
if(vrobots.at(1)->Grab(objPtr)) printf("[success] object asibot exists and grabbed.\n");
else printf("[warning] object asibot exists but not grabbed.\n");
} else printf("[fail] object asibot does not exist.\n");
printf("begin wait\n");
sleep(5);
printf("end wait\n");
RobotBasePtr probot = vrobots.at(1);
vector<dReal> v(2);
/* vector<int> vindices(probot->GetDOF());
for(size_t i = 0; i < vindices.size(); ++i) {
vindices[i] = i;
}
probot->SetActiveDOFs(vindices);*/
vector<int> vindices; // send it empty instead
//probot->SetActiveDOFs(vindices,DOF_X|DOF_Y|DOF_RotationAxis,Vector(0,0,1));
probot->SetActiveDOFs(vindices,DOF_X|DOF_Y,Vector(0,0,1));
ModuleBasePtr pbasemanip = RaveCreateModule(penv,"basemanipulation"); // create the module
penv->Add(pbasemanip,true,probot->GetName()); // load the module
int iteracion = 0;
while(IsOk()) {
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
// find a set of free joint values for the robot
{
RobotBase::RobotStateSaver saver(probot); // save the state
while(1) {
Transform T = probot->GetTransform();
if(iteracion%2) {
v[0] = T.trans.x;
v[1] = T.trans.y+1;
//v[2] = T.trans.z;
iteracion++;
} else {
v[0] = T.trans.x;
v[1] = T.trans.y-0.5;
//v[2] = T.trans.z;
iteracion++;
}
if(iteracion==5) vrobots.at(1)->Release(objPtr);
probot->SetActiveDOFValues(v);
if( !penv->CheckCollision(probot) && !probot->CheckSelfCollision() ) {
break;
}
}
// robot state is restored
}
stringstream cmdin,cmdout;
cmdin << "MoveActiveJoints maxiter 2000 maxtries 1 goal ";
for(size_t i = 0; i < v.size(); ++i) {
cmdin << v[i] << " ";
}
// start the planner and run the robot
RAVELOG_INFO("%s\n",cmdin.str().c_str());
if( !pbasemanip->SendCommand(cmdout,cmdin) ) {
continue;
}
}
// unlock the environment and wait for the robot to finish
while(!probot->GetController()->IsDone() && IsOk()) {
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
}
}
}
int main(int argc, char ** argv)
{
string scenefilename = "data/diffdrive_arm.env.xml";
string viewername = "qtcoin";
RaveInitialize(true);
EnvironmentBasePtr penv = RaveCreateEnvironment();
penv->SetDebugLevel(Level_Debug);
boost::thread thviewer(boost::bind(SetViewer,penv,viewername)); // create the viewer
usleep(400000); // wait for the viewer to init
penv->Load(scenefilename);
// attach a physics engine
penv->SetPhysicsEngine(RaveCreatePhysicsEngine(penv,"ode"));
penv->GetPhysicsEngine()->SetGravity(Vector(0,0,-9.8));
vector<RobotBasePtr> vrobots;
penv->GetRobots(vrobots);
RobotBasePtr probot = vrobots.at(0);
std::vector<dReal> q;
vector<int> wheelindices, restindices;
ControllerBasePtr wheelcontroller, armcontroller;
// create the controllers, make sure to lock environment!
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
MultiControllerPtr multi(new MultiController(penv));
vector<int> dofindices(probot->GetDOF());
for(int i = 0; i < probot->GetDOF(); ++i) {
dofindices[i] = i;
}
probot->SetController(multi,dofindices,1); // control everything
// set the velocity controller on all joints that have 'wheel' in their description
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 ) {
wheelcontroller = RaveCreateController(penv,"odevelocity");
multi->AttachController(wheelcontroller,wheelindices,0);
}
if( restindices.size() > 0 ) {
armcontroller = RaveCreateController(penv,"idealcontroller");
multi->AttachController(armcontroller,restindices,0);
}
else {
RAVELOG_WARN("robot needs to have wheels and arm for demo to work\n");
}
}
while(1) {
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
if( !!armcontroller ) {
// set a trajectory on the arm and velocity on the wheels
TrajectoryBasePtr traj = RaveCreateTrajectory(penv,"");
probot->SetActiveDOFs(restindices);
ConfigurationSpecification spec = probot->GetActiveConfigurationSpecification();
int timeoffset = spec.AddDeltaTime();
traj->Init(spec);
probot->GetActiveDOFValues(q); // get current values
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) {
q.at(RaveRandomInt()%restindices.size()) += RaveRandomFloat()-0.5; // move a random axis
}
// check for collisions
{
RobotBase::RobotStateSaver saver(probot); // add a state saver so robot is not moved permenantly
probot->SetActiveDOFValues(q);
if( probot->CheckSelfCollision() ) { // don't check env collisions since we have physics enabled
continue; // robot in collision at final point, so reject
}
}
std::copy(q.begin(),q.end(),vdata.begin());
vdata.at(timeoffset) = 2; // trajectory takes 2s
traj->Insert(1,vdata);
planningutils::RetimeActiveDOFTrajectory(traj,probot,true);
armcontroller->SetPath(traj);
}
if( !!wheelcontroller ) {
stringstream sout,ss; ss << "setvelocity ";
for(size_t i = 0; i < wheelindices.size(); ++i) {
ss << 2*(RaveRandomFloat()-0.5) << " ";
}
if( !wheelcontroller->SendCommand(sout,ss) ) {
RAVELOG_WARN("failed to send velocity command\n");
}
}
}
// unlock the environment and wait for the arm controller to finish (wheel controller will never finish)
if( !armcontroller ) {
usleep(2000000);
}
else {
while(!armcontroller->IsDone()) {
usleep(1000);
}
}
}
thviewer.join(); // wait for the viewer thread to exit
penv->Destroy(); // destroy
return 0;
}
