hrplib/hrpModel/ModelLoaderUtil.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 
15 #include "ModelLoaderUtil.h"
16 #include "Link.h"
17 #include "Sensor.h"
18 #include "Light.h"
19 #include <hrpUtil/Eigen3d.h>
20 #include <hrpUtil/Eigen4d.h>
21 #include <hrpCorba/OpenHRPCommon.hh>
22 #include <hrpCorba/ViewSimulator.hh>
24 #include <stack>
25 
26 using namespace std;
27 using namespace hrp;
28 using namespace OpenHRP;
29 
30 
31 namespace {
32 
33  const bool debugMode = false;
34 
35  ostream& operator<<(ostream& os, DblSequence_var& data)
36  {
37  int size = data->length();
38  for(int i=0; i < size-1; ++i){
39  cout << data[i] << ", ";
40  }
41  cout << data[size-1];
42 
43  return os;
44  }
45 
46 
47  ostream& operator<<(ostream& os, DblArray3_var& data)
48  {
49  cout << data[CORBA::ULong(0)] << ", " << data[CORBA::ULong(1)] << ", " << data[CORBA::ULong(2)];
50  return os;
51  }
52 
53 
54  ostream& operator<<(ostream& os, DblArray9_var& data)
55  {
56  for(CORBA::ULong i=0; i < 8; ++i){
57  cout << data[i] << ", ";
58  }
59  cout << data[CORBA::ULong(9)];
60  return os;
61  }
62 
63 
64  void dumpBodyInfo(BodyInfo_ptr bodyInfo)
65  {
66  cout << "<<< BodyInfo >>>\n";
67 
68  CORBA::String_var charaName = bodyInfo->name();
69 
70  cout << "name: " << charaName << "\n";
71 
72  LinkInfoSequence_var linkInfoSeq = bodyInfo->links();
73 
74  int numLinks = linkInfoSeq->length();
75  cout << "num links: " << numLinks << "\n";
76 
77  for(int i=0; i < numLinks; ++i){
78 
79  const LinkInfo& linkInfo = linkInfoSeq[i];
80  CORBA::String_var linkName = linkInfo.name;
81 
82  cout << "<<< LinkInfo: " << linkName << " (index " << i << ") >>>\n";
83  cout << "parentIndex: " << linkInfo.parentIndex << "\n";
84 
85  const ShortSequence& childIndices = linkInfo.childIndices;
86  if(childIndices.length() > 0){
87  cout << "childIndices: ";
88  for(CORBA::ULong i=0; i < childIndices.length(); ++i){
89  cout << childIndices[i] << " ";
90  }
91  cout << "\n";
92  }
93 
94  const SensorInfoSequence& sensorInfoSeq = linkInfo.sensors;
95 
96  int numSensors = sensorInfoSeq.length();
97  cout << "num sensors: " << numSensors << "\n";
98 
99  for(int j=0; j < numSensors; ++j){
100  cout << "<<< SensorInfo >>>\n";
101  const SensorInfo& sensorInfo = sensorInfoSeq[j];
102  cout << "id: " << sensorInfo.id << "\n";
103  cout << "type: " << sensorInfo.type << "\n";
104 
105  CORBA::String_var sensorName = sensorInfo.name;
106  cout << "name: \"" << sensorName << "\"\n";
107 
108  const DblArray3& p = sensorInfo.translation;
109  cout << "translation: " << p[0] << ", " << p[1] << ", " << p[2] << "\n";
110 
111  const DblArray4& r = sensorInfo.rotation;
112  cout << "rotation: " << r[0] << ", " << r[1] << ", " << r[2] << ", " << r[3] << "\n";
113 
114  }
115  }
116 
117  cout.flush();
118  }
119 
120 
121  inline double getLimitValue(DblSequence limitseq, double defaultValue)
122  {
123  return (limitseq.length() == 0) ? defaultValue : limitseq[0];
124  }
125 
126  static Link *createNewLink() { return new Link(); }
127  class ModelLoaderHelper
128  {
129  public:
130  ModelLoaderHelper() {
131  collisionDetectionModelLoading = false;
132  createLinkFunc = createNewLink;
133  }
134 
135  void enableCollisionDetectionModelLoading(bool isEnabled) {
136  collisionDetectionModelLoading = isEnabled;
137  };
138  void setLinkFactory(Link *(*f)()) { createLinkFunc = f; }
139 
140  bool createBody(BodyPtr& body, BodyInfo_ptr bodyInfo);
141 
142  private:
143  BodyPtr body;
144  LinkInfoSequence_var linkInfoSeq;
145  ShapeInfoSequence_var shapeInfoSeq;
146  ExtraJointInfoSequence_var extraJointInfoSeq;
147  bool collisionDetectionModelLoading;
148  Link *(*createLinkFunc)();
149 
150  Link* createLink(int index, const Matrix33& parentRs);
151  void createSensors(Link* link, const SensorInfoSequence& sensorInfoSeq, const Matrix33& Rs);
152  void createLights(Link* link, const LightInfoSequence& lightInfoSeq, const Matrix33& Rs);
153  void createColdetModel(Link* link, const LinkInfo& linkInfo);
154  void addLinkPrimitiveInfo(ColdetModelPtr& coldetModel,
155  const double *R, const double *p,
156  const ShapeInfo& shapeInfo);
157  void addLinkVerticesAndTriangles(ColdetModelPtr& coldetModel, const LinkInfo& linkInfo);
158  void addLinkVerticesAndTriangles(ColdetModelPtr& coldetModel, const TransformedShapeIndex& tsi, const Matrix44& Tparent, ShapeInfoSequence_var& shapes, int& vertexIndex, int& triangleIndex);
159  void setExtraJoints();
160  };
161 }
162 
163 
164 bool ModelLoaderHelper::createBody(BodyPtr& body, BodyInfo_ptr bodyInfo)
165 {
166  this->body = body;
167 
168  const char* name = bodyInfo->name();
169  body->setModelName(name);
170  body->setName(name);
171 
172  int n = bodyInfo->links()->length();
173  linkInfoSeq = bodyInfo->links();
174  shapeInfoSeq = bodyInfo->shapes();
175  extraJointInfoSeq = bodyInfo->extraJoints();
176 
177  int rootIndex = -1;
178 
179  for(int i=0; i < n; ++i){
180  if(linkInfoSeq[i].parentIndex < 0){
181  if(rootIndex < 0){
182  rootIndex = i;
183  } else {
184  // more than one root !
185  rootIndex = -1;
186  break;
187  }
188  }
189  }
190 
191  if(rootIndex >= 0){ // root exists
192 
193  Matrix33 Rs(Matrix33::Identity());
194  Link* rootLink = createLink(rootIndex, Rs);
195  body->setRootLink(rootLink);
196  body->setDefaultRootPosition(rootLink->b, rootLink->Rs);
197 
198  body->installCustomizer();
199  body->initializeConfiguration();
200 
201  setExtraJoints();
202 
203  return true;
204  }
205 
206  return false;
207 }
208 
209 
210 Link* ModelLoaderHelper::createLink(int index, const Matrix33& parentRs)
211 {
212  const LinkInfo& linkInfo = linkInfoSeq[index];
213  int jointId = linkInfo.jointId;
214 
215  Link* link = (*createLinkFunc)();
216 
217  CORBA::String_var name0 = linkInfo.name;
218  link->name = string( name0 );
219  link->jointId = jointId;
220 
221  Vector3 relPos(linkInfo.translation[0], linkInfo.translation[1], linkInfo.translation[2]);
222  link->b = parentRs * relPos;
223 
224  Vector3 rotAxis(linkInfo.rotation[0], linkInfo.rotation[1], linkInfo.rotation[2]);
225  Matrix33 R = rodrigues(rotAxis, linkInfo.rotation[3]);
226  link->Rs = (parentRs * R);
227  const Matrix33& Rs = link->Rs;
228 
229  CORBA::String_var jointType = linkInfo.jointType;
230  const std::string jt( jointType );
231 
232  if(jt == "fixed" ){
233  link->jointType = Link::FIXED_JOINT;
234  } else if(jt == "free" ){
235  link->jointType = Link::FREE_JOINT;
236  } else if(jt == "rotate" ){
237  link->jointType = Link::ROTATIONAL_JOINT;
238  } else if(jt == "slide" ){
239  link->jointType = Link::SLIDE_JOINT;
240  } else if(jt == "crawler"){
241  link->jointType = Link::FIXED_JOINT;
242  link->isCrawler = true;
243  } else {
244  link->jointType = Link::FREE_JOINT;
245  }
246 
247  if(jointId < 0){
248  if(link->jointType == Link::ROTATIONAL_JOINT || link->jointType == Link::SLIDE_JOINT){
249  std::cerr << "Warning: Joint ID is not given to joint " << link->name
250  << " of model " << body->modelName() << "." << std::endl;
251  }
252  }
253 
254  link->a.setZero();
255  link->d.setZero();
256 
257  Vector3 axis( Rs * Vector3(linkInfo.jointAxis[0], linkInfo.jointAxis[1], linkInfo.jointAxis[2]));
258 
259  if(link->jointType == Link::ROTATIONAL_JOINT || jt == "crawler"){
260  link->a = axis;
261  } else if(link->jointType == Link::SLIDE_JOINT){
262  link->d = axis;
263  }
264 
265  link->m = linkInfo.mass;
266  link->Ir = linkInfo.rotorInertia;
267 
268  link->gearRatio = linkInfo.gearRatio;
269  link->rotorResistor = linkInfo.rotorResistor;
270  link->torqueConst = linkInfo.torqueConst;
271  link->encoderPulse = linkInfo.encoderPulse;
272 
273  link->Jm2 = link->Ir * link->gearRatio * link->gearRatio;
274 
275  DblSequence ulimit = linkInfo.ulimit;
276  DblSequence llimit = linkInfo.llimit;
277  DblSequence uvlimit = linkInfo.uvlimit;
278  DblSequence lvlimit = linkInfo.lvlimit;
279  DblSequence climit = linkInfo.climit;
280 
281  double maxlimit = (numeric_limits<double>::max)();
282 
283  link->ulimit = getLimitValue(ulimit, +maxlimit);
284  link->llimit = getLimitValue(llimit, -maxlimit);
285  link->uvlimit = getLimitValue(uvlimit, +maxlimit);
286  link->lvlimit = getLimitValue(lvlimit, -maxlimit);
287  link->climit = getLimitValue(climit, +maxlimit);
288 
289  link->c = Rs * Vector3(linkInfo.centerOfMass[0], linkInfo.centerOfMass[1], linkInfo.centerOfMass[2]);
290 
291  Matrix33 Io;
292  getMatrix33FromRowMajorArray(Io, linkInfo.inertia);
293  link->I = Rs * Io * Rs.transpose();
294 
295  // a stack is used for keeping the same order of children
296  std::stack<Link*> children;
297 
298  //##### [Changed] Link Structure (convert NaryTree to BinaryTree).
299  int childNum = linkInfo.childIndices.length();
300  for(int i = 0 ; i < childNum ; i++) {
301  int childIndex = linkInfo.childIndices[i];
302  Link* childLink = createLink(childIndex, Rs);
303  if(childLink) {
304  children.push(childLink);
305  }
306  }
307  while(!children.empty()){
308  link->addChild(children.top());
309  children.pop();
310  }
311 
312  createSensors(link, linkInfo.sensors, Rs);
313  createLights(link, linkInfo.lights, Rs);
314 
315  if(collisionDetectionModelLoading){
316  createColdetModel(link, linkInfo);
317  }
318 
319  return link;
320 }
321 
322 
323 void ModelLoaderHelper::createLights(Link* link, const LightInfoSequence& lightInfoSeq, const Matrix33& Rs)
324 {
325  int numLights = lightInfoSeq.length();
326 
327  for(int i=0 ; i < numLights ; ++i ) {
328  const LightInfo& lightInfo = lightInfoSeq[i];
329  std::string name(lightInfo.name);
330  Light *light = body->createLight(link, lightInfo.type, name);
331  const double *T = lightInfo.transformMatrix;
332  light->localPos << T[3], T[7], T[11];
333  light->localR << T[0], T[1], T[2], T[4], T[5], T[6], T[8], T[9], T[10];
334  switch (lightInfo.type){
335  case Light::POINT:
336  light->ambientIntensity = lightInfo.ambientIntensity;
337  getVector3(light->attenuation, lightInfo.attenuation);
338  getVector3(light->color, lightInfo.color);
339  light->intensity = lightInfo.intensity;
340  getVector3(light->location, lightInfo.location);
341  light->on = lightInfo.on;
342  light->radius = lightInfo.radius;
343  break;
344  case Light::DIRECTIONAL:
345  light->ambientIntensity = lightInfo.ambientIntensity;
346  getVector3(light->color, lightInfo.color);
347  light->intensity = lightInfo.intensity;
348  light->on = lightInfo.on;
349  getVector3(light->direction, lightInfo.color);
350  break;
351  case Light::SPOT:
352  light->ambientIntensity = lightInfo.ambientIntensity;
353  getVector3(light->attenuation, lightInfo.attenuation);
354  getVector3(light->color, lightInfo.color);
355  light->intensity = lightInfo.intensity;
356  getVector3(light->location, lightInfo.location);
357  light->on = lightInfo.on;
358  light->radius = lightInfo.radius;
359  getVector3(light->direction, lightInfo.direction);
360  light->beamWidth = lightInfo.beamWidth;
361  light->cutOffAngle = lightInfo.cutOffAngle;
362  break;
363  default:
364  std::cerr << "unknown light type" << std::endl;
365  }
366  }
367 }
368 
369 void ModelLoaderHelper::createSensors(Link* link, const SensorInfoSequence& sensorInfoSeq, const Matrix33& Rs)
370 {
371  int numSensors = sensorInfoSeq.length();
372 
373  for(int i=0 ; i < numSensors ; ++i ) {
374  const SensorInfo& sensorInfo = sensorInfoSeq[i];
375 
376  int id = sensorInfo.id;
377  if(id < 0) {
378  std::cerr << "Warning: sensor ID is not given to sensor " << sensorInfo.name
379  << "of model " << body->modelName() << "." << std::endl;
380  } else {
381  int sensorType = Sensor::COMMON;
382 
383  CORBA::String_var type0 = sensorInfo.type;
384  string type(type0);
385 
386  if(type == "Force") { sensorType = Sensor::FORCE; }
387  else if(type == "RateGyro") { sensorType = Sensor::RATE_GYRO; }
388  else if(type == "Acceleration") { sensorType = Sensor::ACCELERATION; }
389  else if(type == "Vision") { sensorType = Sensor::VISION; }
390  else if(type == "Range") { sensorType = Sensor::RANGE; }
391 
392  CORBA::String_var name0 = sensorInfo.name;
393  string name(name0);
394 
395  Sensor* sensor = body->createSensor(link, sensorType, id, name);
396 
397  if(sensor) {
398  const DblArray3& p = sensorInfo.translation;
399  sensor->localPos = Rs * Vector3(p[0], p[1], p[2]);
400 
401  const Vector3 axis(sensorInfo.rotation[0], sensorInfo.rotation[1], sensorInfo.rotation[2]);
402  const Matrix33 R(rodrigues(axis, sensorInfo.rotation[3]));
403  sensor->localR = Rs * R;
404  }
405 
406  if ( sensorType == Sensor::RANGE ) {
407  RangeSensor *range = dynamic_cast<RangeSensor *>(sensor);
408  range->scanAngle = sensorInfo.specValues[0];
409  range->scanStep = sensorInfo.specValues[1];
410  range->scanRate = sensorInfo.specValues[2];
411  range->maxDistance = sensorInfo.specValues[3];
412  }else if (sensorType == Sensor::VISION) {
413  VisionSensor *vision = dynamic_cast<VisionSensor *>(sensor);
414  vision->near = sensorInfo.specValues[0];
415  vision->far = sensorInfo.specValues[1];
416  vision->fovy = sensorInfo.specValues[2];
417  vision->width = sensorInfo.specValues[4];
418  vision->height = sensorInfo.specValues[5];
419  int npixel = vision->width*vision->height;
420  switch((int)sensorInfo.specValues[3]){
421  case Camera::NONE:
422  vision->imageType = VisionSensor::NONE;
423  break;
424  case Camera::COLOR:
425  vision->imageType = VisionSensor::COLOR;
426  vision->image.resize(npixel*3);
427  break;
428  case Camera::MONO:
429  vision->imageType = VisionSensor::MONO;
430  vision->image.resize(npixel);
431  break;
432  case Camera::DEPTH:
433  vision->imageType = VisionSensor::DEPTH;
434  break;
435  case Camera::COLOR_DEPTH:
436  vision->imageType = VisionSensor::COLOR_DEPTH;
437  vision->image.resize(npixel*3);
438  break;
439  case Camera::MONO_DEPTH:
440  vision->imageType = VisionSensor::MONO_DEPTH;
441  vision->image.resize(npixel);
442  break;
443  }
444  vision->frameRate = sensorInfo.specValues[6];
445  }
446  }
447  }
448 }
449 
450 
451 void ModelLoaderHelper::createColdetModel(Link* link, const LinkInfo& linkInfo)
452 {
453  int totalNumVertices = 0;
454  int totalNumTriangles = 0;
455  const TransformedShapeIndexSequence& shapeIndices = linkInfo.shapeIndices;
456  unsigned int nshape = shapeIndices.length();
457  short shapeIndex;
458  double R[9], p[3];
459  for(unsigned int i=0; i < shapeIndices.length(); i++){
460  shapeIndex = shapeIndices[i].shapeIndex;
461  const DblArray12 &tform = shapeIndices[i].transformMatrix;
462  R[0] = tform[0]; R[1] = tform[1]; R[2] = tform[2]; p[0] = tform[3];
463  R[3] = tform[4]; R[4] = tform[5]; R[5] = tform[6]; p[1] = tform[7];
464  R[6] = tform[8]; R[7] = tform[9]; R[8] = tform[10]; p[2] = tform[11];
465  const ShapeInfo& shapeInfo = shapeInfoSeq[shapeIndex];
466  totalNumVertices += shapeInfo.vertices.length() / 3;
467  totalNumTriangles += shapeInfo.triangles.length() / 3;
468  }
469 
470  const SensorInfoSequence& sensors = linkInfo.sensors;
471  for (unsigned int i=0; i<sensors.length(); i++){
472  const SensorInfo &sinfo = sensors[i];
473  const TransformedShapeIndexSequence tsis = sinfo.shapeIndices;
474  nshape += tsis.length();
475  for (unsigned int j=0; j<tsis.length(); j++){
476  short shapeIndex = tsis[j].shapeIndex;
477  const ShapeInfo& shapeInfo = shapeInfoSeq[shapeIndex];
478  totalNumTriangles += shapeInfo.triangles.length() / 3;
479  totalNumVertices += shapeInfo.vertices.length() / 3 ;
480  }
481  }
482 
483  ColdetModelPtr coldetModel(new ColdetModel());
484  coldetModel->setName(std::string(linkInfo.name));
485  if(totalNumTriangles > 0){
486  coldetModel->setNumVertices(totalNumVertices);
487  coldetModel->setNumTriangles(totalNumTriangles);
488  if (nshape == 1){
489  addLinkPrimitiveInfo(coldetModel, R, p, shapeInfoSeq[shapeIndex]);
490  }
491  addLinkVerticesAndTriangles(coldetModel, linkInfo);
492  coldetModel->build();
493  }
494  link->coldetModel = coldetModel;
495 }
496 
497 void ModelLoaderHelper::addLinkVerticesAndTriangles
498 (ColdetModelPtr& coldetModel, const TransformedShapeIndex& tsi, const Matrix44& Tparent, ShapeInfoSequence_var& shapes, int& vertexIndex, int& triangleIndex)
499 {
500  short shapeIndex = tsi.shapeIndex;
501  const DblArray12& M = tsi.transformMatrix;;
502  Matrix44 T, Tlocal;
503  Tlocal << M[0], M[1], M[2], M[3],
504  M[4], M[5], M[6], M[7],
505  M[8], M[9], M[10], M[11],
506  0.0, 0.0, 0.0, 1.0;
507  T = Tparent * Tlocal;
508 
509  const ShapeInfo& shapeInfo = shapes[shapeIndex];
510  int vertexIndexBase = vertexIndex;
511  const FloatSequence& vertices = shapeInfo.vertices;
512  const int numVertices = vertices.length() / 3;
513  for(int j=0; j < numVertices; ++j){
514  Vector4 v(T * Vector4(vertices[j*3], vertices[j*3+1], vertices[j*3+2], 1.0));
515  coldetModel->setVertex(vertexIndex++, v[0], v[1], v[2]);
516  }
517 
518  const LongSequence& triangles = shapeInfo.triangles;
519  const int numTriangles = triangles.length() / 3;
520  for(int j=0; j < numTriangles; ++j){
521  int t0 = triangles[j*3] + vertexIndexBase;
522  int t1 = triangles[j*3+1] + vertexIndexBase;
523  int t2 = triangles[j*3+2] + vertexIndexBase;
524  coldetModel->setTriangle( triangleIndex++, t0, t1, t2);
525  }
526 
527 }
528 
529 void ModelLoaderHelper::addLinkVerticesAndTriangles(ColdetModelPtr& coldetModel, const LinkInfo& linkInfo)
530 {
531  int vertexIndex = 0;
532  int triangleIndex = 0;
533 
534  const TransformedShapeIndexSequence& shapeIndices = linkInfo.shapeIndices;
535 
536  Matrix44 E(Matrix44::Identity());
537  for(unsigned int i=0; i < shapeIndices.length(); i++){
538  addLinkVerticesAndTriangles(coldetModel, shapeIndices[i], E, shapeInfoSeq,
539  vertexIndex, triangleIndex);
540  }
541 
542  Matrix44 T(Matrix44::Identity());
543  const SensorInfoSequence& sensors = linkInfo.sensors;
544  for (unsigned int i=0; i<sensors.length(); i++){
545  const SensorInfo& sensor = sensors[i];
546  calcRodrigues(T, Vector3(sensor.rotation[0], sensor.rotation[1],
547  sensor.rotation[2]), sensor.rotation[3]);
548  T(0,3) = sensor.translation[0];
549  T(1,3) = sensor.translation[1];
550  T(2,3) = sensor.translation[2];
551  const TransformedShapeIndexSequence& shapeIndices = sensor.shapeIndices;
552  for (unsigned int j=0; j<shapeIndices.length(); j++){
553  addLinkVerticesAndTriangles(coldetModel, shapeIndices[j], T,
554  shapeInfoSeq,
555  vertexIndex, triangleIndex);
556  }
557  }
558 }
559 
560 void ModelLoaderHelper::addLinkPrimitiveInfo(ColdetModelPtr& coldetModel,
561  const double *R, const double *p,
562  const ShapeInfo& shapeInfo)
563 {
564  switch(shapeInfo.primitiveType){
565  case SP_BOX:
566  coldetModel->setPrimitiveType(ColdetModel::SP_BOX);
567  break;
568  case SP_CYLINDER:
569  coldetModel->setPrimitiveType(ColdetModel::SP_CYLINDER);
570  break;
571  case SP_CONE:
572  coldetModel->setPrimitiveType(ColdetModel::SP_CONE);
573  break;
574  case SP_SPHERE:
575  coldetModel->setPrimitiveType(ColdetModel::SP_SPHERE);
576  break;
577  case SP_PLANE:
578  coldetModel->setPrimitiveType(ColdetModel::SP_PLANE);
579  break;
580  default:
581  break;
582  }
583  coldetModel->setNumPrimitiveParams(shapeInfo.primitiveParameters.length());
584  for (unsigned int i=0; i<shapeInfo.primitiveParameters.length(); i++){
585  coldetModel->setPrimitiveParam(i, shapeInfo.primitiveParameters[i]);
586  }
587  coldetModel->setPrimitivePosition(R, p);
588 }
589 
590 void ModelLoaderHelper::setExtraJoints()
591 {
592  body->extraJoints.clear();
593  int n = extraJointInfoSeq->length();
594 
595  for(int i=0; i < n; ++i){
596  const ExtraJointInfo& extraJointInfo = extraJointInfoSeq[i];
597  Body::ExtraJoint joint;
598  joint.name = extraJointInfo.name;
599  joint.link[0] = body->link(string(extraJointInfo.link[0]));
600  joint.link[1] = body->link(string(extraJointInfo.link[1]));
601 
602  ExtraJointType jointType = extraJointInfo.jointType;
603  if(jointType == OpenHRP::EJ_XY){
604  joint.type = Body::EJ_XY;
605  }else if(jointType == OpenHRP::EJ_XYZ){
606  joint.type = Body::EJ_XYZ;
607  }else if(jointType == OpenHRP::EJ_Z){
608  joint.type = Body::EJ_Z;
609  }
610 
611  joint.axis = Vector3(extraJointInfo.axis[0], extraJointInfo.axis[1], extraJointInfo.axis[2] );
612  joint.point[0] = Vector3(extraJointInfo.point[0][0], extraJointInfo.point[0][1], extraJointInfo.point[0][2] );
613  joint.point[1] = Vector3(extraJointInfo.point[1][0], extraJointInfo.point[1][1], extraJointInfo.point[1][2] );
614 
615  body->extraJoints.push_back(joint);
616  }
617 }
618 
619 bool hrp::loadBodyFromBodyInfo(BodyPtr body, OpenHRP::BodyInfo_ptr bodyInfo, bool loadGeometryForCollisionDetection, Link *(*f)())
620 {
621  if(!CORBA::is_nil(bodyInfo)){
622  ModelLoaderHelper helper;
623  if (f) helper.setLinkFactory(f);
624  if(loadGeometryForCollisionDetection){
625  helper.enableCollisionDetectionModelLoading(true);
626  }
627  return helper.createBody(body, bodyInfo);
628  }
629  return false;
630 }
631 
632 BodyInfo_var hrp::loadBodyInfo(const char* url, int& argc, char* argv[])
633 {
634  CORBA::ORB_var orb = CORBA::ORB_init(argc, argv);
635  return loadBodyInfo(url, orb);
636 }
637 
638 BodyInfo_var hrp::loadBodyInfo(const char* url, CORBA_ORB_var orb)
639 {
640  CosNaming::NamingContext_var cxt;
641  try {
642  CORBA::Object_var nS = orb->resolve_initial_references("NameService");
643  cxt = CosNaming::NamingContext::_narrow(nS);
644  } catch(CORBA::SystemException& ex) {
645  std::cerr << "NameService doesn't exist" << std::endl;
646  return BodyInfo::_nil();
647  }
648  return loadBodyInfo(url, cxt);
649 }
650 
651 ModelLoader_var hrp::getModelLoader(CORBA_ORB_var orb)
652 {
653  CosNaming::NamingContext_var cxt;
654  try {
655  CORBA::Object_var nS = orb->resolve_initial_references("NameService");
656  cxt = CosNaming::NamingContext::_narrow(nS);
657  } catch(CORBA::SystemException& ex) {
658  std::cerr << "NameService doesn't exist" << std::endl;
659  return NULL;
660  }
661  return getModelLoader(cxt);
662 }
663 
664 ModelLoader_var hrp::getModelLoader(CosNaming::NamingContext_var cxt)
665 {
666  CosNaming::Name ncName;
667  ncName.length(1);
668  ncName[0].id = CORBA::string_dup("ModelLoader");
669  ncName[0].kind = CORBA::string_dup("");
670  ModelLoader_var modelLoader = NULL;
671  try {
672  modelLoader = ModelLoader::_narrow(cxt->resolve(ncName));
673  modelLoader->_non_existent();
674  } catch(const CosNaming::NamingContext::NotFound &exc) {
675  std::cerr << "ModelLoader not found: ";
676  switch(exc.why) {
677  case CosNaming::NamingContext::missing_node:
678  std::cerr << "Missing Node" << std::endl;
679  case CosNaming::NamingContext::not_context:
680  std::cerr << "Not Context" << std::endl;
681  break;
682  case CosNaming::NamingContext::not_object:
683  std::cerr << "Not Object" << std::endl;
684  break;
685  }
686  modelLoader = ModelLoader::_nil();
687  } catch(CosNaming::NamingContext::CannotProceed &exc) {
688  std::cerr << "Resolve ModelLoader CannotProceed" << std::endl;
689  modelLoader = ModelLoader::_nil();
690  } catch(CosNaming::NamingContext::AlreadyBound &exc) {
691  std::cerr << "Resolve ModelLoader InvalidName" << std::endl;
692  modelLoader = ModelLoader::_nil();
693  } catch(...){
694  modelLoader = ModelLoader::_nil();
695  }
696  return modelLoader;
697 }
698 
699 BodyInfo_var hrp::loadBodyInfo(const char* url, CosNaming::NamingContext_var cxt)
700 {
701  ModelLoader_var modelLoader = getModelLoader(cxt);
702 
703  BodyInfo_var bodyInfo;
704  try {
705  bodyInfo = modelLoader->getBodyInfo(url);
706  } catch(CORBA::SystemException& ex) {
707  std::cerr << "CORBA::SystemException raised by ModelLoader: " << ex._rep_id() << std::endl;
708  } catch(ModelLoader::ModelLoaderException& ex){
709  std::cerr << "ModelLoaderException : " << ex.description << std::endl;
710  }
711  return bodyInfo;
712 }
713 
714 bool hrp::loadBodyFromModelLoader(BodyPtr body, const char* url, CosNaming::NamingContext_var cxt, bool loadGeometryForCollisionDetection)
715 {
716  BodyInfo_var bodyInfo = loadBodyInfo(url, cxt);
717 
718  if(!CORBA::is_nil(bodyInfo)){
719  ModelLoaderHelper helper;
720  if(loadGeometryForCollisionDetection){
721  helper.enableCollisionDetectionModelLoading(true);
722  }
723  return helper.createBody(body, bodyInfo);
724  }
725 
726  return false;
727 }
728 
729 
730 bool hrp::loadBodyFromModelLoader(BodyPtr body, const char* url, CORBA_ORB_var orb, bool loadGeometryForCollisionDetection)
731 {
732  CosNaming::NamingContext_var cxt;
733  try {
734  CORBA::Object_var nS = orb->resolve_initial_references("NameService");
735  cxt = CosNaming::NamingContext::_narrow(nS);
736  } catch(CORBA::SystemException& ex) {
737  std::cerr << "NameService doesn't exist" << std::endl;
738  return false;
739  }
740 
741  return loadBodyFromModelLoader(body, url, cxt, loadGeometryForCollisionDetection);
742 }
743 
744 
745 bool hrp::loadBodyFromModelLoader(BodyPtr body, const char* url, int& argc, char* argv[], bool loadGeometryForCollisionDetection)
746 {
747  CORBA::ORB_var orb = CORBA::ORB_init(argc, argv);
748  return loadBodyFromModelLoader(body, url, orb, loadGeometryForCollisionDetection);
749 }
NotFound
Definition: hrpPrep.py:129
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2332
HRPMODEL_API OpenHRP::BodyInfo_var loadBodyInfo(const char *url, int &argc, char *argv[])
HRPMODEL_API OpenHRP::ModelLoader_var getModelLoader(CosNaming::NamingContext_var cxt)
ExtraJointType type
Definition: Body.h:250
std::vector< unsigned char > image
static const bool debugMode
png_uint_32 size
Definition: png.h:1521
png_infop png_charpp name
Definition: png.h:2382
Vector3 direction
Definition: Light.h:40
HRPMODEL_API bool loadBodyFromBodyInfo(BodyPtr body, OpenHRP::BodyInfo_ptr bodyInfo, bool loadGeometryForCollisionDetection=false, Link *(*f)()=NULL)
void getMatrix33FromRowMajorArray(Matrix33 &m33, const Array &a, size_t top=0)
Definition: Eigen3d.h:118
png_uint_32 i
Definition: png.h:2735
Vector3 point[2]
Definition: Body.h:253
Eigen::Matrix4d Matrix44
Definition: Eigen4d.h:18
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
OpenHRP::vector3 Vector3
Matrix33 localR
Definition: Light.h:30
#define CORBA_ORB_var
Definition: ORBwrap.h:17
double radius
Definition: Light.h:38
bool on
Definition: Light.h:35
Matrix33 rodrigues(const Vector3 &axis, double q)
Definition: Eigen3d.h:27
std::ostream & operator<<(std::ostream &ost, const Point &p)
Definition: DistFuncs.cpp:3
Eigen::Vector4d Vector4
Definition: Eigen4d.h:21
Link * link[2]
Definition: Body.h:252
Vector3 attenuation
Definition: Light.h:37
Vector3 color
Definition: Light.h:34
double beamWidth
Definition: Light.h:42
static Joint * createLink(::World *world, const char *charname, int index, LinkInfoSequence_var iLinks, Joint *pjoint)
double getLimitValue(DblSequence limitseq, double defaultValue)
HRP_UTIL_EXPORT void calcRodrigues(Matrix33 &out_R, const Vector3 &axis, double q)
Definition: Eigen3d.cpp:22
void getVector3(Vector3 &v3, const V &v, size_t top=0)
Definition: Eigen3d.h:138
JSAMPIMAGE data
Definition: jpeglib.h:945
Vector3 location
Definition: Light.h:37
boost::intrusive_ptr< ColdetModel > ColdetModelPtr
Definition: ColdetModel.h:268
double intensity
Definition: Light.h:33
std::string name
Definition: Body.h:249
HRPMODEL_API bool loadBodyFromModelLoader(BodyPtr body, const char *url, CORBA_ORB_var orb, bool loadGeometryForCollisionDetection=false)
static int max(int a, int b)
Vector3 localPos
Definition: Light.h:31
static void createSensors(::World *world, Joint *jnt, SensorInfoSequence iSensors)
double ambientIntensity
Definition: Light.h:33
double cutOffAngle
Definition: Light.h:42


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:04