bool GetIKSolutions(EnvironmentBasePtr _penv, Transform Pose, std::vector< std::vector< dReal > > &vsolution){ ModuleBasePtr _pikfast = RaveCreateModule(_penv,"ikfast"); RobotBasePtr _probot = _penv->GetRobot(robotname); _probot->SetActiveManipulator("1"); RobotBase::ManipulatorPtr _pmanip = _probot->GetActiveManipulator(); _penv->Add(_pikfast,true,""); std::vector< std::vector< dReal > > solns; solns.resize(6); stringstream ssin,ssout; string iktype = "Transform6D"; ssin << "LoadIKFastSolver " << _probot->GetName() << " " << iktype; if( !_pikfast->SendCommand(ssout,ssin) ) { RAVELOG_ERROR("failed to load iksolver\n"); _penv->Destroy(); return false; } if(_pmanip->FindIKSolutions(IkParameterization(Pose),vsolution,IKFO_CheckEnvCollisions) ) { stringstream ss; ss << "solution is: "; for(size_t i = 0; i < vsolution.size(); ++i) { for(size_t j = 0; j < vsolution[i].size(); ++j){ ss << vsolution[i][j] << " " ; } ss << endl; } ss << endl; ////RAVELOG_INFO(ss.str()); } else { // could fail due to collisions, etc return false; } return true ; }
void SetViewer(EnvironmentBasePtr penv, const string& viewername) { ViewerBasePtr viewer = RaveCreateViewer(penv,viewername); BOOST_ASSERT(!!viewer); // attach it to the environment: penv->Add(viewer); // finally call the viewer's infinite loop (this is why a separate thread is needed) bool showgui = true; viewer->main(showgui); }
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; }