openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
orcollision.cpp
Go to the documentation of this file.
1 
23 #include <openrave-core.h>
24 #include <vector>
25 #include <cstring>
26 #include <sstream>
27 
28 using namespace OpenRAVE;
29 using namespace std;
30 
31 void printhelp()
32 {
33  RAVELOG_INFO("orcollision [--list] [--checker checker_name] [--joints #values [values]] body_model\n");
34 }
35 
37 {
38  std::map<InterfaceType, std::vector<std::string> > interfacenames;
39  RaveGetLoadedInterfaces(interfacenames);
40  stringstream ss;
41 
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;
47  ss << endl;
48  }
49  RAVELOG_INFO(ss.str());
50 }
51 
52 int main(int argc, char ** argv)
53 {
54  if( argc < 2 ) {
55  printhelp();
56  return -1; // no robots to load
57  }
58 
59  RaveInitialize(true); // start openrave core
60  EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
61  vector<dReal> vsetvalues;
62 
63  // parse the command line options
64  int i = 1;
65  while(i < argc) {
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)) {
67  printhelp();
68  return 0;
69  }
70  else if( strcmp(argv[i], "--checker") == 0 ) {
71  // create requested collision checker
72  CollisionCheckerBasePtr pchecker = RaveCreateCollisionChecker(penv,argv[i+1]);
73  if( !pchecker ) {
74  RAVELOG_ERROR("failed to create checker %s\n", argv[i+1]);
75  return -3;
76  }
77  penv->SetCollisionChecker(pchecker);
78  i += 2;
79  }
80  else if( strcmp(argv[i], "--list") == 0 ) {
81  printinterfaces(penv);
82  return 0;
83  }
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]);
88 
89  i += 2+vsetvalues.size();
90  }
91  else
92  break;
93  }
94 
95  if( i >= argc ) {
96  RAVELOG_ERROR("not enough parameters\n");
97  printhelp();
98  return 1;
99  }
100 
101  // load the scene
102  if( !penv->Load(argv[i]) ) {
103  return 2;
104  }
105 
106  int contactpoints = 0;
107  {
108  // lock the environment to prevent data from changing
109  EnvironmentMutex::scoped_lock lock(penv->GetMutex());
110 
111  vector<KinBodyPtr> vbodies;
112  penv->GetBodies(vbodies);
113  // get the first body
114  if( vbodies.size() == 0 ) {
115  RAVELOG_ERROR("no bodies loaded\n");
116  return -3;
117  }
118 
119  KinBodyPtr pbody = vbodies.at(0);
120  vector<dReal> values;
121  pbody->GetDOFValues(values);
122 
123  // set new values
124  for(int i = 0; i < (int)vsetvalues.size() && i < (int)values.size(); ++i) {
125  values[i] = vsetvalues[i];
126  }
127  pbody->SetDOFValues(values,true);
128 
129  CollisionReportPtr report(new CollisionReport());
130  penv->GetCollisionChecker()->SetCollisionOptions(CO_Contacts);
131  if( pbody->CheckSelfCollision(report) ) {
132  contactpoints = (int)report->contacts.size();
133  stringstream ss;
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) {
139  CollisionReport::CONTACT& c = report->contacts[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;
143  }
144 
145  RAVELOG_INFOA(ss.str());
146  }
147  else {
148  RAVELOG_INFO("body not in collision\n");
149  }
150 
151  // get the transformations of all the links
152  vector<Transform> vlinktransforms;
153  pbody->GetLinkTransformations(vlinktransforms);
154  }
155 
156  RaveDestroy(); // destroy
157  return contactpoints;
158 }