12 #include <boost/thread/thread.hpp>
13 #include <boost/bind.hpp>
15 using namespace OpenRAVE;
28 int main(
int argc,
char ** argv)
30 string scenefilename =
"data/pr2test1.env.xml";
33 penv->Load(scenefilename);
35 vector<RobotBasePtr> vrobots;
36 penv->GetRobots(vrobots);
38 probot->SetActiveManipulator(
"leftarm_torso");
43 penv->Add(pikfast,
true,
"");
44 stringstream ssin,ssout;
45 vector<dReal> vsolution;
46 ssin <<
"LoadIKFastSolver " << probot->GetName() <<
" " << (int)
IKP_Transform6D;
47 if( !pikfast->SendCommand(ssout,ssin) ) {
50 if( !pmanip->GetIkSolver()) {
55 probot->SetActiveDOFs(pmanip->GetArmIndices());
56 vector<dReal> vlower,vupper;
60 EnvironmentMutex::scoped_lock lock(penv->GetMutex());
63 probot->GetActiveDOFLimits(vlower,vupper);
64 vector<dReal> v(pmanip->GetArmIndices().size());
65 for(
size_t i = 0; i < vlower.size(); ++i) {
68 probot->SetActiveDOFValues(v);
69 bool bincollision = !penv->CheckCollision(probot) && !probot->CheckSelfCollision();