void GetTaskTime(EnvironmentBasePtr _penv, std::vector<dReal> &vsolutionA, std::vector<dReal> &vsolutionB, TrajectoryBasePtr ptraj){ //thread::id get_id(); std::cout << "this thread ::" << boost::this_thread::get_id() << std::endl; PlannerBasePtr planner = RaveCreatePlanner(_penv,"birrt"); //std::vector<dReal> vinitialconfig,vgoalconfig; ptraj = RaveCreateTrajectory(_penv,""); PlannerBase::PlannerParametersPtr params(new PlannerBase::PlannerParameters()); RobotBasePtr probot = _penv->GetRobot(robotname); params->_nMaxIterations = 4000; // max iterations before failure GraphHandlePtr pgraph; { EnvironmentMutex::scoped_lock lock(_penv->GetMutex()); // lock environment params->SetRobotActiveJoints(probot); // set planning configuration space to current active dofs //initial config probot->SetActiveDOFValues(vsolutionA); params->vinitialconfig.resize(probot->GetActiveDOF()); probot->GetActiveDOFValues(params->vinitialconfig); //goal config probot->SetActiveDOFValues(vsolutionB); params->vgoalconfig.resize(probot->GetActiveDOF()); probot->GetActiveDOFValues(params->vgoalconfig); //setting limits vector<dReal> vlower,vupper; probot->GetActiveDOFLimits(vlower,vupper); //RAVELOG_INFO("starting to plan\n"); if( !planner->InitPlan(probot,params) ) { //return ptraj; } // create a new output trajectory if( !planner->PlanPath(ptraj) ) { RAVELOG_WARN("plan failed \n"); //return NULL; } } ////RAVELOG_INFO(ss.str()); if (!! ptraj){ //std::cout << ptraj->GetDuration() << std::endl; writeTrajectory(ptraj); } std::cout << "this thread ::" << boost::this_thread::get_id() << " has completed its work" << std::endl; mtx__.lock(); completed_thread = boost::this_thread::get_id(); mtx__.unlock(); //return ptraj; }
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; }