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 ; }
int main(int argc, char ** argv) { //int num = 1; string scenefilename = "data/lab1.env.xml"; string viewername = RaveGetDefaultViewerType(); //qtcoin // parse the command line options int i = 1; while(i < argc) { if((strcmp(argv[i], "-h") == 0)||(strcmp(argv[i], "-?") == 0)||(strcmp(argv[i], "/?") == 0)||(strcmp(argv[i], "--help") == 0)||(strcmp(argv[i], "-help") == 0)) { RAVELOG_INFO("orloadviewer [--num n] [--scene filename] viewername\n"); return 0; } else if( strcmp(argv[i], "--scene") == 0 ) { scenefilename = argv[i+1]; i += 2; } else break; } if( i < argc ) { viewername = argv[i++]; } RaveInitialize(true); // start openrave core EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment RaveSetDebugLevel(Level_Debug); boost::thread thviewer(boost::bind(SetViewer,penv,viewername)); penv->Load(scenefilename); // load the scene UserDataPtr pregistration; while(!pregistration) { if( !pregistration && !!penv->GetViewer() ) { pregistration = penv->GetViewer()->RegisterViewerThreadCallback(boost::bind(ViewerCallback,penv->GetViewer())); } boost::this_thread::sleep(boost::posix_time::milliseconds(1)); } thviewer.join(); // wait for the viewer thread to exit pregistration.reset(); penv->Destroy(); // destroy return 0; }
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; }
int main() { traj.clear(); unsigned int mainthreadsleft = numThreads; // boost::shared_ptr<boost::thread> ( new boost::thread(boost::bind( &track_threads ))); Ta.trans = transA; Ta.rot = quatA; Tb.trans = transB; Tb.rot = quatB; std::string scenefilename = "scenes/test6dof.mujin.zae"; std::string viewername = "qtcoin"; RaveInitialize(true); // start openrave core EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment Transform robot_t; RaveVector< dReal > transR(c, d, 0); robot_t.trans = transR; //boost::thread thviewer(boost::bind(SetViewer,penv,viewername)); penv->Load(scenefilename); RobotBasePtr probot = penv->GetRobot("RV-4F"); //removing floor for collision checking EnvironmentBasePtr pclondedenv = penv->CloneSelf(Clone_Bodies); pclondedenv->Remove( pclondedenv->GetKinBody("floor")); RobotBasePtr probot_clone = pclondedenv->GetRobot("RV-4F"); unsigned int tot = ((( abs(a) + abs(c) )/discretization_x )+1) * (((( abs(b) + abs(d) )/discretization_y )+1) * (( abs( z )/discretization_z )+1)); unsigned int tot_o = tot; for (unsigned int i = 0 ; i <= (( abs(a) + abs(c) )/discretization_x );i++) { for (unsigned int j = 0 ; j <= (( abs(b) + abs(d) )/discretization_y ); j++) { for (unsigned int k = 0 ; k <= ( abs( z )/discretization_z ) ; k++) { ////std::cout << transR[0] << ";" << transR[1] << ";" << transR[2] << std::endl; //boost::this_thread::sleep(boost::posix_time::milliseconds(1000)); robot_t.trans = transR; probot->SetTransform(robot_t); probot_clone->SetTransform(robot_t); if( pclondedenv->CheckCollision(RobotBaseConstPtr(probot_clone)) ){ //std::cout << "Robot in collision with the environment" << std::endl; } else { do_task(Ta, Tb, penv,3); } tot -= 1; std::cout << tot << "/" << tot_o << std::endl; transR[2] = transR[2]+ discretization_z; } transR[2] = 0; transR[1] = transR[1] + discretization_y; robot_t.trans = transR; } transR[2] = 0; transR[1] = c; transR[0] = transR[0] + discretization_x; robot_t.trans = transR; } //thviewer.join(); // wait for the viewer thread to exit RaveDestroy(); // make sure to destroy the OpenRAVE runtime penv->Destroy(); // destroy return 0; }
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; }