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)); } } }
int main(int argc, char ** argv) { if( argc < 3 ) { RAVELOG_INFO("ikloader robot iktype\n"); return 1; } string robotname = argv[1]; string iktype = argv[2]; RaveInitialize(true); // start openrave core EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment { // lock the environment to prevent changes EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // load the scene RobotBasePtr probot = penv->ReadRobotXMLFile(robotname); if( !probot ) { penv->Destroy(); return 2; } penv->Add(probot); ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast"); penv->Add(pikfast,true,""); stringstream ssin,ssout; ssin << "LoadIKFastSolver " << probot->GetName() << " " << iktype; // if necessary, add free inc for degrees of freedom //ssin << " " << 0.04f; // get the active manipulator RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator(); if( !pikfast->SendCommand(ssout,ssin) ) { RAVELOG_ERROR("failed to load iksolver\n"); penv->Destroy(); return 1; } RAVELOG_INFO("testing random ik\n"); while(1) { Transform trans; trans.rot = quatFromAxisAngle(Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5)); trans.trans = Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5)*2; vector<dReal> vsolution; if( pmanip->FindIKSolution(IkParameterization(trans),vsolution,IKFO_CheckEnvCollisions) ) { stringstream ss; ss << "solution is: "; for(size_t i = 0; i < vsolution.size(); ++i) { ss << vsolution[i] << " "; } ss << endl; RAVELOG_INFO(ss.str()); } else { // could fail due to collisions, etc } } } RaveDestroy(); // destroy return 0; }