openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
ikfastloader.cpp
Go to the documentation of this file.
1 
18 #include <openrave-core.h>
19 #include <vector>
20 #include <cstring>
21 #include <sstream>
22 #include <stdio.h>
23 
24 #include <boost/iostreams/device/file_descriptor.hpp>
25 #include <boost/iostreams/stream.hpp>
26 #include <boost/format.hpp>
27 
28 using namespace OpenRAVE;
29 using namespace std;
30 
31 int main(int argc, char ** argv)
32 {
33  if( argc < 3 ) {
34  RAVELOG_INFO("ikloader robot iktype\n");
35  return 1;
36  }
37 
38  string robotname = argv[1];
39  string iktype = argv[2];
40  RaveInitialize(true); // start openrave core
41 
42  EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
43 
44  {
45  // lock the environment to prevent changes
46  EnvironmentMutex::scoped_lock lock(penv->GetMutex());
47  // load the scene
48  RobotBasePtr probot = penv->ReadRobotXMLFile(robotname);
49  if( !probot ) {
50  penv->Destroy();
51  return 2;
52  }
53  penv->Add(probot);
54 
55  ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
56  penv->Add(pikfast,true,"");
57  stringstream ssin,ssout;
58  ssin << "LoadIKFastSolver " << probot->GetName() << " " << iktype;
59  // if necessary, add free inc for degrees of freedom
60  //ssin << " " << 0.04f;
61  // get the active manipulator
62  RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
63  if( !pikfast->SendCommand(ssout,ssin) ) {
64  RAVELOG_ERROR("failed to load iksolver\n");
65  penv->Destroy();
66  return 1;
67  }
68 
69  RAVELOG_INFO("testing random ik\n");
70  while(1) {
71  Transform trans;
73  trans.trans = Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5)*2;
74 
75  vector<dReal> vsolution;
76  if( pmanip->FindIKSolution(IkParameterization(trans),vsolution,IKFO_CheckEnvCollisions) ) {
77  stringstream ss; ss << "solution is: ";
78  for(size_t i = 0; i < vsolution.size(); ++i) {
79  ss << vsolution[i] << " ";
80  }
81  ss << endl;
82  RAVELOG_INFO(ss.str());
83  }
84  else {
85  // could fail due to collisions, etc
86  }
87  }
88  }
89 
90  RaveDestroy(); // destroy
91  return 0;
92 }