Shows how to use set a custom inverse kinematics filter to add extra constraints.
#include <vector>
#include <sstream>
#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>
using namespace OpenRAVE;
using namespace std;
{
}
}
int main(
int argc,
char ** argv)
{
string scenefilename = "data/pr2test1.env.xml";
penv->Load(scenefilename);
vector<RobotBasePtr> vrobots;
penv->GetRobots(vrobots);
probot->SetActiveManipulator("leftarm_torso");
penv->Add(pikfast,true,"");
stringstream ssin,ssout;
vector<dReal> vsolution;
ssin <<
"LoadIKFastSolver " << probot->GetName() <<
" " << (int)
IKP_Transform6D;
if( !pikfast->SendCommand(ssout,ssin) ) {
}
if( !pmanip->GetIkSolver()) {
penv->Destroy();
return 1;
}
probot->SetActiveDOFs(pmanip->GetArmIndices());
vector<dReal> vlower,vupper;
while(1) {
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
probot->GetActiveDOFLimits(vlower,vupper);
vector<dReal> v(pmanip->GetArmIndices().size());
for(size_t i = 0; i < vlower.size(); ++i) {
}
probot->SetActiveDOFValues(v);
bool bincollision = !penv->CheckCollision(probot) && !probot->CheckSelfCollision();
}
}
return 0;
}