ColdetBody.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  * General Robotix Inc.
9  */
14 #include "ColdetBody.h"
15 #include <iostream>
16 
17 
18 ColdetBody::ColdetBody(BodyInfo_ptr bodyInfo)
19 {
20  LinkInfoSequence_var links = bodyInfo->links();
21  ShapeInfoSequence_var shapes = bodyInfo->shapes();
22 
23  int numLinks = links->length();
24 
25  linkColdetModels.resize(numLinks);
26 
27  for(int linkIndex = 0; linkIndex < numLinks ; ++linkIndex){
28 
29  LinkInfo& linkInfo = links[linkIndex];
30 
31  int totalNumTriangles = 0;
32  int totalNumVertices = 0;
33  const TransformedShapeIndexSequence& shapeIndices = linkInfo.shapeIndices;
34  short shapeIndex;
35  double R[9], p[3];
36  unsigned int nshape = shapeIndices.length();
37  for(unsigned int i=0; i < shapeIndices.length(); i++){
38  shapeIndex = shapeIndices[i].shapeIndex;
39  const DblArray12 &tform = shapeIndices[i].transformMatrix;
40  R[0] = tform[0]; R[1] = tform[1]; R[2] = tform[2]; p[0] = tform[3];
41  R[3] = tform[4]; R[4] = tform[5]; R[5] = tform[6]; p[1] = tform[7];
42  R[6] = tform[8]; R[7] = tform[9]; R[8] = tform[10]; p[2] = tform[11];
43  const ShapeInfo& shapeInfo = shapes[shapeIndex];
44  totalNumTriangles += shapeInfo.triangles.length() / 3;
45  totalNumVertices += shapeInfo.vertices.length() / 3;
46  }
47 
48  const SensorInfoSequence& sensors = linkInfo.sensors;
49  for (unsigned int i=0; i<sensors.length(); i++){
50  const SensorInfo &sinfo = sensors[i];
51  const TransformedShapeIndexSequence tsis = sinfo.shapeIndices;
52  nshape += tsis.length();
53  for (unsigned int j=0; j<tsis.length(); j++){
54  shapeIndex = tsis[j].shapeIndex;
55  const DblArray12 &tform = tsis[j].transformMatrix;
56  R[0] = tform[0]; R[1] = tform[1]; R[2] = tform[2]; p[0] = tform[3];
57  R[3] = tform[4]; R[4] = tform[5]; R[5] = tform[6]; p[1] = tform[7];
58  R[6] = tform[8]; R[7] = tform[9]; R[8] = tform[10]; p[2] = tform[11];
59  const ShapeInfo& shapeInfo = shapes[shapeIndex];
60  totalNumTriangles += shapeInfo.triangles.length() / 3;
61  totalNumVertices += shapeInfo.vertices.length() / 3 ;
62  }
63  }
64 
65  //int totalNumVertices = totalNumTriangles * 3;
66 
67  ColdetModelPtr coldetModel(new ColdetModel());
68  coldetModel->setName(std::string(linkInfo.name));
69 
70  if(totalNumTriangles > 0){
71  coldetModel->setNumVertices(totalNumVertices);
72  coldetModel->setNumTriangles(totalNumTriangles);
73  if (nshape == 1){
74  addLinkPrimitiveInfo(coldetModel, R, p, shapes[shapeIndex]);
75  }
76  addLinkVerticesAndTriangles(coldetModel, linkInfo, shapes);
77  coldetModel->build();
78  }
79 
80  linkColdetModels[linkIndex] = coldetModel;
81  linkNameToColdetModelMap.insert(make_pair(std::string (linkInfo.name),
82  coldetModel));
83 
84  cout << linkInfo.name << " has "<< totalNumTriangles << " triangles." << endl;
85  }
86 }
87 
89  const double *R, const double *p,
90  const ShapeInfo& shapeInfo)
91 {
92  switch(shapeInfo.primitiveType){
93  case SP_BOX:
94  coldetModel->setPrimitiveType(ColdetModel::SP_BOX);
95  break;
96  case SP_CYLINDER:
97  coldetModel->setPrimitiveType(ColdetModel::SP_CYLINDER);
98  break;
99  case SP_CONE:
100  coldetModel->setPrimitiveType(ColdetModel::SP_CONE);
101  break;
102  case SP_SPHERE:
103  coldetModel->setPrimitiveType(ColdetModel::SP_SPHERE);
104  break;
105  case SP_PLANE:
106  coldetModel->setPrimitiveType(ColdetModel::SP_PLANE);
107  break;
108  default:
109  break;
110  }
111  coldetModel->setNumPrimitiveParams(shapeInfo.primitiveParameters.length());
112  for (unsigned int i=0; i<shapeInfo.primitiveParameters.length(); i++){
113  coldetModel->setPrimitiveParam(i, shapeInfo.primitiveParameters[i]);
114  }
115  coldetModel->setPrimitivePosition(R, p);
116 }
117 
119 (ColdetModelPtr& coldetModel, const TransformedShapeIndex& tsi, const Matrix44& Tparent, ShapeInfoSequence_var& shapes, int& vertexIndex, int& triangleIndex)
120 {
121  short shapeIndex = tsi.shapeIndex;
122  const DblArray12& M = tsi.transformMatrix;;
123  Matrix44 T, Tlocal;
124  Tlocal << M[0], M[1], M[2], M[3],
125  M[4], M[5], M[6], M[7],
126  M[8], M[9], M[10], M[11],
127  0.0, 0.0, 0.0, 1.0;
128  T = Tparent * Tlocal;
129 
130  const ShapeInfo& shapeInfo = shapes[shapeIndex];
131  int vertexIndexBase = vertexIndex;
132  const FloatSequence& vertices = shapeInfo.vertices;
133  const int numVertices = vertices.length() / 3;
134  for(int j=0; j < numVertices; ++j){
135  Vector4 v(T * Vector4(vertices[j*3], vertices[j*3+1], vertices[j*3+2], 1.0));
136  coldetModel->setVertex(vertexIndex++, v[0], v[1], v[2]);
137  }
138 
139  const LongSequence& triangles = shapeInfo.triangles;
140  const int numTriangles = triangles.length() / 3;
141  for(int j=0; j < numTriangles; ++j){
142  int t0 = triangles[j*3] + vertexIndexBase;
143  int t1 = triangles[j*3+1] + vertexIndexBase;
144  int t2 = triangles[j*3+2] + vertexIndexBase;
145  coldetModel->setTriangle( triangleIndex++, t0, t1, t2);
146  }
147 
148 }
149 
151 (ColdetModelPtr& coldetModel, LinkInfo& linkInfo, ShapeInfoSequence_var& shapes)
152 {
153  int vertexIndex = 0;
154  int triangleIndex = 0;
155 
156  const TransformedShapeIndexSequence& shapeIndices = linkInfo.shapeIndices;
157 
158  Matrix44 E(Matrix44::Identity());
159  for(unsigned int i=0; i < shapeIndices.length(); i++){
160  addLinkVerticesAndTriangles(coldetModel, shapeIndices[i], E, shapes,
161  vertexIndex, triangleIndex);
162  }
163 
164  Matrix44 T(Matrix44::Identity());
165  const SensorInfoSequence& sensors = linkInfo.sensors;
166  for (unsigned int i=0; i<sensors.length(); i++){
167  const SensorInfo& sensor = sensors[i];
168  calcRodrigues(T, Vector3(sensor.rotation[0], sensor.rotation[1],
169  sensor.rotation[2]), sensor.rotation[3]);
170  T(0,3) = sensor.translation[0];
171  T(1,3) = sensor.translation[1];
172  T(2,3) = sensor.translation[2];
173  const TransformedShapeIndexSequence& shapeIndices = sensor.shapeIndices;
174  for (unsigned int j=0; j<shapeIndices.length(); j++){
175  addLinkVerticesAndTriangles(coldetModel, shapeIndices[j], T,
176  shapes,
177  vertexIndex, triangleIndex);
178  }
179  }
180 }
181 
182 
184 {
187 }
188 
189 
190 void ColdetBody::setLinkPositions(const LinkPositionSequence& linkPositions)
191 {
192  const int srcNumLinks = linkPositions.length();
193  const int selfNumLinks = linkColdetModels.size();
194  for(int i=0; i < srcNumLinks && i < selfNumLinks; ++i){
195  const LinkPosition& linkPosition = linkPositions[i];
196  if(linkColdetModels[i]){
197  linkColdetModels[i]->setPosition(linkPosition.R, linkPosition.p);
198  }
199  }
200 }
void setLinkPositions(const LinkPositionSequence &linkPositions)
Definition: ColdetBody.cpp:190
vector< ColdetModelPtr > linkColdetModels
Definition: ColdetBody.h:67
void addLinkVerticesAndTriangles(ColdetModelPtr &coldetModel, LinkInfo &linkInfo, ShapeInfoSequence_var &shapes)
Definition: ColdetBody.cpp:151
png_uint_32 i
Definition: png.h:2735
Eigen::Matrix4d Matrix44
Definition: Eigen4d.h:18
ColdetBody(BodyInfo_ptr bodyInfo)
Definition: ColdetBody.cpp:18
OpenHRP::vector3 Vector3
def j(str, encoding="cp932")
Eigen::Vector4d Vector4
Definition: Eigen4d.h:21
unsigned int numLinks() const
Definition: ColdetBody.h:44
void addLinkPrimitiveInfo(ColdetModelPtr &coldetModel, const double *R, const double *p, const ShapeInfo &shapeInfo)
Definition: ColdetBody.cpp:88
HRP_UTIL_EXPORT void calcRodrigues(Matrix33 &out_R, const Vector3 &axis, double q)
Definition: Eigen3d.cpp:22
boost::intrusive_ptr< ColdetModel > ColdetModelPtr
Definition: ColdetModel.h:268
map< string, ColdetModelPtr > linkNameToColdetModelMap
Definition: ColdetBody.h:68


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