openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
orpr2turnlever.cpp
Go to the documentation of this file.
1 
8 #include <openrave-core.h>
9 #include <vector>
10 #include <sstream>
11 #include <boost/format.hpp>
12 
13 #include "orexample.h"
15 #include <openrave/planningutils.h>
16 
17 using namespace OpenRAVE;
18 using namespace std;
19 
20 namespace cppexamples {
21 
23 {
24 public:
25 
26  PlannerAction PlanCallback(const PlannerBase::PlannerProgress& progress)
27  {
28  // plan callback
29  return PA_None;
30  }
31 
32  virtual void WaitRobot(RobotBasePtr probot) {
33  // unlock the environment and wait for the robot to finish
34  while(!probot->GetController()->IsDone() && IsOk()) {
35  boost::this_thread::sleep(boost::posix_time::milliseconds(1));
36  }
37  }
38 
39  virtual void demothread(int argc, char ** argv) {
40  string scenefilename = "data/pr2test1.env.xml";
42  penv->Load(scenefilename);
43  vector<RobotBasePtr> vrobots;
44  penv->GetRobots(vrobots);
45  RobotBasePtr probot = vrobots.at(0);
46 
47  KinBodyPtr target = penv->GetKinBody("lever");
48  if( !target ) {
49  target = RaveCreateKinBody(penv,"");
50  std::vector<AABB> boxes(1);
51  boxes[0].pos = Vector(0,0.1,0);
52  boxes[0].extents = Vector(0.01,0.1,0.01);
53  target->InitFromBoxes(boxes,true);
54  target->SetName("lever");
55  penv->Add(target);
56  Transform t;
57  t.trans = Vector(-0.2,-0.2,1);
58  target->SetTransform(t);
59  }
60 
61  RobotBase::ManipulatorPtr pmanip = probot->SetActiveManipulator("rightarm");
62 
63  // load inverse kinematics using ikfast
64  ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
65  penv->Add(pikfast,true,"");
66  stringstream ssin,ssout;
67  vector<dReal> vsolution;
68  ssin << "LoadIKFastSolver " << probot->GetName() << " " << (int)IKP_Transform6D;
69  if( !pikfast->SendCommand(ssout,ssin) ) {
70  throw OPENRAVE_EXCEPTION_FORMAT0("failed to load iksolver", ORE_Assert);
71  }
72 
73  // create the planner parameters
74  PlannerBasePtr planner = RaveCreatePlanner(penv,"birrt");
75 
76  ModuleBasePtr basemodule = RaveCreateModule(penv,"BaseManipulation");
77  penv->Add(basemodule,true,probot->GetName());
78  ModuleBasePtr taskmodule = RaveCreateModule(penv,"TaskManipulation");
79  penv->Add(taskmodule,true,probot->GetName());
80 
81  {
82  EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
83  stringstream ssin, ssout; ssin << "ReleaseFingers";
84  taskmodule->SendCommand(ssout,ssin);
85  }
86  WaitRobot(probot);
87 
88  TrajectoryBasePtr workspacetraj;
89  Transform Tgrasp0;
90  {
91  EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
92  Transform Toffset;
93  Toffset.trans = Vector(0,0.2,0);
94  Transform Ttarget0 = target->GetTransform();
95  Transform Ttarget1 = Ttarget0 * matrixFromAxisAngle(Vector(PI/2,0,0));
96 
97  Tgrasp0 = matrixFromAxisAngle(Vector(PI/2,0,0))*matrixFromAxisAngle(Vector(0,PI/2,0));
98  Tgrasp0.trans = Ttarget0*Toffset.trans;
99  Transform Tgraspoffset = Ttarget0.inverse() * Tgrasp0;
100  Transform Tgrasp1 = Ttarget1 * Tgraspoffset;
101 
103  workspacetraj = RaveCreateTrajectory(penv,"");
104  vector<dReal> values;
105  workspacetraj->Init(spec);
106  for(size_t i = 0; i < 32; ++i) {
107  dReal angle = i*0.05;
108  Transform Ttarget = Ttarget0 * matrixFromAxisAngle(Vector(angle,0,0));
109  Transform Tgrasp = Ttarget*Tgraspoffset;
110  IkParameterization ikparam(Tgrasp,IKP_Transform6D);
111  values.resize(ikparam.GetNumberOfValues());
112  ikparam.GetValues(values.begin());
113  workspacetraj->Insert(workspacetraj->GetNumWaypoints(),values);
114  }
115 
116  std::vector<dReal> maxvelocities(7,1.0);
117  std::vector<dReal> maxaccelerations(7,5.0);
118  planningutils::RetimeAffineTrajectory(workspacetraj,maxvelocities,maxaccelerations);
119  RAVELOG_INFO(str(boost::format("duration=%f, points=%d")%workspacetraj->GetDuration()%workspacetraj->GetNumWaypoints()));
120  }
121 
122  {
123  stringstream ssout, ssin; ssin << "MoveToHandPosition poses 1 " << Tgrasp0;
124  basemodule->SendCommand(ssout,ssin);
125  }
126  WaitRobot(probot);
127  {
128  stringstream ssin, ssout; ssin << "CloseFingers";
129  taskmodule->SendCommand(ssout,ssin);
130  }
131  WaitRobot(probot);
132 
133  list<TrajectoryBasePtr> listtrajectories;
134  {
135  EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
136  probot->SetActiveDOFs(pmanip->GetArmIndices());
137  probot->Grab(target);
138  PlannerBasePtr planner = RaveCreatePlanner(penv,"workspacetrajectorytracker");
140  params->SetRobotActiveJoints(probot); // set planning configuration space to current active dofs
141  params->workspacetraj = workspacetraj;
142 
143  RAVELOG_INFO("starting to plan\n");
144  if( !planner->InitPlan(probot,params) ) {
145  throw OPENRAVE_EXCEPTION_FORMAT0("plan init failed",ORE_Assert);
146  }
147 
148  // create a new output trajectory
149  TrajectoryBasePtr outputtraj = RaveCreateTrajectory(penv,"");
150  if( !planner->PlanPath(outputtraj) ) {
151  throw OPENRAVE_EXCEPTION_FORMAT0("plan failed",ORE_Assert);
152  }
153  listtrajectories.push_back(outputtraj);
154  listtrajectories.push_back(planningutils::ReverseTrajectory(outputtraj));
155  }
156 
157  while(IsOk()) {
158  for(list<TrajectoryBasePtr>::iterator it = listtrajectories.begin(); it != listtrajectories.end(); ++it) {
159  probot->GetController()->SetPath(*it);
160  WaitRobot(probot);
161  }
162  }
163  }
164 };
165 
166 } // end namespace cppexamples
167 
168 int main(int argc, char ** argv)
169 {
171  return example.main(argc,argv);
172 }
173