28 using namespace OpenRAVE;
33 RAVELOG_INFO(
"orcollision [--list] [--checker checker_name] [--joints #values [values]] body_model\n");
38 std::map<InterfaceType, std::vector<std::string> > interfacenames;
42 ss << endl <<
"Loadable interfaces: " << endl;
43 for(std::map<InterfaceType, std::vector<std::string> >::iterator itinterface = interfacenames.begin(); itinterface != interfacenames.end(); ++itinterface) {
44 ss <<
RaveGetInterfaceName(itinterface->first) <<
"(" << itinterface->second.size() <<
"):" << endl;
45 for(vector<string>::iterator it = itinterface->second.begin(); it != itinterface->second.end(); ++it)
46 ss <<
" " << *it << endl;
52 int main(
int argc,
char ** argv)
61 vector<dReal> vsetvalues;
66 if((strcmp(argv[i],
"-h") == 0)||(strcmp(argv[i],
"-?") == 0)||(strcmp(argv[i],
"/?") == 0)||(strcmp(argv[i],
"--help") == 0)||(strcmp(argv[i],
"-help") == 0)) {
70 else if( strcmp(argv[i],
"--checker") == 0 ) {
77 penv->SetCollisionChecker(pchecker);
80 else if( strcmp(argv[i],
"--list") == 0 ) {
84 else if( strcmp(argv[i],
"--joints") == 0 ) {
85 vsetvalues.resize(atoi(argv[i+1]));
86 for(
int j = 0; j < (int)vsetvalues.size(); ++j)
87 vsetvalues[j] = atoi(argv[i+j+2]);
89 i += 2+vsetvalues.size();
102 if( !penv->Load(argv[i]) ) {
106 int contactpoints = 0;
109 EnvironmentMutex::scoped_lock lock(penv->GetMutex());
111 vector<KinBodyPtr> vbodies;
112 penv->GetBodies(vbodies);
114 if( vbodies.size() == 0 ) {
120 vector<dReal> values;
121 pbody->GetDOFValues(values);
124 for(
int i = 0; i < (int)vsetvalues.size() && i < (int)values.size(); ++i) {
125 values[i] = vsetvalues[i];
127 pbody->SetDOFValues(values,
true);
130 penv->GetCollisionChecker()->SetCollisionOptions(
CO_Contacts);
131 if( pbody->CheckSelfCollision(report) ) {
132 contactpoints = (int)report->contacts.size();
134 ss <<
"body in self-collision "
135 << (!!report->plink1 ? report->plink1->GetName() :
"") <<
":"
136 << (!!report->plink2 ? report->plink2->GetName() :
"") <<
" at "
137 << contactpoints <<
"contacts" << endl;
138 for(
int i = 0; i < contactpoints; ++i) {
140 ss <<
"contact" << i <<
": pos=("
141 << c.
pos.x <<
", " << c.
pos.y <<
", " << c.
pos.z <<
"), norm=("
142 << c.
norm.x <<
", " << c.
norm.y <<
", " << c.
norm.z <<
")" << endl;
152 vector<Transform> vlinktransforms;
153 pbody->GetLinkTransformations(vlinktransforms);
157 return contactpoints;