ODE_ModelLoaderUtil.cpp
Go to the documentation of this file.
1 #include "ODE_ModelLoaderUtil.h"
2 #include <stack>
3 
4 using namespace hrp;
5 using namespace OpenHRP;
6 
7 namespace ODESim
8 {
10  {
11  public:
12  bool createBody(BodyPtr body, ODE_World* worldId, BodyInfo_ptr bodyInfo);
13 
14  private:
16  dWorldID worldId;
17  dSpaceID spaceId;
18  LinkInfoSequence_var linkInfoSeq;
19  ShapeInfoSequence_var shapeInfoSeq;
20 
21  ODE_Link* createLink(int index, dBodyID parentBodyId, const Matrix44& parentT);
22  void createSensors(Link* link, const SensorInfoSequence& sensorInfoSeq, const Matrix33& Rs);
23  void createGeometry(ODE_Link* link, const LinkInfo& linkInfo);
24  void addLinkVerticesAndTriangles(ODE_Link* link, const LinkInfo& linkInfo);
25  };
26 };
27 
28  inline double getLimitValue(DblSequence limitseq, double defaultValue)
29  {
30  return (limitseq.length() == 0) ? defaultValue : limitseq[0];
31  }
32 
33 
34 using namespace ODESim;
35 using namespace std;
36 bool ModelLoaderHelper::createBody(BodyPtr body, ODE_World* world, BodyInfo_ptr bodyInfo)
37 {
38  worldId = world->getWorldID();
39  spaceId = world->getSpaceID();
40  this->body = body;
41  const char* name = bodyInfo->name();
42  body->setModelName(name);
43  body->setName(name);
44 
45  int n = bodyInfo->links()->length();
46  linkInfoSeq = bodyInfo->links();
47  shapeInfoSeq = bodyInfo->shapes();
48 
49  int rootIndex = -1;
50 
51  for(int i=0; i < n; ++i){
52  if(linkInfoSeq[i].parentIndex < 0){
53  if(rootIndex < 0){
54  rootIndex = i;
55  } else {
56  // more than one root !
57  rootIndex = -1;
58  break;
59  }
60  }
61  }
62 
63  if(rootIndex >= 0){ // root exists
64  Matrix44 T(Matrix44::Identity());
65  ODE_Link* rootLink = createLink(rootIndex, 0, T);
66  body->setRootLink(rootLink);
67 
68  body->setDefaultRootPosition(rootLink->b, rootLink->Rs);
69 
70  body->initializeConfiguration();
71 
72  return true;
73  }
74 
75  return false;
76 }
77 
78 ODE_Link* ModelLoaderHelper::createLink(int index, dBodyID parentBodyId, const Matrix44& parentT)
79 {
80  const LinkInfo& linkInfo = linkInfoSeq[index];
81  int jointId = linkInfo.jointId;
82 
83  ODE_Link* link = new ODE_Link();
84  dBodyID bodyId = dBodyCreate(worldId);
85  link->bodyId = bodyId;
86 
87  CORBA::String_var name0 = linkInfo.name;
88  link->name = string( name0 );
89  link->jointId = jointId;
90 
91  Matrix33 parentRs;
92  parentRs << parentT(0,0), parentT(0,1), parentT(0,2),
93  parentT(1,0), parentT(1,1), parentT(1,2),
94  parentT(2,0), parentT(2,1), parentT(2,2);
95  Vector3 relPos(linkInfo.translation[0], linkInfo.translation[1], linkInfo.translation[2]);
96  link->b = parentRs * relPos;
97 
98  Vector3 rotAxis(linkInfo.rotation[0], linkInfo.rotation[1], linkInfo.rotation[2]);
99  Matrix44 localT;
100  calcRodrigues(localT, rotAxis,linkInfo.rotation[3]);
101  localT(0,3) = linkInfo.translation[0];
102  localT(1,3) = linkInfo.translation[1];
103  localT(2,3) = linkInfo.translation[2];
104  Matrix44 T(parentT*localT);
105 
106  link->Rs << T(0,0), T(0,1), T(0,2),
107  T(1,0), T(1,1), T(1,2),
108  T(2,0), T(2,1), T(2,2);
109  Vector3 p(T(0,3), T(1,3), T(2,3));
110  const Matrix33& Rs = link->Rs;
111  link->C = Vector3(linkInfo.centerOfMass[0], linkInfo.centerOfMass[1], linkInfo.centerOfMass[2]);
112 
113  link->setTransform(p, Rs);
114 
115  CORBA::String_var jointType = linkInfo.jointType;
116  const string jt( jointType );
117  if(jt == "fixed" ){
119  dJointID djointId = dJointCreateFixed(worldId, 0);
120  dJointAttach(djointId, bodyId, 0);
121  link->odeJointId = djointId;
122  } else if(jt == "free" ){
124  } else if(jt == "rotate" ){
126  dJointID djointId = dJointCreateHinge(worldId, 0);
127  dJointAttach(djointId, bodyId, parentBodyId);
128  dJointSetHingeAnchor(djointId, T(0,3), T(1,3), T(2,3));
129  Vector4 axis( T * Vector4(linkInfo.jointAxis[0], linkInfo.jointAxis[1], linkInfo.jointAxis[2], 0));
130  dJointSetHingeAxis(djointId, axis(0), axis(1), axis(2));
131  link->odeJointId = djointId;
132  } else if(jt == "slide" ){
134  dJointID djointId = dJointCreateSlider(worldId, 0);
135  dJointAttach(djointId, bodyId, parentBodyId);
136  Vector4 axis( T * Vector4(linkInfo.jointAxis[0], linkInfo.jointAxis[1], linkInfo.jointAxis[2], 0));
137  dJointSetSliderAxis(djointId, axis(0), axis(1), axis(2));
138  link->odeJointId = djointId;
139  } else {
141  }
142 
143  link->a.setZero();
144  link->d.setZero();
145  Vector3 axis( Rs * Vector3(linkInfo.jointAxis[0], linkInfo.jointAxis[1], linkInfo.jointAxis[2]));
146 
147  if(link->jointType == Link::ROTATIONAL_JOINT){
148  link->a = axis;
149  } else if(link->jointType == Link::SLIDE_JOINT){
150  link->d = axis;
151  }
152 
153  if(jointId < 0){
155  cerr << "Warning: Joint ID is not given to joint " << link->name
156  << " of model " << body->modelName() << "." << endl;
157  }
158  }
159 
160  link->m = linkInfo.mass;
161  link->Ir = linkInfo.rotorInertia;
162 
163  if(jt != "fixed" ){
164  dMass mass;
165  dMassSetZero(&mass);
166  dMassSetParameters(&mass, linkInfo.mass,
167  0,0,0,
168  linkInfo.inertia[0], linkInfo.inertia[4], linkInfo.inertia[8],
169  linkInfo.inertia[1], linkInfo.inertia[2], linkInfo.inertia[5] );
170  dBodySetMass(bodyId, &mass);
171  }
172 
173  link->gearRatio = linkInfo.gearRatio;
174  link->rotorResistor = linkInfo.rotorResistor;
175  link->torqueConst = linkInfo.torqueConst;
176  link->encoderPulse = linkInfo.encoderPulse;
177 
178  link->Jm2 = link->Ir * link->gearRatio * link->gearRatio;
179 
180  DblSequence ulimit = linkInfo.ulimit;
181  DblSequence llimit = linkInfo.llimit;
182  DblSequence uvlimit = linkInfo.uvlimit;
183  DblSequence lvlimit = linkInfo.lvlimit;
184 
185  double maxlimit = (numeric_limits<double>::max)();
186 
187  link->ulimit = getLimitValue(ulimit, +maxlimit);
188  link->llimit = getLimitValue(llimit, -maxlimit);
189  link->uvlimit = getLimitValue(uvlimit, +maxlimit);
190  link->lvlimit = getLimitValue(lvlimit, -maxlimit);
191 
192  link->c = Rs * Vector3(linkInfo.centerOfMass[0], linkInfo.centerOfMass[1], linkInfo.centerOfMass[2]);
193 
194  Matrix33 Io;
195  getMatrix33FromRowMajorArray(Io, linkInfo.inertia);
196  link->I = Rs * Io * Rs.transpose();
197 
198  // a stack is used for keeping the same order of children
199  stack<Link*> children;
200 
201  //##### [Changed] Link Structure (convert NaryTree to BinaryTree).
202  int childNum = linkInfo.childIndices.length();
203  for(int i = 0 ; i < childNum ; i++) {
204  int childIndex = linkInfo.childIndices[i];
205  Link* childLink = createLink(childIndex, bodyId, T);
206  if(childLink) {
207  children.push(childLink);
208  }
209  }
210  while(!children.empty()){
211  link->addChild(children.top());
212  children.pop();
213  }
214 
215  createSensors(link, linkInfo.sensors, Rs);
216 
217  createGeometry(link, linkInfo);
218 
219  return link;
220 }
221 
222 void ModelLoaderHelper::createGeometry(ODE_Link* link, const LinkInfo& linkInfo)
223 {
224  int totalNumTriangles = 0;
225  const TransformedShapeIndexSequence& shapeIndices = linkInfo.shapeIndices;
226  int numofGeom = 0;
227  for(unsigned int i=0; i < shapeIndices.length(); i++){
228  const TransformedShapeIndex& tsi = shapeIndices[i];
229  const DblArray12& M = tsi.transformMatrix;
230  dMatrix3 R = { M[0], M[1], M[2], 0,
231  M[4], M[5], M[6], 0,
232  M[8], M[9], M[10], 0 };
233  short shapeIndex = shapeIndices[i].shapeIndex;
234  const ShapeInfo& shapeInfo = shapeInfoSeq[shapeIndex];
235  Matrix33 R0;
236  R0 << M[0],M[1],M[2],
237  M[4],M[5],M[6],
238  M[8],M[9],M[10];
239  if(isOrthogonalMatrix(R0)){
240  switch(shapeInfo.primitiveType){
241  case OpenHRP::SP_BOX :{
242  dReal x = shapeInfo.primitiveParameters[0];
243  dReal y = shapeInfo.primitiveParameters[1];
244  dReal z = shapeInfo.primitiveParameters[2];
245  link->geomIds.push_back(dCreateBox( spaceId, x, y, z));
246  dGeomSetBody(link->geomIds.at(numofGeom), link->bodyId);
247  dGeomSetOffsetRotation(link->geomIds.at(numofGeom), R);
248  dGeomSetOffsetPosition(link->geomIds.at(numofGeom), M[3]-link->C(0), M[7]-link->C(1), M[11]-link->C(2));
249  numofGeom++;
250  }
251  break;
252  case OpenHRP::SP_CYLINDER :{
253  dReal radius = shapeInfo.primitiveParameters[0];
254  dReal height = shapeInfo.primitiveParameters[1];
255  link->geomIds.push_back(dCreateCylinder( spaceId, radius, height));
256  dGeomSetBody(link->geomIds.at(numofGeom), link->bodyId);
257  dGeomSetOffsetRotation(link->geomIds.at(numofGeom), R);
258  dGeomSetOffsetPosition(link->geomIds.at(numofGeom), M[3]-link->C(0), M[7]-link->C(1), M[11]-link->C(2));
259  numofGeom++;
260  }
261  break;
262  case OpenHRP::SP_SPHERE :{
263  dReal radius = shapeInfo.primitiveParameters[0];
264  link->geomIds.push_back(dCreateSphere( spaceId, radius ));
265  dGeomSetBody(link->geomIds.at(numofGeom), link->bodyId);
266  dGeomSetOffsetRotation(link->geomIds.at(numofGeom), R);
267  dGeomSetOffsetPosition(link->geomIds.at(numofGeom), M[3]-link->C(0), M[7]-link->C(1), M[11]-link->C(2));
268  numofGeom++;
269  }
270  break;
271  default :
272  totalNumTriangles += shapeInfo.triangles.length() / 3;
273  }
274  }else
275  totalNumTriangles += shapeInfo.triangles.length() / 3;
276  }
277  int totalNumVertices = totalNumTriangles * 3;
278 
279  link->vertices.resize(totalNumVertices*3);
280  link->indices.resize(totalNumTriangles*3);
281  addLinkVerticesAndTriangles( link, linkInfo );
282  if(totalNumTriangles){
283  link->triMeshDataId = dGeomTriMeshDataCreate();
284  dGeomTriMeshDataBuildSingle(link->triMeshDataId, &link->vertices[0], 3 * sizeof(dReal),
285  totalNumVertices, &link->indices[0], totalNumTriangles*3, 3*sizeof(int));
286  link->geomIds.push_back( dCreateTriMesh( spaceId, link->triMeshDataId, 0, 0, 0) );
287  dGeomSetBody(link->geomIds.at(numofGeom), link->bodyId);
288  dGeomSetOffsetPosition (link->geomIds.at(numofGeom), -link->C(0), -link->C(1), -link->C(2));
289  }else{
290  link->triMeshDataId = 0;
291  }
292 }
293 
294 void ModelLoaderHelper::addLinkVerticesAndTriangles(ODE_Link* link, const LinkInfo& linkInfo)
295 {
296  int vertexIndex = 0;
297  int triangleIndex = 0;
298 
299  const TransformedShapeIndexSequence& shapeIndices = linkInfo.shapeIndices;
300 
301  for(unsigned int i=0; i < shapeIndices.length(); i++){
302  const TransformedShapeIndex& tsi = shapeIndices[i];
303  short shapeIndex = tsi.shapeIndex;
304  const ShapeInfo& shapeInfo = shapeInfoSeq[shapeIndex];
305  const DblArray12& M = tsi.transformMatrix;
306  Matrix33 R0;
307  R0 << M[0],M[1],M[2],
308  M[4],M[5],M[6],
309  M[8],M[9],M[10];
310  if(isOrthogonalMatrix(R0) &&
311  (shapeInfo.primitiveType == OpenHRP::SP_BOX ||
312  shapeInfo.primitiveType == OpenHRP::SP_CYLINDER ||
313  shapeInfo.primitiveType == OpenHRP::SP_SPHERE ) )
314  continue;
315 
316  Matrix44 T;
317  T << M[0], M[1], M[2], M[3],
318  M[4], M[5], M[6], M[7],
319  M[8], M[9], M[10], M[11],
320  0.0, 0.0, 0.0, 1.0;
321  const FloatSequence& vertices = shapeInfo.vertices;
322  const LongSequence& triangles = shapeInfo.triangles;
323  const int numTriangles = triangles.length() / 3;
324 
325  for(int j=0; j < numTriangles; ++j){
326  int vertexIndexTop = vertexIndex;
327  for(int k=0; k < 3; ++k){
328  long orgVertexIndex = shapeInfo.triangles[j * 3 + k];
329  int p = orgVertexIndex * 3;
330  Vector4 v(T * Vector4(vertices[p+0], vertices[p+1], vertices[p+2], 1.0));
331  link->vertices[vertexIndex*3] = v[0];
332  link->vertices[vertexIndex*3+1] = v[1];
333  link->vertices[vertexIndex*3+2] = v[2];
334  vertexIndex++;
335  }
336  link->indices[triangleIndex++] = vertexIndexTop;
337  link->indices[triangleIndex++] = vertexIndexTop+1;
338  link->indices[triangleIndex++] = vertexIndexTop+2;
339  }
340  }
341 }
342 
343 void ModelLoaderHelper::createSensors(Link* link, const SensorInfoSequence& sensorInfoSeq, const Matrix33& Rs)
344 {
345  int numSensors = sensorInfoSeq.length();
346 
347  for(int i=0 ; i < numSensors ; ++i ) {
348  const SensorInfo& sensorInfo = sensorInfoSeq[i];
349 
350  int id = sensorInfo.id;
351  if(id < 0) {
352  cerr << "Warning: sensor ID is not given to sensor " << sensorInfo.name
353  << "of model " << body->modelName() << "." << endl;
354  } else {
355  int sensorType = Sensor::COMMON;
356 
357  CORBA::String_var type0 = sensorInfo.type;
358  string type(type0);
359 
360  if(type == "Force") { sensorType = Sensor::FORCE; }
361  else if(type == "RateGyro") { sensorType = Sensor::RATE_GYRO; }
362  else if(type == "Acceleration") { sensorType = Sensor::ACCELERATION; }
363  else if(type == "Vision") { sensorType = Sensor::VISION; }
364  else if(type == "Range") { sensorType = Sensor::RANGE; }
365 
366  CORBA::String_var name0 = sensorInfo.name;
367  string name(name0);
368 
369  Sensor* sensor = 0;
370  if(sensorType == Sensor::FORCE){
371  sensor = new ODE_ForceSensor();
372  }else
373  sensor = Sensor::create(sensorType);
374  if(sensor){
375  sensor->id = id;
376  sensor->link = link;
377  sensor->name = name;
378  body->addSensor(sensor, sensorType, id);
379 
380 
381  const DblArray3& p = sensorInfo.translation;
382  sensor->localPos = Rs * Vector3(p[0], p[1], p[2]);
383 
384  const Vector3 axis(sensorInfo.rotation[0], sensorInfo.rotation[1], sensorInfo.rotation[2]);
385  const Matrix33 R(rodrigues(axis, sensorInfo.rotation[3]));
386  sensor->localR = Rs * R;
387 
388  if(sensorType == Sensor::FORCE){
389  dJointSetFeedback(((ODE_Link*)link)->odeJointId, &((ODE_ForceSensor*)sensor)->feedback);
390  }
391  }
392 
393  if ( sensorType == Sensor::RANGE ) {
394  RangeSensor *range = dynamic_cast<RangeSensor *>(sensor);
395  range->scanAngle = sensorInfo.specValues[0];
396  range->scanStep = sensorInfo.specValues[1];
397  range->scanRate = sensorInfo.specValues[2];
398  range->maxDistance = sensorInfo.specValues[3];
399  }
400  }
401  }
402 }
403 
404 bool ODE_loadBodyFromBodyInfo(BodyPtr body, ODE_World* world, OpenHRP::BodyInfo_ptr bodyInfo)
405 {
406  if(!CORBA::is_nil(bodyInfo)){
408  return helper.createBody(body, world, bodyInfo);
409  }
410  return false;
411 }
dSpaceID getSpaceID()
Definition: ODE_World.h:113
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2332
* x
Definition: IceUtils.h:98
png_infop png_charpp name
Definition: png.h:2382
HRP_UTIL_EXPORT bool isOrthogonalMatrix(Matrix33 &m)
Definition: Eigen3d.cpp:152
void getMatrix33FromRowMajorArray(Matrix33 &m33, const Array &a, size_t top=0)
Definition: Eigen3d.h:118
png_uint_32 i
Definition: png.h:2735
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
png_infop png_uint_32 png_uint_32 * height
Definition: png.h:2309
double dReal
Definition: ColladaUtil.h:78
static Sensor * create(int type)
Matrix33 rodrigues(const Vector3 &axis, double q)
Definition: Eigen3d.h:27
list index
dWorldID getWorldID()
Definition: ODE_World.h:112
Eigen::Vector4d Vector4
Definition: Eigen4d.h:21
LinkInfoSequence_var linkInfoSeq
bool ODE_loadBodyFromBodyInfo(BodyPtr body, ODE_World *world, OpenHRP::BodyInfo_ptr bodyInfo)
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
bool createBody(BodyPtr body, ODE_World *worldId, BodyInfo_ptr bodyInfo)
* y
Definition: IceUtils.h:97
ShapeInfoSequence_var shapeInfoSeq
static int max(int a, int b)
static void createSensors(::World *world, Joint *jnt, SensorInfoSequence iSensors)


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