11 #include <boost/format.hpp>
17 using namespace OpenRAVE;
20 namespace cppexamples {
34 while(!probot->GetController()->IsDone() && IsOk()) {
35 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
40 string scenefilename =
"data/pr2test1.env.xml";
42 penv->Load(scenefilename);
43 vector<RobotBasePtr> vrobots;
44 penv->GetRobots(vrobots);
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");
57 t.trans =
Vector(-0.2,-0.2,1);
58 target->SetTransform(t);
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) ) {
77 penv->Add(basemodule,
true,probot->GetName());
79 penv->Add(taskmodule,
true,probot->GetName());
82 EnvironmentMutex::scoped_lock lock(penv->GetMutex());
83 stringstream ssin, ssout; ssin <<
"ReleaseFingers";
84 taskmodule->SendCommand(ssout,ssin);
91 EnvironmentMutex::scoped_lock lock(penv->GetMutex());
93 Toffset.trans =
Vector(0,0.2,0);
94 Transform Ttarget0 = target->GetTransform();
98 Tgrasp0.trans = Ttarget0*Toffset.trans;
99 Transform Tgraspoffset = Ttarget0.inverse() * Tgrasp0;
100 Transform Tgrasp1 = Ttarget1 * Tgraspoffset;
104 vector<dReal> values;
105 workspacetraj->Init(spec);
106 for(
size_t i = 0; i < 32; ++i) {
107 dReal angle = i*0.05;
113 workspacetraj->Insert(workspacetraj->GetNumWaypoints(),values);
116 std::vector<dReal> maxvelocities(7,1.0);
117 std::vector<dReal> maxaccelerations(7,5.0);
119 RAVELOG_INFO(str(boost::format(
"duration=%f, points=%d")%workspacetraj->GetDuration()%workspacetraj->GetNumWaypoints()));
123 stringstream ssout, ssin; ssin <<
"MoveToHandPosition poses 1 " << Tgrasp0;
124 basemodule->SendCommand(ssout,ssin);
128 stringstream ssin, ssout; ssin <<
"CloseFingers";
129 taskmodule->SendCommand(ssout,ssin);
133 list<TrajectoryBasePtr> listtrajectories;
135 EnvironmentMutex::scoped_lock lock(penv->GetMutex());
136 probot->SetActiveDOFs(pmanip->GetArmIndices());
137 probot->Grab(target);
140 params->SetRobotActiveJoints(probot);
141 params->workspacetraj = workspacetraj;
144 if( !planner->InitPlan(probot,params) ) {
150 if( !planner->PlanPath(outputtraj) ) {
153 listtrajectories.push_back(outputtraj);
158 for(list<TrajectoryBasePtr>::iterator it = listtrajectories.begin(); it != listtrajectories.end(); ++it) {
159 probot->GetController()->SetPath(*it);
168 int main(
int argc,
char ** argv)
171 return example.
main(argc,argv);