Load a robot into the openrave environment, set it at [joint values] and check for self collisions. Returns number of contact points.
#include <vector>
#include <cstring>
#include <sstream>
using namespace OpenRAVE;
using namespace std;
{
RAVELOG_INFO(
"orcollision [--list] [--checker checker_name] [--joints #values [values]] body_model\n");
}
{
std::map<InterfaceType, std::vector<std::string> > interfacenames;
stringstream ss;
ss << endl << "Loadable interfaces: " << endl;
for(std::map<InterfaceType, std::vector<std::string> >::iterator itinterface = interfacenames.begin(); itinterface != interfacenames.end(); ++itinterface) {
ss <<
RaveGetInterfaceName(itinterface->first) <<
"(" << itinterface->second.size() <<
"):" << endl;
for(vector<string>::iterator it = itinterface->second.begin(); it != itinterface->second.end(); ++it)
ss << " " << *it << endl;
ss << endl;
}
}
int main(
int argc,
char ** argv)
{
if( argc < 2 ) {
return -1;
}
vector<dReal> vsetvalues;
int i = 1;
while(i < argc) {
if((strcmp(argv[i], "-h") == 0)||(strcmp(argv[i], "-?") == 0)||(strcmp(argv[i], "/?") == 0)||(strcmp(argv[i], "--help") == 0)||(strcmp(argv[i], "-help") == 0)) {
return 0;
}
else if( strcmp(argv[i], "--checker") == 0 ) {
if( !pchecker ) {
return -3;
}
penv->SetCollisionChecker(pchecker);
i += 2;
}
else if( strcmp(argv[i], "--list") == 0 ) {
return 0;
}
else if( strcmp(argv[i], "--joints") == 0 ) {
vsetvalues.resize(atoi(argv[i+1]));
for(int j = 0; j < (int)vsetvalues.size(); ++j)
vsetvalues[j] = atoi(argv[i+j+2]);
i += 2+vsetvalues.size();
}
else
break;
}
if( i >= argc ) {
return 1;
}
if( !penv->Load(argv[i]) ) {
return 2;
}
int contactpoints = 0;
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
vector<KinBodyPtr> vbodies;
penv->GetBodies(vbodies);
if( vbodies.size() == 0 ) {
return -3;
}
vector<dReal> values;
pbody->GetDOFValues(values);
for(int i = 0; i < (int)vsetvalues.size() && i < (int)values.size(); ++i) {
values[i] = vsetvalues[i];
}
pbody->SetDOFValues(values,true);
penv->GetCollisionChecker()->SetCollisionOptions(
CO_Contacts);
if( pbody->CheckSelfCollision(report) ) {
contactpoints = (int)report->contacts.size();
stringstream ss;
ss << "body in self-collision "
<< (!!report->plink1 ? report->plink1->GetName() : "") << ":"
<< (!!report->plink2 ? report->plink2->GetName() : "") << " at "
<< contactpoints << "contacts" << endl;
for(int i = 0; i < contactpoints; ++i) {
ss << "contact" << i << ": pos=("
<< c.
pos.x <<
", " << c.
pos.y <<
", " << c.
pos.z <<
"), norm=("
<< c.
norm.x <<
", " << c.
norm.y <<
", " << c.
norm.z <<
")" << endl;
}
}
else {
}
vector<Transform> vlinktransforms;
pbody->GetLinkTransformations(vlinktransforms);
}
return contactpoints;
}