openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
orplanning_ik.cpp
Go to the documentation of this file.
1 
9 #include <openrave-core.h>
10 #include <vector>
11 #include <sstream>
12 #include <boost/thread/thread.hpp>
13 #include <boost/bind.hpp>
14 
15 #include "orexample.h"
16 
17 using namespace OpenRAVE;
18 using namespace std;
19 
20 namespace cppexamples {
21 
23 {
24 public:
25  virtual void demothread(int argc, char ** argv) {
26  string scenefilename = "data/pa10grasp2.env.xml";
27  penv->Load(scenefilename);
28 
29  vector<RobotBasePtr> vrobots;
30  penv->GetRobots(vrobots);
31  RobotBasePtr probot = vrobots.at(0);
32 
33  // find a manipulator chain to move
34  for(size_t i = 0; i < probot->GetManipulators().size(); ++i) {
35  if( probot->GetManipulators()[i]->GetName().find("arm") != string::npos ) {
36  probot->SetActiveManipulator(probot->GetManipulators()[i]);
37  break;
38  }
39  }
40  RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
41 
42  // load inverse kinematics using ikfast
43  ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
44  penv->Add(pikfast,true,"");
45  stringstream ssin,ssout;
46  vector<dReal> vsolution;
47  ssin << "LoadIKFastSolver " << probot->GetName() << " " << (int)IKP_Transform6D;
48  if( !pikfast->SendCommand(ssout,ssin) ) {
49  RAVELOG_ERROR("failed to load iksolver\n");
50  }
51  if( !pmanip->GetIkSolver()) {
52  throw OPENRAVE_EXCEPTION_FORMAT0("need ik solver",ORE_Assert);
53  }
54 
55  ModuleBasePtr pbasemanip = RaveCreateModule(penv,"basemanipulation"); // create the module
56  penv->Add(pbasemanip,true,probot->GetName()); // load the module
57 
58  while(IsOk()) {
59  {
60  EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
61 
62  // find a new manipulator position and feed that into the planner. If valid, robot will move to it safely.
63  Transform t = pmanip->GetEndEffectorTransform();
64  t.trans += Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f);
65  t.rot = quatMultiply(t.rot,quatFromAxisAngle(Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f)*0.2f));
66  ssin.str("");
67  ssin.clear();
68  ssin << "MoveToHandPosition pose " << t;
69  // start the planner and run the robot
70  RAVELOG_INFO("%s\n",ssin.str().c_str());
71  if( !pbasemanip->SendCommand(ssout,ssin) ) {
72  continue;
73  }
74  }
75 
76  // unlock the environment and wait for the robot to finish
77  while(!probot->GetController()->IsDone() && IsOk()) {
78  boost::this_thread::sleep(boost::posix_time::milliseconds(1));
79  }
80  }
81  }
82 };
83 
84 } // end namespace cppexamples
85 
86 int main(int argc, char ** argv)
87 {
89  return example.main(argc,argv);
90 }