scc.cpp
Go to the documentation of this file.
00001 #include <hrpModel/Link.h>
00002 #include "scc.h"
00003 
00004 using namespace hrp;
00005 
00006 SelfCollisionChecker::SelfCollisionChecker(hrp::BodyPtr body, const hrp::LinkNamePairList &pairs) : m_robot(body)
00007 {
00008     for (unsigned int i=0; i<m_robot->numLinks(); i++){
00009         Link *link1 = m_robot->link(i);
00010         for (unsigned int j=i+1; j<m_robot->numLinks(); j++){
00011             Link *link2 = m_robot->link(j);
00012             if (link1->parent != link2 && link2->parent != link1){
00013                 bool skip = false;
00014                 for (unsigned int k=0; k<pairs.size(); k++){
00015                     if ((pairs[k].first == link1->name
00016                          && pairs[k].second == link2->name)
00017                         ||(pairs[k].first == link2->name
00018                            && pairs[k].second == link1->name)){
00019                         skip = true;
00020                         break;
00021                     }
00022                 }
00023                 if (!skip){
00024                     m_checkPairs.push_back(ColdetModelPair(link1->coldetModel,
00025                                                            link2->coldetModel));
00026                 }
00027             }
00028         }
00029     }
00030 }
00031 
00032 
00033 LinkNamePairList SelfCollisionChecker::check(const double *q)
00034 {
00035     LinkNamePairList pairs;
00036 
00037     for (unsigned int i=0; i<m_robot->numJoints(); i++){
00038         m_robot->joint(i)->q = q[i];
00039     }
00040     m_robot->calcForwardKinematics();
00041     for (unsigned int i=0; i<m_robot->numLinks(); i++){
00042         Link *l = m_robot->link(i);
00043         l->coldetModel->setPosition(l->attitude(), l->p);
00044     }
00045     for (unsigned int i=0; i<m_checkPairs.size(); i++){
00046         if (m_checkPairs[i].checkCollision()){
00047             pairs.push_back(std::make_pair(m_checkPairs[i].model(0)->name(),
00048                                            m_checkPairs[i].model(1)->name()));
00049         }
00050     }
00051     return pairs;
00052 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56