virtual void demothread(int argc, char ** argv) { string scenefilename = "data/pa10grasp2.env.xml"; penv->Load(scenefilename); vector<RobotBasePtr> vrobots; penv->GetRobots(vrobots); RobotBasePtr probot = vrobots.at(0); // find a manipulator chain to move for(size_t i = 0; i < probot->GetManipulators().size(); ++i) { if( probot->GetManipulators()[i]->GetName().find("arm") != string::npos ) { probot->SetActiveManipulator(probot->GetManipulators()[i]); break; } } RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator(); // load inverse kinematics using ikfast ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast"); penv->Add(pikfast,true,""); stringstream ssin,ssout; vector<dReal> vsolution; ssin << "LoadIKFastSolver " << probot->GetName() << " " << (int)IKP_Transform6D; if( !pikfast->SendCommand(ssout,ssin) ) { RAVELOG_ERROR("failed to load iksolver\n"); } if( !pmanip->GetIkSolver()) { throw OPENRAVE_EXCEPTION_FORMAT0("need ik solver",ORE_Assert); } ModuleBasePtr pbasemanip = RaveCreateModule(penv,"basemanipulation"); // create the module penv->Add(pbasemanip,true,probot->GetName()); // load the module while(IsOk()) { { EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment // find a new manipulator position and feed that into the planner. If valid, robot will move to it safely. Transform t = pmanip->GetEndEffectorTransform(); t.trans += Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f); t.rot = quatMultiply(t.rot,quatFromAxisAngle(Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f)*0.2f)); ssin.str(""); ssin.clear(); ssin << "MoveToHandPosition pose " << t; // start the planner and run the robot RAVELOG_INFO("%s\n",ssin.str().c_str()); if( !pbasemanip->SendCommand(ssout,ssin) ) { 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)); } } }
virtual void demothread(int argc, char ** argv) { string scenefilename = "data/lab1.env.xml"; penv->Load(scenefilename); vector<RobotBasePtr> vrobots; penv->GetRobots(vrobots); RobotBasePtr probot = vrobots.at(0); std::vector<dReal> q; while(IsOk()) { { EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment TrajectoryBasePtr traj = RaveCreateTrajectory(penv,""); traj->Init(probot->GetActiveConfigurationSpecification()); probot->GetActiveDOFValues(q); // get current values traj->Insert(0,q); q[RaveRandomInt()%probot->GetDOF()] += 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->SetDOFValues(q); if( penv->CheckCollision(RobotBaseConstPtr(probot)) ) { continue; // robot in collision at final point, so reject } } traj->Insert(1,q); planningutils::SmoothActiveDOFTrajectory(traj,probot); probot->GetController()->SetPath(traj); // setting through the robot is also possible: probot->SetMotion(traj); } // unlock the environment and wait for the robot to finish while(!probot->GetController()->IsDone() && IsOk()) { boost::this_thread::sleep(boost::posix_time::milliseconds(1)); } } }
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)); } } }