bool GetIKSolutions(EnvironmentBasePtr _penv, Transform Pose, std::vector< std::vector< dReal > > &vsolution){
ModuleBasePtr _pikfast = RaveCreateModule(_penv,"ikfast");
RobotBasePtr _probot = _penv->GetRobot(robotname);
_probot->SetActiveManipulator("1");
RobotBase::ManipulatorPtr _pmanip = _probot->GetActiveManipulator();
_penv->Add(_pikfast,true,"");
std::vector< std::vector< dReal > > solns;
solns.resize(6);
stringstream ssin,ssout;
string iktype = "Transform6D";
ssin << "LoadIKFastSolver " << _probot->GetName() << " " << iktype;
if( !_pikfast->SendCommand(ssout,ssin) ) {
RAVELOG_ERROR("failed to load iksolver\n");
_penv->Destroy();
return false;
}
if(_pmanip->FindIKSolutions(IkParameterization(Pose),vsolution,IKFO_CheckEnvCollisions) ) {
stringstream ss; ss << "solution is: ";
for(size_t i = 0; i < vsolution.size(); ++i) {
for(size_t j = 0; j < vsolution[i].size(); ++j){
ss << vsolution[i][j] << " " ;
} ss << endl;
}
ss << endl;
////RAVELOG_INFO(ss.str());
}
else {
// could fail due to collisions, etc
return false;
}
return true ;
}
void SetViewer(EnvironmentBasePtr penv, const string& viewername)
{
ViewerBasePtr viewer = RaveCreateViewer(penv,viewername);
BOOST_ASSERT(!!viewer);
// attach it to the environment:
penv->Add(viewer);
// finally call the viewer's infinite loop (this is why a separate thread is needed)
bool showgui = true;
viewer->main(showgui);
}
int main(int argc, char ** argv)
{
if( argc < 3 ) {
RAVELOG_INFO("ikloader robot iktype\n");
return 1;
}
string robotname = argv[1];
string iktype = argv[2];
RaveInitialize(true); // start openrave core
EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
{
// lock the environment to prevent changes
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
// load the scene
RobotBasePtr probot = penv->ReadRobotXMLFile(robotname);
if( !probot ) {
penv->Destroy();
return 2;
}
penv->Add(probot);
ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
penv->Add(pikfast,true,"");
stringstream ssin,ssout;
ssin << "LoadIKFastSolver " << probot->GetName() << " " << iktype;
// if necessary, add free inc for degrees of freedom
//ssin << " " << 0.04f;
// get the active manipulator
RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
if( !pikfast->SendCommand(ssout,ssin) ) {
RAVELOG_ERROR("failed to load iksolver\n");
penv->Destroy();
return 1;
}
RAVELOG_INFO("testing random ik\n");
while(1) {
Transform trans;
trans.rot = quatFromAxisAngle(Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5));
trans.trans = Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5)*2;
vector<dReal> vsolution;
if( pmanip->FindIKSolution(IkParameterization(trans),vsolution,IKFO_CheckEnvCollisions) ) {
stringstream ss; ss << "solution is: ";
for(size_t i = 0; i < vsolution.size(); ++i) {
ss << vsolution[i] << " ";
}
ss << endl;
RAVELOG_INFO(ss.str());
}
else {
// could fail due to collisions, etc
}
}
}
RaveDestroy(); // destroy
return 0;
}
