00001
00002
00003
00004
00005
00006
00007
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
00296 requireField("center", SFVEC3F);
00297 requireField("humanoidBody", MFNODE);
00298 requireField("rotation", SFROTATION);
00299 requireField("translation", SFVEC3F);
00300
00301
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
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
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 }