ModelNodeSet.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
9 
14 #include "ModelNodeSet.h"
15 
16 #include <bitset>
17 #include <iostream>
18 #include <algorithm>
19 #include <hrpUtil/EasyScanner.h>
20 #include <hrpUtil/VrmlParser.h>
21 
22 
23 using namespace hrp;
24 using namespace std;
25 using namespace boost;
26 
27 
28 namespace {
29 
30  typedef void (ModelNodeSetImpl::*ProtoCheckFunc)(void);
31 
32  struct ProtoInfo
33  {
34  ProtoInfo() { }
35  ProtoInfo(int id, ProtoCheckFunc func) : id(id), protoCheckFunc(func) { }
36  int id;
37  ProtoCheckFunc protoCheckFunc;
38  };
39 
40  typedef map<string,ProtoInfo> ProtoNameToInfoMap;
41  ProtoNameToInfoMap protoNameToInfoMap;
42 }
43 
44 
45 namespace hrp {
46 
48  {
49  public:
51 
52  bool loadModelFile(const std::string& filename);
53 
54  ModelNodeSet* self;
55 
60 
62 
63  enum {
64  PROTO_UNDEFINED = 0,
71  NUM_PROTOS
72  };
73 
74  std::vector<VrmlProtoInstancePtr> extraJointNodes;
75 
76  typedef std::bitset<NUM_PROTOS> ProtoIdSet;
77 
78  void extractHumanoidNode(VrmlParser& parser);
79 
80  void throwExceptionOfIllegalField(const std::string& name, VrmlFieldTypeId typeId);
81  void requireField(const std::string& name, VrmlFieldTypeId type);
82  void checkFieldType(const std::string& name, VrmlFieldTypeId type);
83  VrmlVariantField* addField(const std::string& name, VrmlFieldTypeId type);
84  void addFloatField(const std::string& name, double defaultValue);
85 
86  void checkHumanoidProto();
87  void checkJointProto();
88  void checkSegmentProto();
89  void checkSensorProtoCommon();
90  void checkHardwareComponentProto();
91  void extractJointNodes();
92  void checkExtraJointProto();
93  JointNodeSetPtr addJointNodeSet(VrmlProtoInstancePtr jointNode);
94  void extractChildNodes
95  (JointNodeSetPtr jointNodeSet, MFNode& childNodes, const ProtoIdSet acceptableProtoIds, const Matrix44& T);
96 
97  void putMessage(const std::string& message);
98  };
99 }
100 
101 
103 {
104  impl = new ModelNodeSetImpl(this);
105 }
106 
107 
109 {
110  numJointNodes = 0;
111  messageIndent = 0;
112 
113  if(protoNameToInfoMap.empty()){
114 
115  protoNameToInfoMap["Humanoid"]
117 
118  protoNameToInfoMap["Joint"]
120 
121  protoNameToInfoMap["Segment"]
123 
124  protoNameToInfoMap["ForceSensor"]
126 
127  protoNameToInfoMap["Gyro"]
129 
130  protoNameToInfoMap["AccelerationSensor"]
132 
133  protoNameToInfoMap["VisionSensor"]
135 
136  protoNameToInfoMap["RangeSensor"]
138 
139  protoNameToInfoMap["HardwareComponent"]
141 
142  protoNameToInfoMap["ExtraJoint"]
144  }
145 }
146 
147 
149 {
150  delete impl;
151 }
152 
153 
155 {
156  return impl->numJointNodes;
157 }
158 
159 
161 {
162  return impl->humanoidNode;
163 }
164 
165 
167 {
168  return impl->rootJointNodeSet;
169 }
170 
172 {
173  return impl->extraJointNodes.size();
174 }
175 
177 {
178  return impl->extraJointNodes[index];
179 }
180 
181 bool ModelNodeSet::loadModelFile(const std::string& filename)
182 {
183  return impl->loadModelFile(filename);
184 }
185 
186 
188 {
189  numJointNodes = 0;
190  humanoidNode = 0;
191  rootJointNodeSet.reset();
192  messageIndent = 0;
193  extraJointNodes.clear();
194 
195  try {
196  VrmlParser parser;
197  parser.load(filename);
198  extractHumanoidNode(parser);
199 
200  } catch(EasyScanner::Exception& ex){
202  }
203 
204  return (humanoidNode && rootJointNodeSet);
205 }
206 
207 
209 {
210  while(VrmlNodePtr node = parser.readNode()){
211 
212  if(node->isCategoryOf(PROTO_DEF_NODE)){
213 
214  protoToCheck = static_pointer_cast<VrmlProto>(node);
215 
216  ProtoNameToInfoMap::iterator p = protoNameToInfoMap.find(protoToCheck->protoName);
217  if(p != protoNameToInfoMap.end()){
218  ProtoInfo& info = p->second;
219  (this->*info.protoCheckFunc)();
220  }
221 
222  } else if(node->isCategoryOf(PROTO_INSTANCE_NODE)){
223 
224  VrmlProtoInstancePtr instance = static_pointer_cast<VrmlProtoInstance>(node);
225  if(instance->proto->protoName == "Humanoid") {
226  humanoidNode = instance;
227  } else if(instance->proto->protoName == "ExtraJoint") {
228  extraJointNodes.push_back(instance);
229  }
230  }
231  }
232 
233  if(humanoidNode){
234  putMessage("Humanoid node");
236  } else {
237  throw ModelNodeSet::Exception("Humanoid node is not found");
238  }
239 }
240 
241 
243 {
244  string message = "Proto \"";
245  message += protoToCheck->protoName + "\" must have the \"" + name + "\" field of " +
246  VrmlNode::getLabelOfFieldType(typeId) + " type";
247  throw ModelNodeSet::Exception(message);
248 }
249 
250 
251 void ModelNodeSetImpl::requireField(const std::string& name, VrmlFieldTypeId typeId)
252 {
253  VrmlVariantField* field = protoToCheck->getField(name);
254  if(!field || field->typeId() != typeId){
255  throwExceptionOfIllegalField(name, typeId);
256  }
257 }
258 
259 
260 void ModelNodeSetImpl::checkFieldType(const std::string& name, VrmlFieldTypeId typeId)
261 {
262  VrmlVariantField* field = protoToCheck->getField(name);
263  if(field && field->typeId() != typeId){
264  throwExceptionOfIllegalField(name, typeId);
265  }
266 }
267 
268 
270 {
271  VrmlVariantField* field = protoToCheck->getField(name);
272  if(!field){
273  field = protoToCheck->addField(name, typeId);
274  } else if(field->typeId() != typeId){
275  throwExceptionOfIllegalField(name, typeId);
276  }
277  return field;
278 }
279 
280 
281 void ModelNodeSetImpl::addFloatField(const std::string& name, double defaultValue)
282 {
283  VrmlVariantField* field = protoToCheck->getField(name);
284  if(!field){
285  field = protoToCheck->addField(name, SFFLOAT);
286  field->sfFloat() = defaultValue;
287  } else if(field->typeId() != SFFLOAT){
289  }
290 }
291 
292 
294 {
295  // necessary fields
296  requireField("center", SFVEC3F);
297  requireField("humanoidBody", MFNODE);
298  requireField("rotation", SFROTATION);
299  requireField("translation", SFVEC3F);
300 
301  // optional fields
302  addField("info", MFSTRING);
303  addField("name", SFSTRING);
304  addField("version", SFSTRING);
305  addField("scaleOrientation", SFROTATION);
306 
307  VrmlVariantField* field;
308 
309  if( (field = addField("scale", SFVEC3F)) != 0){
310  SFVec3f& scale = field->sfVec3f();
311  std::fill(scale.begin(), scale.end(), 1.0);
312  }
313 }
314 
315 
317 {
318  // necessary fields
319  requireField("center", SFVEC3F);
320  requireField("children", MFNODE);
321  requireField("rotation", SFROTATION);
322  requireField("translation", SFVEC3F);
323  requireField("jointType", SFSTRING);
324  requireField("jointId", SFINT32);
325 
326  VrmlVariantField* field;
327 
328  field = protoToCheck->getField("jointAxis");
329  if(!field){
331  ("Prototype of Humanoid must have the \"jointAxis\" field");
332  }
333  if(field->typeId() != SFSTRING && field->typeId() != SFVEC3F){
335  ("The type of \"jointAxis\" field in \"Humanoid\" prototype must be SFString or SFVec3f");
336  }
337 
338  // optional fields
339  addField("llimit", MFFLOAT);
340  addField("ulimit", MFFLOAT);
341  addField("lvlimit", MFFLOAT);
342  addField("uvlimit", MFFLOAT);
343  addField("limitOrientation", SFROTATION);
344  addField("name", SFSTRING);
345 
346  addFloatField("gearRatio", 1.0);
347  addFloatField("rotorInertia", 0.0);
348  addFloatField("rotorResistor", 0.0);
349  addFloatField("torqueConst", 1.0);
350  addFloatField("encoderPulse", 1.0);
351 
352  addFloatField("jointValue", 0.0);
353 
354  if( (field = addField("scale", SFVEC3F)) != 0){
355  SFVec3f& scale = field->sfVec3f();
356  std::fill(scale.begin(), scale.end(), 1.0);
357  }
358 
359  if(protoToCheck->getField("equivalentInertia")){
360  putMessage("The \"equivalentInertia\" field of the Joint node is obsolete.");
361  }
362 }
363 
364 
366 {
367  requireField("centerOfMass", SFVEC3F);
368  requireField("mass", SFFLOAT);
369  requireField("momentsOfInertia", MFFLOAT);
370  addField("name", SFSTRING);
371 }
372 
373 
375 {
376  requireField("sensorId", SFINT32);
377  requireField("translation", SFVEC3F);
378  requireField("rotation", SFROTATION);
379 }
380 
381 
383 {
384  requireField("id", SFINT32);
385  requireField("translation", SFVEC3F);
386  requireField("rotation", SFROTATION);
387  requireField("url", SFSTRING);
388 }
389 
391 {
392  requireField("link1Name", SFSTRING);
393  requireField("link2Name", SFSTRING);
394  requireField("link1LocalPos", SFVEC3F);
395  requireField("link2LocalPos", SFVEC3F);
396  requireField("jointType", SFSTRING);
397  requireField("jointAxis", SFVEC3F);
398 }
399 
401 {
402  MFNode& nodes = humanoidNode->fields["humanoidBody"].mfNode();
403 
404  if(nodes.size() > 1){
406  ("The Humanoid node must have a unique Joint node in its \"humanoidBody\" field.");
407 
408  } else if(nodes.size() == 1){
409  if(nodes[0]->isCategoryOf(PROTO_INSTANCE_NODE)){
410  VrmlProtoInstancePtr jointNode = dynamic_pointer_cast<VrmlProtoInstance>(nodes[0]);
411  if(jointNode && jointNode->proto->protoName == "Joint"){
412  rootJointNodeSet = addJointNodeSet(jointNode);
413  }
414  }
415  }
416 
417  if(!rootJointNodeSet){
419  ("The Humanoid node does not have a Joint node in its \"humanoidBody\" field.");
420  }
421 }
422 
423 
425 {
426  numJointNodes++;
427 
428  putMessage(string("Joint node ") + jointNode->defName);
429 
430  JointNodeSetPtr jointNodeSet(new JointNodeSet());
431 
432  jointNodeSet->jointNode = jointNode;
433 
434  MFNode& childNodes = jointNode->fields["children"].mfNode();
435 
436  ProtoIdSet acceptableProtoIds;
437  acceptableProtoIds.set(PROTO_JOINT);
438  acceptableProtoIds.set(PROTO_SEGMENT);
439  acceptableProtoIds.set(PROTO_SENSOR);
440  acceptableProtoIds.set(PROTO_HARDWARECOMPONENT);
441 
442  Matrix44 T(Matrix44::Identity());
443  extractChildNodes(jointNodeSet, childNodes, acceptableProtoIds, T);
444 
445  return jointNodeSet;
446 }
447 
449 (JointNodeSetPtr jointNodeSet, MFNode& childNodes, const ProtoIdSet acceptableProtoIds, const Matrix44& T)
450 {
451  for(size_t i = 0; i < childNodes.size(); i++){
452  VrmlNode* childNode = childNodes[i].get();
453  const Matrix44* pT;
454  if(childNode->isCategoryOf(GROUPING_NODE)){
455  VrmlGroup* groupNode = static_cast<VrmlGroup*>(childNode);
456  VrmlTransform* transformNode = dynamic_cast<VrmlTransform*>(groupNode);
457  Matrix44 T2;
458  if( transformNode ){
459  Matrix44 Tlocal;
460  calcTransformMatrix(transformNode, Tlocal);
461  T2 = T * Tlocal;
462  pT = &T2;
463  } else {
464  pT = &T;
465  }
466  extractChildNodes(jointNodeSet, groupNode->getChildren(), acceptableProtoIds, *pT);
467 
468  } else if(childNode->isCategoryOf(LIGHT_NODE)){
469  jointNodeSet->lightNodes.push_back(std::make_pair(T,childNode));
470  } else if(childNode->isCategoryOf(PROTO_INSTANCE_NODE)){
471 
472  VrmlProtoInstance* protoInstance = static_cast<VrmlProtoInstance*>(childNode);
473  int id = PROTO_UNDEFINED;
474  bool doTraverseChildren = false;
475  ProtoIdSet acceptableChildProtoIds(acceptableProtoIds);
476 
477  const string& protoName = protoInstance->proto->protoName;
478  ProtoNameToInfoMap::iterator p = protoNameToInfoMap.find(protoName);
479 
480  if(p == protoNameToInfoMap.end()){
481  doTraverseChildren = true;
482  } else {
483  id = p->second.id;
484  if(!acceptableProtoIds.test(id)){
485  throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
486  }
487  }
488 
489  messageIndent += 2;
490 
491  switch(id){
492 
493  case PROTO_JOINT:
494  if(T != Matrix44::Identity())
495  throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
496  jointNodeSet->childJointNodeSets.push_back(addJointNodeSet(protoInstance));
497  break;
498 
499  case PROTO_SENSOR:
500  if(T != Matrix44::Identity())
501  throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
502  jointNodeSet->sensorNodes.push_back(protoInstance);
503  putMessage(protoName + protoInstance->defName);
504  break;
505 
507  if(T != Matrix44::Identity())
508  throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
509  jointNodeSet->hwcNodes.push_back(protoInstance);
510  putMessage(protoName + protoInstance->defName);
511  break;
512 
513  case PROTO_SEGMENT:
514  {
515  jointNodeSet->segmentNodes.push_back(protoInstance);
516  jointNodeSet->transforms.push_back(T);
517  putMessage(string("Segment node ") + protoInstance->defName);
518 
519  doTraverseChildren = true;
520  acceptableChildProtoIds.reset();
521  acceptableChildProtoIds.set(PROTO_SENSOR);
522  }
523  break;
524 
525  default:
526  break;
527  }
528 
529  if(doTraverseChildren){
530  if (protoInstance->fields.count("children")){
531  MFNode& childNodes = protoInstance->fields["children"].mfNode();
532  extractChildNodes(jointNodeSet, childNodes, acceptableChildProtoIds, T);
533  }
534  }
535 
536  messageIndent -= 2;
537  }
538  }
539 }
540 
541 
542 void ModelNodeSetImpl::putMessage(const std::string& message)
543 {
544  if(!self->sigMessage.empty()) {
545  string space(messageIndent, ' ');
546  self->sigMessage(space + message + "\n");
547  }
548 }
std::vector< SFNode > MFNode
Definition: VrmlNodes.h:158
VrmlProtoInstancePtr humanoidNode
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2332
Modifications controlling boost library behavior.
Definition: ColladaUtil.h:306
char * filename
Definition: cdjpeg.h:133
Abstract base class of all vrml nodes.
Definition: VrmlNodes.h:121
JointNodeSetPtr rootJointNodeSet
virtual MFNode & getChildren()
Definition: VrmlNodes.cpp:139
VrmlProtoPtr proto
Definition: VrmlNodes.h:886
VrmlProtoInstancePtr extraJointNode(int index)
static const char * getLabelOfFieldType(int type)
Definition: VrmlNodes.cpp:19
png_infop png_charpp name
Definition: png.h:2382
boost::intrusive_ptr< VrmlProtoInstance > VrmlProtoInstancePtr
Definition: VrmlNodes.h:898
void extractChildNodes(JointNodeSetPtr jointNodeSet, MFNode &childNodes, const ProtoIdSet acceptableProtoIds, const Matrix44 &T)
VRML Group node.
Definition: VrmlNodes.h:236
string message
HRP_UTIL_EXPORT void calcTransformMatrix(VrmlTransform *transform, Matrix44 &out_T)
Definition: Eigen4d.cpp:49
png_uint_32 i
Definition: png.h:2735
JointNodeSetPtr addJointNodeSet(VrmlProtoInstancePtr jointNode)
Eigen::Matrix4d Matrix44
Definition: Eigen4d.h:18
virtual ~ModelNodeSet()
void addFloatField(const std::string &name, double defaultValue)
VrmlNodePtr readNode()
Definition: VrmlParser.cpp:496
bool isCategoryOf(VrmlNodeCategory category)
Definition: VrmlNodes.cpp:60
std::vector< VrmlProtoInstancePtr > extraJointNodes
boost::intrusive_ptr< VrmlProto > VrmlProtoPtr
Definition: VrmlNodes.h:879
ModelNodeSet * self
typedef void(PNGAPI *png_error_ptr) PNGARG((png_structp
void load(const std::string &filename)
Definition: VrmlParser.cpp:463
self
std::bitset< NUM_PROTOS > ProtoIdSet
VRML Proto definition.
Definition: VrmlNodes.h:859
VRML node which is instance of VRML Prototype.
Definition: VrmlNodes.h:883
TProtoFieldMap fields
Definition: VrmlNodes.h:887
void checkFieldType(const std::string &name, VrmlFieldTypeId type)
boost::intrusive_ptr< VrmlNode > VrmlNodePtr
Definition: VrmlNodes.h:155
id
bool loadModelFile(const std::string &filename)
backing_store_ptr info
Definition: jmemsys.h:181
void extractHumanoidNode(VrmlParser &parser)
VrmlProtoPtr protoToCheck
boost::array< SFFloat, 3 > SFVec3f
Definition: VrmlNodes.h:55
Parser for VRML97 format.
Definition: VrmlParser.h:30
void putMessage(const std::string &message)
JointNodeSetPtr rootJointNodeSet()
VrmlProtoInstancePtr humanoidNode()
boost::shared_ptr< JointNodeSet > JointNodeSetPtr
Definition: ModelNodeSet.h:34
VRML Transform node.
Definition: VrmlNodes.h:254
VrmlFieldTypeId typeId()
Definition: VrmlNodes.h:828
ModelNodeSetImpl(ModelNodeSet *self)
void requireField(const std::string &name, VrmlFieldTypeId type)
std::string defName
Definition: VrmlNodes.h:130
boost::signal< void(const std::string &message)> sigMessage
Definition: ModelNodeSet.h:79
VrmlFieldTypeId
Definition: VrmlNodes.h:33
VrmlVariantField * addField(const std::string &name, VrmlFieldTypeId type)
The header file of a text scanner class.
void throwExceptionOfIllegalField(const std::string &name, VrmlFieldTypeId typeId)
bool loadModelFile(const std::string &filename)


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:24