ModelNodeSet.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  */
00009 
00014 #include "ModelNodeSet.h"
00015 
00016 #include <bitset>
00017 #include <iostream>
00018 #include <algorithm>
00019 #include <hrpUtil/EasyScanner.h>
00020 #include <hrpUtil/VrmlParser.h>
00021 
00022 
00023 using namespace hrp;
00024 using namespace std;
00025 using namespace boost;
00026 
00027 
00028 namespace {
00029 
00030     typedef void (ModelNodeSetImpl::*ProtoCheckFunc)(void);
00031     
00032     struct ProtoInfo
00033     {
00034         ProtoInfo() { }
00035         ProtoInfo(int id, ProtoCheckFunc func) : id(id), protoCheckFunc(func) { }
00036         int id;
00037         ProtoCheckFunc protoCheckFunc;
00038     };
00039     
00040     typedef map<string,ProtoInfo> ProtoNameToInfoMap;
00041     ProtoNameToInfoMap protoNameToInfoMap;
00042 }
00043 
00044 
00045 namespace hrp {
00046 
00047     class ModelNodeSetImpl
00048     {
00049     public:
00050         ModelNodeSetImpl(ModelNodeSet* self);
00051 
00052         bool loadModelFile(const std::string& filename);
00053 
00054         ModelNodeSet* self;
00055         
00056         int numJointNodes;
00057         VrmlProtoInstancePtr humanoidNode;
00058         JointNodeSetPtr rootJointNodeSet;
00059         int messageIndent;
00060 
00061         VrmlProtoPtr protoToCheck;
00062 
00063                 enum {
00064             PROTO_UNDEFINED = 0,
00065             PROTO_HUMANOID,
00066             PROTO_JOINT,
00067             PROTO_SEGMENT,
00068             PROTO_SENSOR,
00069             PROTO_HARDWARECOMPONENT,
00070             PROTO_EXTRAJOINT,
00071             NUM_PROTOS
00072         };
00073 
00074                 std::vector<VrmlProtoInstancePtr> extraJointNodes;
00075 
00076         typedef std::bitset<NUM_PROTOS> ProtoIdSet;
00077     
00078         void extractHumanoidNode(VrmlParser& parser);
00079 
00080         void throwExceptionOfIllegalField(const std::string& name, VrmlFieldTypeId typeId);
00081         void requireField(const std::string& name, VrmlFieldTypeId type);
00082         void checkFieldType(const std::string& name, VrmlFieldTypeId type);
00083         VrmlVariantField* addField(const std::string& name, VrmlFieldTypeId type);
00084         void addFloatField(const std::string& name, double defaultValue);
00085                 
00086         void checkHumanoidProto();
00087         void checkJointProto();
00088         void checkSegmentProto();
00089         void checkSensorProtoCommon();
00090         void checkHardwareComponentProto();
00091         void extractJointNodes();
00092                 void checkExtraJointProto();
00093         JointNodeSetPtr addJointNodeSet(VrmlProtoInstancePtr jointNode);
00094         void extractChildNodes
00095             (JointNodeSetPtr jointNodeSet, MFNode& childNodes, const ProtoIdSet acceptableProtoIds, const Matrix44& T);
00096 
00097         void putMessage(const std::string& message);
00098     };
00099 }
00100 
00101 
00102 ModelNodeSet::ModelNodeSet()
00103 {
00104     impl = new ModelNodeSetImpl(this);
00105 }
00106 
00107 
00108 ModelNodeSetImpl::ModelNodeSetImpl(ModelNodeSet* self) : self(self)
00109 {
00110     numJointNodes = 0;
00111     messageIndent = 0;
00112 
00113     if(protoNameToInfoMap.empty()){
00114 
00115         protoNameToInfoMap["Humanoid"]
00116             = ProtoInfo(PROTO_HUMANOID, &ModelNodeSetImpl::checkHumanoidProto);
00117 
00118         protoNameToInfoMap["Joint"]
00119             = ProtoInfo(PROTO_JOINT, &ModelNodeSetImpl::checkJointProto);
00120         
00121         protoNameToInfoMap["Segment"]
00122             = ProtoInfo(PROTO_SEGMENT, &ModelNodeSetImpl::checkSegmentProto);
00123         
00124         protoNameToInfoMap["ForceSensor"]
00125             = ProtoInfo(PROTO_SENSOR, &ModelNodeSetImpl::checkSensorProtoCommon);
00126         
00127         protoNameToInfoMap["Gyro"]
00128             = ProtoInfo(PROTO_SENSOR, &ModelNodeSetImpl::checkSensorProtoCommon);
00129         
00130         protoNameToInfoMap["AccelerationSensor"]
00131             = ProtoInfo(PROTO_SENSOR, &ModelNodeSetImpl::checkSensorProtoCommon);
00132         
00133         protoNameToInfoMap["VisionSensor"]
00134             = ProtoInfo(PROTO_SENSOR, &ModelNodeSetImpl::checkSensorProtoCommon);
00135         
00136         protoNameToInfoMap["RangeSensor"]
00137             = ProtoInfo(PROTO_SENSOR, &ModelNodeSetImpl::checkSensorProtoCommon);
00138         
00139         protoNameToInfoMap["HardwareComponent"]
00140             = ProtoInfo(PROTO_HARDWARECOMPONENT, &ModelNodeSetImpl::checkHardwareComponentProto);
00141 
00142                 protoNameToInfoMap["ExtraJoint"]
00143             = ProtoInfo(PROTO_EXTRAJOINT, &ModelNodeSetImpl::checkExtraJointProto);
00144     }
00145 }
00146 
00147 
00148 ModelNodeSet::~ModelNodeSet()
00149 {
00150     delete impl;
00151 }
00152 
00153 
00154 int ModelNodeSet::numJointNodes()
00155 {
00156     return impl->numJointNodes;
00157 }
00158 
00159 
00160 VrmlProtoInstancePtr ModelNodeSet::humanoidNode()
00161 {
00162     return impl->humanoidNode;
00163 }
00164 
00165 
00166 JointNodeSetPtr ModelNodeSet::rootJointNodeSet()
00167 {
00168     return impl->rootJointNodeSet;
00169 }
00170 
00171 int ModelNodeSet::numExtraJointNodes()
00172 {
00173     return impl->extraJointNodes.size();
00174 }
00175 
00176 VrmlProtoInstancePtr ModelNodeSet::extraJointNode(int index)
00177 {
00178     return impl->extraJointNodes[index];
00179 }
00180 
00181 bool ModelNodeSet::loadModelFile(const std::string& filename)
00182 {
00183     return impl->loadModelFile(filename);
00184 }
00185 
00186 
00187 bool ModelNodeSetImpl::loadModelFile(const std::string& filename)
00188 {
00189     numJointNodes = 0;
00190     humanoidNode = 0;
00191     rootJointNodeSet.reset();
00192     messageIndent = 0;
00193         extraJointNodes.clear();
00194 
00195     try {
00196         VrmlParser parser;
00197         parser.load(filename);
00198         extractHumanoidNode(parser);
00199 
00200     } catch(EasyScanner::Exception& ex){
00201             throw ModelNodeSet::Exception(ex.getFullMessage());
00202     }
00203 
00204     return (humanoidNode && rootJointNodeSet);
00205 }
00206 
00207 
00208 void ModelNodeSetImpl::extractHumanoidNode(VrmlParser& parser)
00209 {
00210     while(VrmlNodePtr node = parser.readNode()){
00211                 
00212         if(node->isCategoryOf(PROTO_DEF_NODE)){
00213                         
00214             protoToCheck = static_pointer_cast<VrmlProto>(node);
00215                         
00216             ProtoNameToInfoMap::iterator p = protoNameToInfoMap.find(protoToCheck->protoName);
00217             if(p != protoNameToInfoMap.end()){
00218                 ProtoInfo& info = p->second;
00219                 (this->*info.protoCheckFunc)();
00220             }
00221                         
00222         } else if(node->isCategoryOf(PROTO_INSTANCE_NODE)){
00223                         
00224             VrmlProtoInstancePtr instance = static_pointer_cast<VrmlProtoInstance>(node);
00225             if(instance->proto->protoName == "Humanoid") {
00226                 humanoidNode = instance;
00227             } else if(instance->proto->protoName == "ExtraJoint") {
00228                 extraJointNodes.push_back(instance);
00229             }
00230         }
00231     }
00232         
00233     if(humanoidNode){
00234         putMessage("Humanoid node");
00235         extractJointNodes();
00236     } else {
00237         throw ModelNodeSet::Exception("Humanoid node is not found");
00238         }
00239 }
00240 
00241 
00242 void ModelNodeSetImpl::throwExceptionOfIllegalField(const std::string& name, VrmlFieldTypeId typeId)
00243 {
00244     string message = "Proto \"";
00245     message += protoToCheck->protoName + "\" must have the \"" + name + "\" field of " +
00246         VrmlNode::getLabelOfFieldType(typeId) + " type";
00247     throw ModelNodeSet::Exception(message);
00248 }
00249 
00250 
00251 void ModelNodeSetImpl::requireField(const std::string& name, VrmlFieldTypeId typeId)
00252 {
00253     VrmlVariantField* field = protoToCheck->getField(name);
00254     if(!field || field->typeId() != typeId){
00255         throwExceptionOfIllegalField(name, typeId);
00256     }
00257 }
00258 
00259 
00260 void ModelNodeSetImpl::checkFieldType(const std::string& name, VrmlFieldTypeId typeId)
00261 {
00262     VrmlVariantField* field = protoToCheck->getField(name);
00263     if(field && field->typeId() != typeId){
00264         throwExceptionOfIllegalField(name, typeId);
00265     }
00266 }
00267 
00268 
00269 VrmlVariantField* ModelNodeSetImpl::addField(const std::string& name, VrmlFieldTypeId typeId)
00270 {
00271     VrmlVariantField* field = protoToCheck->getField(name);
00272     if(!field){
00273         field = protoToCheck->addField(name, typeId);
00274     } else if(field->typeId() != typeId){
00275         throwExceptionOfIllegalField(name, typeId);
00276     }
00277     return field;
00278 }
00279 
00280 
00281 void ModelNodeSetImpl::addFloatField(const std::string& name, double defaultValue)
00282 {
00283     VrmlVariantField* field = protoToCheck->getField(name);
00284     if(!field){
00285         field = protoToCheck->addField(name, SFFLOAT);
00286         field->sfFloat() = defaultValue;
00287     } else if(field->typeId() != SFFLOAT){
00288         throwExceptionOfIllegalField(name, SFFLOAT);
00289     }
00290 }
00291 
00292 
00293 void ModelNodeSetImpl::checkHumanoidProto()
00294 {
00295     // necessary fields
00296     requireField("center", SFVEC3F);
00297     requireField("humanoidBody", MFNODE);
00298     requireField("rotation", SFROTATION);
00299     requireField("translation", SFVEC3F);
00300 
00301     // optional fields
00302     addField("info", MFSTRING);
00303     addField("name", SFSTRING);
00304     addField("version", SFSTRING);
00305     addField("scaleOrientation", SFROTATION);
00306 
00307     VrmlVariantField* field;
00308 
00309     if( (field = addField("scale", SFVEC3F)) != 0){
00310         SFVec3f& scale = field->sfVec3f();
00311         std::fill(scale.begin(), scale.end(), 1.0);
00312     }
00313 }
00314 
00315 
00316 void ModelNodeSetImpl::checkJointProto()
00317 {
00318     // necessary fields
00319     requireField("center", SFVEC3F);
00320     requireField("children", MFNODE);
00321     requireField("rotation", SFROTATION);
00322     requireField("translation", SFVEC3F);
00323     requireField("jointType", SFSTRING);
00324     requireField("jointId", SFINT32);
00325 
00326     VrmlVariantField* field;
00327 
00328     field = protoToCheck->getField("jointAxis");
00329     if(!field){
00330         throw ModelNodeSet::Exception
00331             ("Prototype of Humanoid must have the \"jointAxis\" field");
00332     }
00333     if(field->typeId() != SFSTRING && field->typeId() != SFVEC3F){
00334         throw ModelNodeSet::Exception
00335             ("The type of \"jointAxis\" field in \"Humanoid\" prototype must be SFString or SFVec3f");
00336     }
00337 
00338     // optional fields
00339     addField("llimit", MFFLOAT);
00340     addField("ulimit", MFFLOAT);
00341     addField("lvlimit", MFFLOAT);
00342     addField("uvlimit", MFFLOAT);
00343     addField("limitOrientation", SFROTATION);
00344     addField("name", SFSTRING);
00345 
00346     addFloatField("gearRatio", 1.0);
00347     addFloatField("rotorInertia", 0.0);
00348     addFloatField("rotorResistor", 0.0);
00349     addFloatField("torqueConst", 1.0);
00350     addFloatField("encoderPulse", 1.0);
00351 
00352     addFloatField("jointValue", 0.0);
00353 
00354     if( (field = addField("scale", SFVEC3F)) != 0){
00355         SFVec3f& scale = field->sfVec3f();
00356         std::fill(scale.begin(), scale.end(), 1.0);
00357     }
00358 
00359     if(protoToCheck->getField("equivalentInertia")){
00360         putMessage("The \"equivalentInertia\" field of the Joint node is obsolete.");
00361     }
00362 }
00363 
00364 
00365 void ModelNodeSetImpl::checkSegmentProto()
00366 {
00367     requireField("centerOfMass", SFVEC3F);
00368     requireField("mass", SFFLOAT);
00369     requireField("momentsOfInertia", MFFLOAT);
00370     addField("name", SFSTRING);
00371 }
00372 
00373 
00374 void ModelNodeSetImpl::checkSensorProtoCommon()
00375 {
00376     requireField("sensorId", SFINT32);
00377     requireField("translation", SFVEC3F);
00378     requireField("rotation", SFROTATION);
00379 }
00380 
00381 
00382 void ModelNodeSetImpl::checkHardwareComponentProto()
00383 {
00384     requireField("id", SFINT32);
00385     requireField("translation", SFVEC3F);
00386     requireField("rotation", SFROTATION);
00387     requireField("url", SFSTRING);
00388 }
00389 
00390 void ModelNodeSetImpl::checkExtraJointProto()
00391 {
00392     requireField("link1Name", SFSTRING);
00393     requireField("link2Name", SFSTRING);
00394         requireField("link1LocalPos", SFVEC3F);
00395     requireField("link2LocalPos", SFVEC3F);
00396     requireField("jointType", SFSTRING);
00397     requireField("jointAxis", SFVEC3F);
00398 }
00399 
00400 void ModelNodeSetImpl::extractJointNodes()
00401 {
00402     MFNode& nodes = humanoidNode->fields["humanoidBody"].mfNode();
00403 
00404     if(nodes.size() > 1){
00405         throw ModelNodeSet::Exception
00406             ("The Humanoid node must have a unique Joint node in its \"humanoidBody\" field.");
00407 
00408     } else if(nodes.size() == 1){
00409         if(nodes[0]->isCategoryOf(PROTO_INSTANCE_NODE)){
00410             VrmlProtoInstancePtr jointNode = dynamic_pointer_cast<VrmlProtoInstance>(nodes[0]);
00411             if(jointNode && jointNode->proto->protoName == "Joint"){
00412                 rootJointNodeSet = addJointNodeSet(jointNode);
00413             }
00414         }
00415     }
00416 
00417     if(!rootJointNodeSet){
00418         throw ModelNodeSet::Exception
00419             ("The Humanoid node does not have a Joint node in its \"humanoidBody\" field.");
00420     }
00421 }
00422 
00423 
00424 JointNodeSetPtr ModelNodeSetImpl::addJointNodeSet(VrmlProtoInstancePtr jointNode)
00425 {
00426     numJointNodes++;
00427 
00428     putMessage(string("Joint node ") + jointNode->defName);
00429 
00430     JointNodeSetPtr jointNodeSet(new JointNodeSet());
00431 
00432     jointNodeSet->jointNode = jointNode;
00433 
00434     MFNode& childNodes = jointNode->fields["children"].mfNode();
00435 
00436     ProtoIdSet acceptableProtoIds;
00437     acceptableProtoIds.set(PROTO_JOINT);
00438     acceptableProtoIds.set(PROTO_SEGMENT);
00439     acceptableProtoIds.set(PROTO_SENSOR);
00440     acceptableProtoIds.set(PROTO_HARDWARECOMPONENT);
00441     
00442     Matrix44 T(Matrix44::Identity());
00443     extractChildNodes(jointNodeSet, childNodes, acceptableProtoIds, T);
00444 
00445     return jointNodeSet;
00446 }
00447 
00448 void ModelNodeSetImpl::extractChildNodes
00449 (JointNodeSetPtr jointNodeSet, MFNode& childNodes, const ProtoIdSet acceptableProtoIds, const Matrix44& T)
00450 {
00451     for(size_t i = 0; i < childNodes.size(); i++){
00452         VrmlNode* childNode = childNodes[i].get();
00453         const Matrix44* pT;
00454         if(childNode->isCategoryOf(GROUPING_NODE)){
00455             VrmlGroup* groupNode = static_cast<VrmlGroup*>(childNode);
00456             VrmlTransform* transformNode = dynamic_cast<VrmlTransform*>(groupNode);
00457             Matrix44 T2;
00458             if( transformNode ){
00459                 Matrix44 Tlocal;
00460                 calcTransformMatrix(transformNode, Tlocal);
00461                 T2 = T * Tlocal;
00462                 pT = &T2;
00463             } else {
00464                 pT = &T;
00465             }
00466             extractChildNodes(jointNodeSet, groupNode->getChildren(), acceptableProtoIds, *pT);
00467 
00468         } else if(childNode->isCategoryOf(LIGHT_NODE)){
00469             jointNodeSet->lightNodes.push_back(std::make_pair(T,childNode));
00470         } else if(childNode->isCategoryOf(PROTO_INSTANCE_NODE)){
00471 
00472             VrmlProtoInstance* protoInstance = static_cast<VrmlProtoInstance*>(childNode);
00473             int id = PROTO_UNDEFINED;
00474             bool doTraverseChildren = false;
00475             ProtoIdSet acceptableChildProtoIds(acceptableProtoIds);
00476 
00477             const string& protoName = protoInstance->proto->protoName;
00478             ProtoNameToInfoMap::iterator p = protoNameToInfoMap.find(protoName);
00479 
00480             if(p == protoNameToInfoMap.end()){
00481                 doTraverseChildren = true;
00482             } else {
00483                 id = p->second.id;
00484                 if(!acceptableProtoIds.test(id)){
00485                     throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
00486                 }
00487             }
00488 
00489             messageIndent += 2;
00490 
00491             switch(id){
00492                 
00493             case PROTO_JOINT:
00494                 if(T != Matrix44::Identity())
00495                     throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
00496                 jointNodeSet->childJointNodeSets.push_back(addJointNodeSet(protoInstance));
00497                 break;
00498                 
00499             case PROTO_SENSOR:
00500                 if(T != Matrix44::Identity())
00501                     throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
00502                 jointNodeSet->sensorNodes.push_back(protoInstance);
00503                 putMessage(protoName + protoInstance->defName);
00504                 break;
00505                 
00506             case PROTO_HARDWARECOMPONENT:
00507                 if(T != Matrix44::Identity())
00508                     throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
00509                 jointNodeSet->hwcNodes.push_back(protoInstance);
00510                 putMessage(protoName + protoInstance->defName);
00511                 break;
00512                 
00513             case PROTO_SEGMENT:
00514                 {
00515                     jointNodeSet->segmentNodes.push_back(protoInstance);
00516                     jointNodeSet->transforms.push_back(T);
00517                     putMessage(string("Segment node ") + protoInstance->defName);
00518 
00519                     doTraverseChildren = true;
00520                     acceptableChildProtoIds.reset();
00521                     acceptableChildProtoIds.set(PROTO_SENSOR);
00522                 }
00523                 break;
00524                 
00525             default:
00526                 break;
00527             }
00528 
00529             if(doTraverseChildren){
00530                 MFNode& childNodes = protoInstance->fields["children"].mfNode();
00531                 if(&childNodes)
00532                     extractChildNodes(jointNodeSet, childNodes, acceptableChildProtoIds, T);
00533             }
00534 
00535             messageIndent -= 2;
00536         }
00537     }
00538 }
00539 
00540 
00541 void ModelNodeSetImpl::putMessage(const std::string& message)
00542 {
00543     if(!self->sigMessage.empty()) {
00544         string space(messageIndent, ' ');
00545         self->sigMessage(space + message + "\n");
00546     }
00547 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:55