Shows how to set a workspace trajectory for the hand and have a robot plan it.
#include <vector>
#include <sstream>
#include <boost/format.hpp>
using namespace OpenRAVE;
using namespace std;
namespace cppexamples {
class PR2TurnLevelExample : public OpenRAVEExample
{
public:
{
}
while(!probot->GetController()->IsDone() && IsOk()) {
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
}
}
virtual void demothread(int argc, char ** argv) {
string scenefilename = "data/pr2test1.env.xml";
penv->Load(scenefilename);
vector<RobotBasePtr> vrobots;
penv->GetRobots(vrobots);
if( !target ) {
std::vector<AABB> boxes(1);
boxes[0].pos =
Vector(0,0.1,0);
boxes[0].extents =
Vector(0.01,0.1,0.01);
target->InitFromBoxes(boxes,true);
target->SetName("lever");
penv->Add(target);
t.trans =
Vector(-0.2,-0.2,1);
target->SetTransform(t);
}
penv->Add(pikfast,true,"");
stringstream ssin,ssout;
vector<dReal> vsolution;
ssin <<
"LoadIKFastSolver " << probot->GetName() <<
" " << (int)
IKP_Transform6D;
if( !pikfast->SendCommand(ssout,ssin) ) {
}
penv->Add(basemodule,true,probot->GetName());
penv->Add(taskmodule,true,probot->GetName());
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
stringstream ssin, ssout; ssin << "ReleaseFingers";
taskmodule->SendCommand(ssout,ssin);
}
WaitRobot(probot);
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
Toffset.trans =
Vector(0,0.2,0);
Tgrasp0.trans = Ttarget0*Toffset.trans;
Transform Tgraspoffset = Ttarget0.inverse() * Tgrasp0;
vector<dReal> values;
workspacetraj->Init(spec);
for(size_t i = 0; i < 32; ++i) {
workspacetraj->Insert(workspacetraj->GetNumWaypoints(),values);
}
std::vector<dReal> maxvelocities(7,1.0);
std::vector<dReal> maxaccelerations(7,5.0);
RAVELOG_INFO(str(boost::format(
"duration=%f, points=%d")%workspacetraj->GetDuration()%workspacetraj->GetNumWaypoints()));
}
{
stringstream ssout, ssin; ssin << "MoveToHandPosition poses 1 " << Tgrasp0;
basemodule->SendCommand(ssout,ssin);
}
WaitRobot(probot);
{
stringstream ssin, ssout; ssin << "CloseFingers";
taskmodule->SendCommand(ssout,ssin);
}
WaitRobot(probot);
list<TrajectoryBasePtr> listtrajectories;
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
probot->SetActiveDOFs(pmanip->GetArmIndices());
probot->Grab(target);
params->SetRobotActiveJoints(probot);
params->workspacetraj = workspacetraj;
if( !planner->InitPlan(probot,params) ) {
}
if( !planner->PlanPath(outputtraj) ) {
}
listtrajectories.push_back(outputtraj);
}
while(IsOk()) {
for(list<TrajectoryBasePtr>::iterator it = listtrajectories.begin(); it != listtrajectories.end(); ++it) {
probot->GetController()->SetPath(*it);
WaitRobot(probot);
}
}
}
};
}
int main(
int argc,
char ** argv)
{
return example.
main(argc,argv);
}