BodyInfo_impl.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 
16 #include "BodyInfo_impl.h"
17 
18 #include <map>
19 #include <vector>
20 #include <iostream>
21 #include <boost/bind.hpp>
22 #include <boost/format.hpp>
23 
24 #include <hrpCorba/ViewSimulator.hh>
25 #include <hrpUtil/EasyScanner.h>
26 #include <hrpUtil/VrmlNodes.h>
27 #include <hrpUtil/VrmlParser.h>
28 #include <hrpUtil/ImageConverter.h>
29 
30 #include "VrmlUtil.h"
31 
32 
33 
34 using namespace std;
35 using namespace boost;
36 
37 namespace {
38  typedef map<string, string> SensorTypeMap;
39  SensorTypeMap sensorTypeMap;
40 }
41 
42 
43 BodyInfo_impl::BodyInfo_impl(PortableServer::POA_ptr poa) :
45 {
46  lastUpdate_ = 0;
47 }
48 
49 
51 {
52 
53 }
54 
55 
56 const std::string& BodyInfo_impl::topUrl()
57 {
58  return url_;
59 }
60 
61 
63 {
64  return CORBA::string_dup(name_.c_str());
65 }
66 
67 
69 {
70  return CORBA::string_dup(url_.c_str());
71 }
72 
73 
74 StringSequence* BodyInfo_impl::info()
75 {
76  return new StringSequence(info_);
77 }
78 
79 
80 LinkInfoSequence* BodyInfo_impl::links()
81 {
82  return new LinkInfoSequence(links_);
83 }
84 
85 
86 AllLinkShapeIndexSequence* BodyInfo_impl::linkShapeIndices()
87 {
88  return new AllLinkShapeIndexSequence(linkShapeIndices_);
89 }
90 
91 ExtraJointInfoSequence* BodyInfo_impl::extraJoints()
92 {
93  return new ExtraJointInfoSequence(extraJoints_);
94 }
95 
104 void BodyInfo_impl::loadModelFile(const std::string& url)
105 {
106  string filename( deleteURLScheme( url ) );
107 
108  // URL文字列の' \' 区切り子を'/' に置き換え Windows ファイルパス対応
109  string url2;
110  url2 = filename;
111  replace( url2, string("\\"), string("/") );
112  filename = url2;
113  url_ = CORBA::string_dup(url2.c_str());
114 
115 
116  ModelNodeSet modelNodeSet;
117  modelNodeSet.sigMessage.connect(boost::bind(&putMessage, _1));
118 
119  bool result = false;
120 
121  try {
122  result = modelNodeSet.loadModelFile( filename );
123 
124  if(result){
125  applyTriangleMeshShaper(modelNodeSet.humanoidNode());
126  }
127  cout.flush();
128  }
129  catch(const ModelNodeSet::Exception& ex) {
130  cout << ex.what() << endl;
131  cout << "Retrying to load the file as a standard VRML file" << endl;
132  try {
133  VrmlParser parser;
134  parser.load(filename);
135 
136  links_.length(1);
137  LinkInfo &linfo = links_[0];
138  linfo.name = CORBA::string_dup("root");
139  linfo.parentIndex = -1;
140  linfo.jointId = -1;
141  linfo.jointType = CORBA::string_dup("fixed");
142  linfo.jointValue = 0;
143  for (int i=0; i<3; i++){
144  linfo.jointAxis[i] = 0;
145  linfo.translation[i] = 0;
146  linfo.rotation[i] = 0;
147  linfo.centerOfMass[i] = 0;
148  }
149  linfo.jointAxis[2] = 1;
150  linfo.rotation[2] = 1; linfo.rotation[3] = 0;
151  linfo.mass = 0;
152  for (int i=0; i<9; i++) linfo.inertia[i] = 0;
153 
154 
155  Matrix44 E(Matrix44::Identity());
156 
157  while(VrmlNodePtr node = parser.readNode()){
158  if(!node->isCategoryOf(PROTO_DEF_NODE)){
160  traverseShapeNodes(node.get(), E, linfo.shapeIndices, linfo.inlinedShapeTransformMatrices, &topUrl());
161  }
162  }
163  return;
164  } catch(EasyScanner::Exception& ex){
165  cout << ex.getFullMessage() << endl;
166  throw ModelLoader::ModelLoaderException(ex.getFullMessage().c_str());
167  }
168  }
169 
170  if(!result){
171  throw ModelLoader::ModelLoaderException("The model file cannot be loaded.");
172  }
173 
174  const string& humanoidName = modelNodeSet.humanoidNode()->defName;
175  name_ = humanoidName.c_str();
176  const MFString& info = modelNodeSet.humanoidNode()->fields["info"].mfString();
177  info_.length(info.size());
178  for (unsigned int i=0; i<info_.length(); i++){
179  info_[i] = CORBA::string_dup(info[i].c_str());
180  }
181 
182  int numJointNodes = modelNodeSet.numJointNodes();
183 
184  links_.length(numJointNodes);
185  if( 0 < numJointNodes ) {
186  int currentIndex = 0;
187  JointNodeSetPtr rootJointNodeSet = modelNodeSet.rootJointNodeSet();
188  readJointNodeSet(rootJointNodeSet, currentIndex, -1);
189  }
190 
191  linkShapeIndices_.length(numJointNodes);
192  for(int i = 0 ; i < numJointNodes ; ++i) {
193  linkShapeIndices_[i] = links_[i].shapeIndices;
194  }
195 
196  // build coldetModels
197  linkColdetModels.resize(numJointNodes);
198  for(int linkIndex = 0; linkIndex < numJointNodes ; ++linkIndex){
199  ColdetModelPtr coldetModel(new ColdetModel());
200  coldetModel->setName(std::string(links_[linkIndex].name));
201  int vertexIndex = 0;
202  int triangleIndex = 0;
203 
204  Matrix44 E(Matrix44::Identity());
205  const TransformedShapeIndexSequence& shapeIndices = linkShapeIndices_[linkIndex];
206  setColdetModel(coldetModel, shapeIndices, E, vertexIndex, triangleIndex);
207 
208  Matrix44 T(Matrix44::Identity());
209  const SensorInfoSequence& sensors = links_[linkIndex].sensors;
210  for (unsigned int i=0; i<sensors.length(); i++){
211  const SensorInfo& sensor = sensors[i];
212  calcRodrigues(T, Vector3(sensor.rotation[0], sensor.rotation[1],
213  sensor.rotation[2]), sensor.rotation[3]);
214  T(0,3) = sensor.translation[0];
215  T(1,3) = sensor.translation[1];
216  T(2,3) = sensor.translation[2];
217  const TransformedShapeIndexSequence& sensorShapeIndices = sensor.shapeIndices;
218  setColdetModel(coldetModel, sensorShapeIndices, T, vertexIndex, triangleIndex);
219  }
220 
221  if(triangleIndex>0)
222  coldetModel->build();
223 
224  linkColdetModels[linkIndex] = coldetModel;
225  links_[linkIndex].AABBmaxDepth = coldetModel->getAABBTreeDepth();
226  links_[linkIndex].AABBmaxNum = coldetModel->getAABBmaxNum();
227  }
228  //saveOriginalData();
229  //originlinkShapeIndices_ = linkShapeIndices_;
230 
231  int n = modelNodeSet.numExtraJointNodes();
232  extraJoints_.length(n);
233  for(int i=0; i < n; ++i){
234 
235  TProtoFieldMap& f = modelNodeSet.extraJointNode(i)->fields;
236  ExtraJointInfo_var extraJointInfo(new ExtraJointInfo());
237  extraJointInfo->name = CORBA::string_dup( modelNodeSet.extraJointNode(i)->defName.c_str() );
238 
239  string link1Name, link2Name;
240  copyVrmlField( f, "link1Name", link1Name );
241  copyVrmlField( f, "link2Name", link2Name );
242  extraJointInfo->link[0] = CORBA::string_dup(link1Name.c_str());
243  extraJointInfo->link[1] = CORBA::string_dup(link2Name.c_str());
244 
245  string jointType;
246  copyVrmlField( f, "jointType", jointType);
247  if(jointType == "xy"){
248  extraJointInfo->jointType = EJ_XY;
249  } else if(jointType == "xyz"){
250  extraJointInfo->jointType = EJ_XYZ;
251  } else if(jointType == "z"){
252  extraJointInfo->jointType = EJ_Z;
253  }else {
254  throw ModelNodeSet::Exception(str(format("JointType \"%1%\" is not supported.") % jointType));
255  }
256  copyVrmlField( f, "jointAxis", extraJointInfo->axis );
257  copyVrmlField( f, "link1LocalPos", extraJointInfo->point[0] );
258  copyVrmlField( f, "link2LocalPos", extraJointInfo->point[1] );
259 
260  extraJoints_[i] = extraJointInfo;
261  }
262 }
263 
264 
265 int BodyInfo_impl::readJointNodeSet(JointNodeSetPtr jointNodeSet, int& currentIndex, int parentIndex)
266 {
267  int index = currentIndex;
268  currentIndex++;
269 
270  LinkInfo_var linkInfo(new LinkInfo());
271  linkInfo->parentIndex = parentIndex;
272 
273  size_t numChildren = jointNodeSet->childJointNodeSets.size();
274 
275  for( size_t i = 0 ; i < numChildren ; ++i ){
276  JointNodeSetPtr childJointNodeSet = jointNodeSet->childJointNodeSets[i];
277  int childIndex = readJointNodeSet(childJointNodeSet, currentIndex, index);
278 
279  long childIndicesLength = linkInfo->childIndices.length();
280  linkInfo->childIndices.length( childIndicesLength + 1 );
281  linkInfo->childIndices[childIndicesLength] = childIndex;
282  }
283 
284  links_[index] = linkInfo;
285  try {
286  vector<VrmlProtoInstancePtr>& segmentNodes = jointNodeSet->segmentNodes;
287  int numSegment = segmentNodes.size();
288  links_[index].segments.length(numSegment);
289  for(int i = 0 ; i < numSegment ; ++i){
290  SegmentInfo_var segmentInfo(new SegmentInfo());
291  Matrix44 T = jointNodeSet->transforms.at(i);
292  long s = links_[index].shapeIndices.length();
293  int p = 0;
294  for(int row=0; row < 3; ++row){
295  for(int col=0; col < 4; ++col){
296  segmentInfo->transformMatrix[p++] = T(row, col);
297  }
298  }
299  traverseShapeNodes(segmentNodes[i].get(), T, links_[index].shapeIndices, links_[index].inlinedShapeTransformMatrices, &topUrl());
300  long e =links_[index].shapeIndices.length();
301  segmentInfo->shapeIndices.length(e-s);
302  for(int j=0, k=s; k<e; k++)
303  segmentInfo->shapeIndices[j++] = k;
304  links_[index].segments[i] = segmentInfo;
305  }
306  setJointParameters(index, jointNodeSet->jointNode);
307  setSegmentParameters(index, jointNodeSet);
308  setSensors(index, jointNodeSet);
309  setHwcs(index, jointNodeSet);
310  setLights(index, jointNodeSet);
311  }
312  catch( ModelLoader::ModelLoaderException& ex ) {
313  string name(linkInfo->name);
314  string error = name.empty() ? "Unnamed JointNode" : name;
315  error += ": ";
316  error += ex.description;
317  throw ModelLoader::ModelLoaderException( error.c_str() );
318  }
319 
320  return index;
321 }
322 
323 
324 void BodyInfo_impl::setJointParameters(int linkInfoIndex, VrmlProtoInstancePtr jointNode)
325 {
326  LinkInfo& linkInfo = links_[linkInfoIndex];
327 
328  linkInfo.name = CORBA::string_dup( jointNode->defName.c_str() );
329 
330  TProtoFieldMap& fmap = jointNode->fields;
331 
332  CORBA::Long jointId;
333  copyVrmlField( fmap, "jointId", jointId );
334  linkInfo.jointId = (CORBA::Short)jointId;
335 
336  linkInfo.jointAxis[0] = 0.0;
337  linkInfo.jointAxis[1] = 0.0;
338  linkInfo.jointAxis[2] = 0.0;
339 
340  VrmlVariantField& fJointAxis = fmap["jointAxis"];
341 
342  switch( fJointAxis.typeId() ) {
343 
344  case SFSTRING:
345  {
346  SFString& axisLabel = fJointAxis.sfString();
347  if( axisLabel == "X" ) { linkInfo.jointAxis[0] = 1.0; }
348  else if( axisLabel == "Y" ) { linkInfo.jointAxis[1] = 1.0; }
349  else if( axisLabel == "Z" ) { linkInfo.jointAxis[2] = 1.0; }
350  }
351  break;
352 
353  case SFVEC3F:
354  copyVrmlField( fmap, "jointAxis", linkInfo.jointAxis );
355  break;
356 
357  default:
358  break;
359  }
360 
361  std::string jointType;
362  copyVrmlField( fmap, "jointType", jointType );
363  linkInfo.jointType = CORBA::string_dup( jointType.c_str() );
364 
365  copyVrmlField( fmap, "translation", linkInfo.translation );
366  copyVrmlRotationFieldToDblArray4( fmap, "rotation", linkInfo.rotation );
367 
368  copyVrmlField( fmap, "ulimit", linkInfo.ulimit );
369  copyVrmlField( fmap, "llimit", linkInfo.llimit );
370  copyVrmlField( fmap, "uvlimit", linkInfo.uvlimit );
371  copyVrmlField( fmap, "lvlimit", linkInfo.lvlimit );
372 
373  if(fmap["climit"].typeId() != UNDETERMINED_FIELD_TYPE){
374  copyVrmlField( fmap, "climit", linkInfo.climit );
375  }else{
376  //std::cout << "No climit type. climit was ignored." << std::endl;
377  linkInfo.climit.length((CORBA::ULong)0); // dummy
378  }
379 
380  copyVrmlField( fmap, "gearRatio", linkInfo.gearRatio );
381  copyVrmlField( fmap, "rotorInertia", linkInfo.rotorInertia );
382  copyVrmlField( fmap, "rotorResistor", linkInfo.rotorResistor );
383  copyVrmlField( fmap, "torqueConst", linkInfo.torqueConst );
384  copyVrmlField( fmap, "encoderPulse", linkInfo.encoderPulse );
385  copyVrmlField( fmap, "jointValue", linkInfo.jointValue );
386 }
387 
388 void BodyInfo_impl::setSegmentParameters(int linkInfoIndex, JointNodeSetPtr jointNodeSet)
389 {
390  LinkInfo& linkInfo = links_[linkInfoIndex];
391 
392  vector<VrmlProtoInstancePtr>& segmentNodes = jointNodeSet->segmentNodes;
393  int numSegment = segmentNodes.size();
394 
395  linkInfo.mass = 0.0;
396  for( int i = 0 ; i < 3 ; ++i ) {
397  linkInfo.centerOfMass[i] = 0.0;
398  for( int j = 0 ; j < 3 ; ++j ) {
399  linkInfo.inertia[i*3 + j] = 0.0;
400  }
401  }
402 
403  // Mass = Σmass //
404  // C = (Σmass * T * c) / Mass //
405  // I = Σ(R * I * Rt + G) //
406  // R = Tの回転行列 //
407  // G = y*y+z*z, -x*y, -x*z, -y*x, z*z+x*x, -y*z, -z*x, -z*y, x*x+y*y //
408  // (x, y, z ) = T * c - C //
409  std::vector<Vector4, Eigen::aligned_allocator<Vector4> > centerOfMassArray;
410  std::vector<double> massArray;
411  for(int i = 0 ; i < numSegment ; ++i){
412  SegmentInfo& segmentInfo = linkInfo.segments[i];
413  Matrix44 T = jointNodeSet->transforms.at(i);
414  DblArray3& centerOfMass = segmentInfo.centerOfMass;
415  CORBA::Double& mass =segmentInfo.mass;
416  DblArray9& inertia = segmentInfo.inertia;
417  TProtoFieldMap& fmap = segmentNodes[i]->fields;
418  copyVrmlField( fmap, "centerOfMass", centerOfMass );
419  copyVrmlField( fmap, "mass", mass );
420  copyVrmlField( fmap, "momentsOfInertia", inertia );
421  Vector4 c0(centerOfMass[0], centerOfMass[1], centerOfMass[2], 1.0);
422  Vector4 c1(T * c0);
423  centerOfMassArray.push_back(c1);
424  massArray.push_back(mass);
425  for(int j=0; j<3; j++){
426  linkInfo.centerOfMass[j] = c1(j) * mass + linkInfo.centerOfMass[j] * linkInfo.mass;
427  }
428  linkInfo.mass += mass;
429  if(linkInfo.mass > 0.0){
430  for(int j=0; j<3; j++){
431  linkInfo.centerOfMass[j] /= linkInfo.mass;
432  }
433  }
434  Matrix33 I;
435  I << inertia[0], inertia[1], inertia[2], inertia[3], inertia[4], inertia[5], inertia[6], inertia[7], inertia[8];
436  Matrix33 R;
437  R << T(0,0), T(0,1), T(0,2), T(1,0), T(1,1), T(1,2), T(2,0), T(2,1), T(2,2);
438  Matrix33 I1(R * I * R.transpose());
439  for(int j=0; j<3; j++){
440  for(int k=0; k<3; k++)
441  linkInfo.inertia[j*3+k] += I1(j,k);
442  }
443  segmentInfo.name = CORBA::string_dup( segmentNodes[i]->defName.c_str() );
444  }
445  if(linkInfo.mass <=0.0 )
446  std::cerr << "Warning: Mass is zero. <Model>" << name_ << " <Link>" << linkInfo.name << std::endl;
447 
448  for(int i = 0 ; i < numSegment ; ++i){
449  Vector4 c( centerOfMassArray.at(i) );
450  double x = c(0) - linkInfo.centerOfMass[0];
451  double y = c(1) - linkInfo.centerOfMass[1];
452  double z = c(2) - linkInfo.centerOfMass[2];
453  double m = massArray.at(i);
454 
455  linkInfo.inertia[0] += m * (y*y + z*z);
456  linkInfo.inertia[1] += -m * x * y;
457  linkInfo.inertia[2] += -m * x * z;
458  linkInfo.inertia[3] += -m * y * x;
459  linkInfo.inertia[4] += m * (z*z + x*x);
460  linkInfo.inertia[5] += -m * y * z;
461  linkInfo.inertia[6] += -m * z * x;
462  linkInfo.inertia[7] += -m * z * y;
463  linkInfo.inertia[8] += m * (x*x + y*y);
464  }
465 }
466 
467 
468 void BodyInfo_impl::setSensors(int linkInfoIndex, JointNodeSetPtr jointNodeSet)
469 {
470  LinkInfo& linkInfo = links_[linkInfoIndex];
471 
472  vector<VrmlProtoInstancePtr>& sensorNodes = jointNodeSet->sensorNodes;
473 
474  int numSensors = sensorNodes.size();
475  linkInfo.sensors.length(numSensors);
476 
477  for(int i = 0 ; i < numSensors ; ++i) {
478  SensorInfo_var sensorInfo( new SensorInfo() );
479  readSensorNode( linkInfoIndex, sensorInfo, sensorNodes[i] );
480  linkInfo.sensors[i] = sensorInfo;
481  }
482 }
483 
484 
485 void BodyInfo_impl::setHwcs(int linkInfoIndex, JointNodeSetPtr jointNodeSet)
486 {
487  LinkInfo& linkInfo = links_[linkInfoIndex];
488 
489  vector<VrmlProtoInstancePtr>& hwcNodes = jointNodeSet->hwcNodes;
490 
491  int numHwcs = hwcNodes.size();
492  linkInfo.hwcs.length(numHwcs);
493 
494  for(int i = 0 ; i < numHwcs ; ++i) {
495  HwcInfo_var hwcInfo( new HwcInfo() );
496  readHwcNode( linkInfoIndex, hwcInfo, hwcNodes[i] );
497  linkInfo.hwcs[i] = hwcInfo;
498  }
499 }
500 
501 void BodyInfo_impl::setLights(int linkInfoIndex, JointNodeSetPtr jointNodeSet)
502 {
503  LinkInfo& linkInfo = links_[linkInfoIndex];
504 
505  vector<pair<Matrix44, VrmlNodePtr>,
506  Eigen::aligned_allocator<pair<Matrix44, VrmlNodePtr> > >& lightNodes = jointNodeSet->lightNodes;
507 
508  int numLights = lightNodes.size();
509  linkInfo.lights.length(numLights);
510 
511  for(int i = 0 ; i < numLights ; ++i) {
512  LightInfo_var lightInfo( new LightInfo() );
513  readLightNode( linkInfoIndex, lightInfo, lightNodes[i] );
514  linkInfo.lights[i] = lightInfo;
515  }
516 }
517 
518 
519 void BodyInfo_impl::readSensorNode(int linkInfoIndex, SensorInfo& sensorInfo, VrmlProtoInstancePtr sensorNode)
520 {
521  if(sensorTypeMap.empty()) {
522  sensorTypeMap["ForceSensor"] = "Force";
523  sensorTypeMap["Gyro"] = "RateGyro";
524  sensorTypeMap["AccelerationSensor"] = "Acceleration";
525  sensorTypeMap["PressureSensor"] = "";
526  sensorTypeMap["PhotoInterrupter"] = "";
527  sensorTypeMap["VisionSensor"] = "Vision";
528  sensorTypeMap["TorqueSensor"] = "";
529  sensorTypeMap["RangeSensor"] = "Range";
530  }
531 
532  try {
533  sensorInfo.name = CORBA::string_dup( sensorNode->defName.c_str() );
534 
535  TProtoFieldMap& fmap = sensorNode->fields;
536 
537  copyVrmlField(fmap, "sensorId", sensorInfo.id );
538  copyVrmlField(fmap, "translation", sensorInfo.translation );
539  copyVrmlRotationFieldToDblArray4( fmap, "rotation", sensorInfo.rotation );
540 
541  SensorTypeMap::iterator p = sensorTypeMap.find( sensorNode->proto->protoName );
542  std::string sensorType;
543  if(p != sensorTypeMap.end()){
544  sensorType = p->second;
545  sensorInfo.type = CORBA::string_dup( sensorType.c_str() );
546  } else {
547  throw ModelLoader::ModelLoaderException("Unknown Sensor Node");
548  }
549 
550  if(sensorType == "Force") {
551  sensorInfo.specValues.length( CORBA::ULong(6) );
552  DblArray3 maxForce, maxTorque;
553  copyVrmlField(fmap, "maxForce", maxForce );
554  copyVrmlField(fmap, "maxTorque", maxTorque );
555  sensorInfo.specValues[0] = maxForce[0];
556  sensorInfo.specValues[1] = maxForce[1];
557  sensorInfo.specValues[2] = maxForce[2];
558  sensorInfo.specValues[3] = maxTorque[0];
559  sensorInfo.specValues[4] = maxTorque[1];
560  sensorInfo.specValues[5] = maxTorque[2];
561 
562  } else if(sensorType == "RateGyro") {
563  sensorInfo.specValues.length( CORBA::ULong(3) );
564  DblArray3 maxAngularVelocity;
565  copyVrmlField(fmap, "maxAngularVelocity", maxAngularVelocity);
566  sensorInfo.specValues[0] = maxAngularVelocity[0];
567  sensorInfo.specValues[1] = maxAngularVelocity[1];
568  sensorInfo.specValues[2] = maxAngularVelocity[2];
569 
570  } else if( sensorType == "Acceleration" ){
571  sensorInfo.specValues.length( CORBA::ULong(3) );
572  DblArray3 maxAcceleration;
573  copyVrmlField(fmap, "maxAcceleration", maxAcceleration);
574  sensorInfo.specValues[0] = maxAcceleration[0];
575  sensorInfo.specValues[1] = maxAcceleration[1];
576  sensorInfo.specValues[2] = maxAcceleration[2];
577 
578  } else if( sensorType == "Vision" ){
579  sensorInfo.specValues.length( CORBA::ULong(7) );
580 
581  CORBA::Double specValues[3];
582  copyVrmlField(fmap, "frontClipDistance", specValues[0] );
583  copyVrmlField(fmap, "backClipDistance", specValues[1] );
584  copyVrmlField(fmap, "fieldOfView", specValues[2] );
585  sensorInfo.specValues[0] = specValues[0];
586  sensorInfo.specValues[1] = specValues[1];
587  sensorInfo.specValues[2] = specValues[2];
588 
589  std::string sensorTypeString;
590  copyVrmlField(fmap, "type", sensorTypeString );
591 
592  if(sensorTypeString=="NONE" ) {
593  sensorInfo.specValues[3] = Camera::NONE;
594  } else if(sensorTypeString=="COLOR") {
595  sensorInfo.specValues[3] = Camera::COLOR;
596  } else if(sensorTypeString=="MONO") {
597  sensorInfo.specValues[3] = Camera::MONO;
598  } else if(sensorTypeString=="DEPTH") {
599  sensorInfo.specValues[3] = Camera::DEPTH;
600  } else if(sensorTypeString=="COLOR_DEPTH") {
601  sensorInfo.specValues[3] = Camera::COLOR_DEPTH;
602  } else if(sensorTypeString=="MONO_DEPTH") {
603  sensorInfo.specValues[3] = Camera::MONO_DEPTH;
604  } else {
605  throw ModelLoader::ModelLoaderException("Sensor node has unkown type string");
606  }
607 
608  CORBA::Long width, height;
609  copyVrmlField(fmap, "width", width);
610  copyVrmlField(fmap, "height", height);
611 
612  sensorInfo.specValues[4] = static_cast<CORBA::Double>(width);
613  sensorInfo.specValues[5] = static_cast<CORBA::Double>(height);
614 
615  double frameRate;
616  copyVrmlField(fmap, "frameRate", frameRate);
617  sensorInfo.specValues[6] = frameRate;
618  } else if( sensorType == "Range" ){
619  sensorInfo.specValues.length( CORBA::ULong(4) );
620  CORBA::Double v;
621  copyVrmlField(fmap, "scanAngle", v);
622  sensorInfo.specValues[0] = v;
623  copyVrmlField(fmap, "scanStep", v);
624  sensorInfo.specValues[1] = v;
625  copyVrmlField(fmap, "scanRate", v);
626  sensorInfo.specValues[2] = v;
627  copyVrmlField(fmap, "maxDistance", v);
628  sensorInfo.specValues[3] = v;
629  }
630 
631  /*
632  Matrix44 T;
633  const DblArray4& r = sensorInfo.rotation;
634  calcRodrigues(T, Vector3(r[0], r[1], r[2]), r[3]);
635  for(int i=0; i < 3; ++i){
636  T(i,3) = sensorInfo.translation[i];
637  }
638  */
639 
640  Matrix44 E(Matrix44::Identity());
641  VrmlVariantField *field = sensorNode->getField("children");
642  if (field){
643  MFNode &children = field->mfNode();
644  for (unsigned int i=0; i<children.size(); i++){
645  traverseShapeNodes(children[i].get(), E,
646  sensorInfo.shapeIndices, sensorInfo.inlinedShapeTransformMatrices, &topUrl());
647  }
648  }
649  } catch(ModelLoader::ModelLoaderException& ex) {
650  string error = name_.empty() ? "Unnamed sensor node" : name_;
651  error += ": ";
652  error += ex.description;
653  throw ModelLoader::ModelLoaderException( error.c_str() );
654  }
655 }
656 
657 void BodyInfo_impl::readHwcNode(int linkInfoIndex, HwcInfo& hwcInfo, VrmlProtoInstancePtr hwcNode )
658 {
659  try {
660  hwcInfo.name = CORBA::string_dup( hwcNode->defName.c_str() );
661 
662  TProtoFieldMap& fmap = hwcNode->fields;
663 
664  copyVrmlField(fmap, "id", hwcInfo.id );
665  copyVrmlField(fmap, "translation", hwcInfo.translation );
666  copyVrmlRotationFieldToDblArray4( fmap, "rotation", hwcInfo.rotation );
667  std::string url;
668  copyVrmlField( fmap, "url", url );
669  hwcInfo.url = CORBA::string_dup( url.c_str() );
670 
671  Matrix44 E(Matrix44::Identity());
672  VrmlVariantField *field = hwcNode->getField("children");
673  if (field){
674  MFNode &children = field->mfNode();
675  for (unsigned int i=0; i<children.size(); i++){
676  traverseShapeNodes(children[i].get(), E,
677  hwcInfo.shapeIndices, hwcInfo.inlinedShapeTransformMatrices, &topUrl());
678  }
679  }
680  } catch(ModelLoader::ModelLoaderException& ex) {
681  throw ModelLoader::ModelLoaderException( ex.description );
682  }
683 }
684 
685 void BodyInfo_impl::readLightNode(int linkInfoIndex, LightInfo& lightInfo,
686  std::pair<Matrix44, VrmlNodePtr> &transformedLight )
687 {
688  VrmlNode *lightNode = transformedLight.second.get();
689  Matrix44 &T = transformedLight.first;
690  for (int i=0; i<3; i++){
691  for (int j=0; j<4; j++){
692  lightInfo.transformMatrix[i*4+j] = T(i,j);
693  }
694  }
695  try {
696  lightInfo.name = CORBA::string_dup( lightNode->defName.c_str() );
697  VrmlPointLight *plight = dynamic_cast<VrmlPointLight *>(lightNode);
698  VrmlDirectionalLight *dlight = dynamic_cast<VrmlDirectionalLight *>(lightNode);
699  VrmlSpotLight *slight = dynamic_cast<VrmlSpotLight *>(lightNode);
700  if (plight){
701  lightInfo.type = OpenHRP::POINT;
702  lightInfo.ambientIntensity = plight->ambientIntensity;
703  lightInfo.intensity = plight->intensity;
704  lightInfo.on = plight->on;
705  lightInfo.radius = plight->radius;
706  for (int i=0; i<3; i++){
707  lightInfo.attenuation[i] = plight->attenuation[i];
708  lightInfo.color[i] = plight->color[i];
709  lightInfo.location[i] = plight->location[i];
710  }
711  }else if(dlight){
712  lightInfo.type = OpenHRP::DIRECTIONAL;
713  lightInfo.ambientIntensity = dlight->ambientIntensity;
714  lightInfo.intensity = dlight->intensity;
715  lightInfo.on = dlight->on;
716  for (int i=0; i<3; i++){
717  lightInfo.color[i] = dlight->color[i];
718  lightInfo.direction[i] = dlight->direction[i];
719  }
720  }else if(slight){
721  lightInfo.type = OpenHRP::SPOT;
722  lightInfo.ambientIntensity = slight->ambientIntensity;
723  lightInfo.intensity = slight->intensity;
724  lightInfo.on = slight->on;
725  lightInfo.radius = slight->radius;
726  lightInfo.beamWidth = slight->beamWidth;
727  lightInfo.cutOffAngle = slight->cutOffAngle;
728  for (int i=0; i<3; i++){
729  lightInfo.attenuation[i] = slight->attenuation[i];
730  lightInfo.color[i] = slight->color[i];
731  lightInfo.location[i] = slight->location[i];
732  lightInfo.direction[i] = slight->direction[i];
733  }
734  }else{
735  throw ModelLoader::ModelLoaderException("unknown light type");
736  }
737  } catch(ModelLoader::ModelLoaderException& ex) {
738  throw ModelLoader::ModelLoaderException( ex.description );
739  }
740 }
741 
742 void BodyInfo_impl::setParam(std::string param, bool value){
743  if(param == "readImage")
744  readImage = value;
745  else
746  ;
747 }
748 
749 bool BodyInfo_impl::getParam(std::string param){
750  // FIXME: should this be an assert?
751  // pros for assert: can be turned off on release builds
752  // cons for assert: can lead to unpredictable behavior in
753  // release builds if the assertion is violated
754  if(param == "readImage")
755  return readImage;
756  else
757  abort ();
758 }
759 
760 void BodyInfo_impl::setParam(std::string param, int value){
761  if(param == "AABBType")
762  AABBdataType_ = (OpenHRP::ModelLoader::AABBdataType)value;
763  else
764  ;
765 }
766 
767 void BodyInfo_impl::changetoBoundingBox(unsigned int* inputData){
768  const double EPS = 1.0e-6;
770  std::vector<Vector3> boxSizeMap;
771  std::vector<Vector3> boundingBoxData;
772 
773  for(unsigned int i=0; i<links_.length(); i++){
774  int _depth;
775  if( AABBdataType_ == OpenHRP::ModelLoader::AABB_NUM )
776  _depth = linkColdetModels[i]->numofBBtoDepth(inputData[i]);
777  else
778  _depth = inputData[i];
779  if( _depth >= links_[i].AABBmaxDepth)
780  _depth = links_[i].AABBmaxDepth-1;
781  if(_depth >= 0 ){
782  linkColdetModels[i]->getBoundingBoxData(_depth, boundingBoxData);
783  std::vector<TransformedShapeIndex> tsiMap;
784  links_[i].shapeIndices.length(0);
785  SensorInfoSequence& sensors = links_[i].sensors;
786  for (unsigned int j=0; j<sensors.length(); j++){
787  SensorInfo& sensor = sensors[j];
788  sensor.shapeIndices.length(0);
789  }
790 
791  for(unsigned int j=0; j<boundingBoxData.size()/2; j++){
792 
793  bool flg=false;
794  unsigned int k=0;
795  for( ; k<boxSizeMap.size(); k++)
796  if((boxSizeMap[k] - boundingBoxData[j*2+1]).norm() < EPS)
797  break;
798  if( k<boxSizeMap.size() )
799  flg=true;
800  else{
801  boxSizeMap.push_back(boundingBoxData[j*2+1]);
802  setBoundingBoxData(boundingBoxData[j*2+1],k);
803  }
804 
805  if(flg){
806  unsigned int l=0;
807  for( ; l<tsiMap.size(); l++){
808  Vector3 p(tsiMap[l].transformMatrix[3],tsiMap[l].transformMatrix[7],tsiMap[l].transformMatrix[11]);
809  if((p - boundingBoxData[j*2]).norm() < EPS && tsiMap[l].shapeIndex == (int)k)
810  break;
811  }
812  if( l==tsiMap.size() )
813  flg=false;
814  }
815 
816  if(!flg){
817  int num = links_[i].shapeIndices.length();
818  links_[i].shapeIndices.length(num+1);
819  TransformedShapeIndex& tsi = links_[i].shapeIndices[num];
820  tsi.inlinedShapeTransformMatrixIndex = -1;
821  tsi.shapeIndex = k;
822  Matrix44 T(Matrix44::Identity());
823  for(int p = 0,row=0; row < 3; ++row)
824  for(int col=0; col < 4; ++col)
825  if(col==3){
826  switch(row){
827  case 0:
828  tsi.transformMatrix[p++] = boundingBoxData[j*2][0];
829  break;
830  case 1:
831  tsi.transformMatrix[p++] = boundingBoxData[j*2][1];
832  break;
833  case 2:
834  tsi.transformMatrix[p++] = boundingBoxData[j*2][2];
835  break;
836  default:
837  ;
838  }
839  }else
840  tsi.transformMatrix[p++] = T(row, col);
841 
842  tsiMap.push_back(tsi);
843  }
844  }
845  }
846  linkShapeIndices_[i] = links_[i].shapeIndices;
847  }
848 }
849 
852  for(size_t i = 0 ; i < links_.length() ; ++i) {
853  links_[i].shapeIndices = linkShapeIndices_[i];
854  }
856 }
std::vector< SFNode > MFNode
Definition: VrmlNodes.h:158
int c
Definition: autoplay.py:16
SFVec3f attenuation
Definition: VrmlNodes.h:787
void setParam(std::string param, bool value)
bool getParam(std::string param)
AllLinkShapeIndexSequence linkShapeIndices_
Definition: BodyInfo_impl.h:71
std::string & replace(std::string &str, const std::string &sb, const std::string &sa)
void setBoundingBoxData(const Vector3 &boxSize, int shapeIndex)
BodyInfo_impl(PortableServer::POA_ptr poa)
Modifications controlling boost library behavior.
Definition: ColladaUtil.h:306
char * filename
Definition: cdjpeg.h:133
SFFloat cutOffAngle
Definition: VrmlNodes.h:789
* x
Definition: IceUtils.h:98
Abstract base class of all vrml nodes.
Definition: VrmlNodes.h:121
OpenHRP::ModelLoader::AABBdataType AABBdataType_
Definition: BodyInfo_impl.h:65
void changetoOriginData()
virtual ~BodyInfo_impl()
virtual LinkInfoSequence * links()
std::map< std::string, VrmlVariantField > TProtoFieldMap
Definition: VrmlNodes.h:854
png_voidp int value
Definition: png.h:2113
boost::intrusive_ptr< VrmlProtoInstance > VrmlProtoInstancePtr
Definition: VrmlNodes.h:898
void readLightNode(int linkInfoIndex, LightInfo &LightInfo, std::pair< Matrix44, VrmlNodePtr > &transformedLight)
void error(char *msg) const
Definition: minigzip.c:87
#define EPS
void readSensorNode(int linkInfoIndex, SensorInfo &sensorInfo, VrmlProtoInstancePtr sensorNode)
png_uint_32 i
Definition: png.h:2735
SFFloat ambientIntensity
Definition: VrmlNodes.h:786
Eigen::Matrix4d Matrix44
Definition: Eigen4d.h:18
SFString & sfString()
Definition: VrmlNodes.h:844
virtual const std::string & topUrl()
void applyTriangleMeshShaper(VrmlNodePtr node)
AllLinkShapeIndexSequence originlinkShapeIndices_
Definition: BodyInfo_impl.h:72
void changetoBoundingBox(unsigned int *depth)
png_infop png_uint_32 * width
Definition: png.h:2309
virtual char * url()
VrmlNodePtr readNode()
Definition: VrmlParser.cpp:496
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
const char * what() const
Definition: ModelNodeSet.h:85
time_t lastUpdate_
Definition: BodyInfo_impl.h:63
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
OpenHRP::vector3 Vector3
std::string SFString
Definition: VrmlNodes.h:52
void copyVrmlField(TProtoFieldMap &fmap, const std::string &name, std::string &out_s)
Definition: VrmlUtil.cpp:22
png_infop png_uint_32 png_uint_32 * height
Definition: png.h:2309
list index
LinkInfoSequence links_
Definition: BodyInfo_impl.h:70
void loadModelFile(const std::string &filename)
This function loads a model file and creates a BodyInfo object.
void load(const std::string &filename)
Definition: VrmlParser.cpp:463
void setHwcs(int linkInfoIndex, JointNodeSetPtr jointNodeSet)
png_bytepp row
Definition: png.h:1759
Header file of Image Converter class.
void setSegmentParameters(int linkInfoIndex, JointNodeSetPtr jointNodeSet)
Eigen::Vector4d Vector4
Definition: Eigen4d.h:21
std::string name_
Definition: BodyInfo_impl.h:67
std::vector< ColdetModelPtr > linkColdetModels
Definition: BodyInfo_impl.h:75
void copyVrmlRotationFieldToDblArray4(TProtoFieldMap &fieldMap, const std::string name, DblArray4 &out_R)
Definition: VrmlUtil.cpp:193
ExtraJointInfoSequence extraJoints_
Definition: BodyInfo_impl.h:73
boost::intrusive_ptr< VrmlNode > VrmlNodePtr
Definition: VrmlNodes.h:155
void setColdetModel(ColdetModelPtr &coldetModel, TransformedShapeIndexSequence shapeIndices, const Matrix44 &Tparent, int &vertexIndex, int &triangleIndex)
HRP_UTIL_EXPORT string deleteURLScheme(string url)
Definition: UrlUtil.cpp:25
void setJointParameters(int linkInfoIndex, VrmlProtoInstancePtr jointNode)
void setSensors(int linkInfoIndex, JointNodeSetPtr jointNodeSet)
Parser for VRML97 format.
Definition: VrmlParser.h:30
virtual StringSequence * info()
void readHwcNode(int linkInfoIndex, HwcInfo &hwcInfo, VrmlProtoInstancePtr hwcNode)
std::vector< SFString > MFString
Definition: VrmlNodes.h:77
virtual AllLinkShapeIndexSequence * linkShapeIndices()
int readJointNodeSet(JointNodeSetPtr jointNodeSet, int &currentIndex, int motherIndex)
HRP_UTIL_EXPORT void calcRodrigues(Matrix33 &out_R, const Vector3 &axis, double q)
Definition: Eigen3d.cpp:22
virtual char * name()
static void putMessage(const std::string &message)
virtual ExtraJointInfoSequence * extraJoints()
boost::shared_ptr< JointNodeSet > JointNodeSetPtr
Definition: ModelNodeSet.h:34
int num
Definition: png.h:1502
VrmlFieldTypeId typeId()
Definition: VrmlNodes.h:828
std::string url_
Definition: BodyInfo_impl.h:68
void traverseShapeNodes(VrmlNode *node, const Matrix44 &T, TransformedShapeIndexSequence &io_shapeIndices, DblArray12Sequence &inlinedShapeM, const SFString *url=NULL)
boost::intrusive_ptr< ColdetModel > ColdetModelPtr
Definition: ColdetModel.h:268
* y
Definition: IceUtils.h:97
void setLights(int linkInfoIndex, JointNodeSetPtr jointNodeSet)
StringSequence info_
Definition: BodyInfo_impl.h:69
std::string defName
Definition: VrmlNodes.h:130
boost::signal< void(const std::string &message)> sigMessage
Definition: ModelNodeSet.h:79
The header file of a text scanner class.


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:02