adds a tool, hrpsys-self-collision-checker

util/CMakeLists.txt

@@ -1,4 +1,5 @@
 add_subdirectory(ProjectGenerator)
+add_subdirectory(SelfCollisionChecker)
 if(USE_HRPSYSUTIL)
   add_subdirectory(simulator)
   add_subdirectory(viewer)

util/SelfCollisionChecker/CMakeLists.txt

@@ -0,0 +1,8 @@
+add_executable(hrpsys-self-collision-checker scc.cpp main.cpp)
+target_link_libraries(hrpsys-self-collision-checker ${OPENHRP_LIBRARIES})
+
+set(target hrpsys-self-collision-checker)
+
+install(TARGETS ${target}
+  RUNTIME DESTINATION bin
+)

util/SelfCollisionChecker/main.cpp

@@ -0,0 +1,54 @@
+#include <hrpModel/ModelLoaderUtil.h>
+#include <iostream>
+#include <fstream>
+#include "scc.h"
+
+int main(int argc, char *argv[])
+{
+    if (argc < 3){
+        std::cerr << "Usage: " << argv[0] << "[VRML model] [log file]" 
+                  << std::endl;
+        return 1;
+    }
+
+    hrp::LinkNamePairList pairs;
+    for (int i=3; i<argc; i++){
+        std::string str = argv[i];
+        int pos;
+        if ((pos = str.find(":")) != std::string::npos){
+            std::string link1 = str.substr(0, pos);
+            std::string link2 = str.substr(pos+1);
+            std::cerr << "[" << link1 << "],[" << link2 << "]" << std::endl;
+            pairs.push_back(std::make_pair(link1, link2));
+        }
+    }
+
+    hrp::BodyPtr robot = hrp::BodyPtr(new hrp::Body());
+    if (!loadBodyFromModelLoader(robot, argv[1], 
+                                 argc, argv, true)){
+        std::cerr << "Error: failed to load model[" << argv[1] << "]" 
+                  << std::endl;
+        return 2;
+    }
+
+    hrp::SelfCollisionChecker scc(robot, pairs);
+    std::cerr << scc.numOfCheckPairs() << " pairs are defined" << std::endl;
+
+    std::ifstream ifs(argv[2]);
+    double tm, q[robot->numJoints()];
+
+    ifs >> tm;
+    while (!ifs.eof()){
+        for (int i=0; i<robot->numJoints(); i++){
+            ifs >> q[i];
+        }
+        pairs = scc.check(q);
+        for (unsigned int i=0; i<pairs.size(); i++){
+            std::cout << tm << " " << pairs[i].first << ":" << pairs[i].second
+                      << std::endl;
+        }
+        ifs >> tm;
+    }
+
+    return 0;
+}

util/SelfCollisionChecker/scc.cpp

@@ -0,0 +1,52 @@
+#include <hrpModel/Link.h>
+#include "scc.h"
+
+using namespace hrp;
+
+SelfCollisionChecker::SelfCollisionChecker(hrp::BodyPtr body, const hrp::LinkNamePairList &pairs) : m_robot(body)
+{
+    for (int i=0; i<m_robot->numLinks(); i++){
+        Link *link1 = m_robot->link(i);
+        for (int j=i+1; j<m_robot->numLinks(); j++){
+            Link *link2 = m_robot->link(j);
+            if (link1->parent != link2 && link2->parent != link1){
+                bool skip = false;
+                for (unsigned int k=0; k<pairs.size(); k++){
+                    if ((pairs[k].first == link1->name
+                         && pairs[k].second == link2->name)
+                        ||(pairs[k].first == link2->name
+                           && pairs[k].second == link1->name)){
+                        skip = true;
+                        break;
+                    }
+                }
+                if (!skip){
+                    m_checkPairs.push_back(ColdetModelPair(link1->coldetModel,
+                                                           link2->coldetModel));
+                }
+            }
+        }
+    }
+}
+
+
+LinkNamePairList SelfCollisionChecker::check(const double *q)
+{
+    LinkNamePairList pairs;
+
+    for (int i=0; i<m_robot->numJoints(); i++){
+        m_robot->joint(i)->q = q[i];
+    }
+    m_robot->calcForwardKinematics();
+    for (int i=0; i<m_robot->numLinks(); i++){
+        Link *l = m_robot->link(i);
+        l->coldetModel->setPosition(l->attitude(), l->p);
+    }
+    for (unsigned int i=0; i<m_checkPairs.size(); i++){
+        if (m_checkPairs[i].checkCollision()){
+            pairs.push_back(std::make_pair(m_checkPairs[i].model(0)->name(),
+                                           m_checkPairs[i].model(1)->name()));
+        }
+    }
+    return pairs;
+}

util/SelfCollisionChecker/scc.h

@@ -0,0 +1,20 @@
+#include <hrpModel/Body.h>
+#include <hrpCollision/ColdetModelPair.h>
+
+namespace hrp{
+
+typedef std::vector<std::pair<std::string, std::string> > LinkNamePairList;
+
+class SelfCollisionChecker
+{
+public:
+    SelfCollisionChecker(hrp::BodyPtr body, 
+                         const LinkNamePairList& pairs=LinkNamePairList());
+    LinkNamePairList check(const double *q);
+    unsigned int numOfCheckPairs() const { return m_checkPairs.size(); }
+private:
+    hrp::BodyPtr m_robot;
+    std::vector<hrp::ColdetModelPair> m_checkPairs;
+};
+
+}