openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
orikfilter.cpp
Go to the documentation of this file.
1 
8 #include <openrave-core.h>
9 #include <openrave/utils.h>
10 #include <vector>
11 #include <sstream>
12 #include <boost/thread/thread.hpp>
13 #include <boost/bind.hpp>
14 
15 using namespace OpenRAVE;
16 using namespace std;
17 
18 // quit after 100 milliseconds
19 IkReturn MyTimeoutFilter(std::vector<dReal>&, RobotBase::ManipulatorConstPtr, const IkParameterization&, uint32_t starttime)
20 {
21  if( utils::GetMilliTime()-starttime > 100 ) {
22  RAVELOG_INFO("quitting\n");
23  return IKRA_Quit;
24  }
25  return IKRA_Success;
26 }
27 
28 int main(int argc, char ** argv)
29 {
30  string scenefilename = "data/pr2test1.env.xml";
31  RaveInitialize(true);
33  penv->Load(scenefilename);
34 
35  vector<RobotBasePtr> vrobots;
36  penv->GetRobots(vrobots);
37  RobotBasePtr probot = vrobots.at(0);
38  probot->SetActiveManipulator("leftarm_torso");
39  RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
40 
41  // load inverse kinematics using ikfast
42  ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
43  penv->Add(pikfast,true,"");
44  stringstream ssin,ssout;
45  vector<dReal> vsolution;
46  ssin << "LoadIKFastSolver " << probot->GetName() << " " << (int)IKP_Transform6D;
47  if( !pikfast->SendCommand(ssout,ssin) ) {
48  RAVELOG_ERROR("failed to load iksolver\n");
49  }
50  if( !pmanip->GetIkSolver()) {
51  penv->Destroy();
52  return 1;
53  }
54 
55  probot->SetActiveDOFs(pmanip->GetArmIndices());
56  vector<dReal> vlower,vupper;
57 
58  while(1) {
59  {
60  EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
61 
62  // move robot randomly
63  probot->GetActiveDOFLimits(vlower,vupper);
64  vector<dReal> v(pmanip->GetArmIndices().size());
65  for(size_t i = 0; i < vlower.size(); ++i) {
66  v[i] = vlower[i] + (vupper[i]-vlower[i])*RaveRandomFloat();
67  }
68  probot->SetActiveDOFValues(v);
69  bool bincollision = !penv->CheckCollision(probot) && !probot->CheckSelfCollision();
70 
71  uint32_t starttime = utils::GetMilliTime();
72  UserDataPtr filterhandle = pmanip->GetIkSolver()->RegisterCustomFilter(0,boost::bind(MyTimeoutFilter,_1,_2,_3,starttime));
73  bool bsuccess = pmanip->FindIKSolution(pmanip->GetIkParameterization(IKP_Transform6D),v,IKFO_CheckEnvCollisions);
74  RAVELOG_INFO("in collision: %d, real success %d, time passed: %d\n",bincollision,bsuccess,utils::GetMilliTime()-starttime);
75  }
76  }
77 
78  RaveDestroy();
79  return 0;
80 }