Cheetah Software  1.0
DrawList.h
Go to the documentation of this file.
1 
12 #ifndef PROJECT_DRAWLIST_H
13 #define PROJECT_DRAWLIST_H
14 
15 #include "Checkerboard.h"
17 #include "Colors.h"
20 #include "Dynamics/spatial.h"
22 #include "cppTypes.h"
23 #include "obj_loader.h"
24 #include "sim_utilities.h"
25 
26 #include <QMatrix4x4>
27 
28 #include <stdlib.h>
29 #include <vector>
30 
31 class BoxInfo {
32  public:
33  double depth, width, height;
34  float frame[16]; // SE3
35  /*
36  T[0] T[4] T[8] T[12]
37  T[1] T[5] T[9] T[13]
38  T[2] T[6] T[10] T[14]
39  T[3] T[7] T[11] T[15] = (0, 0, 0, 1)
40  */
41 };
42 
43 struct SolidColor {
44  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
47 };
48 
49 class DrawList {
50  public:
52 
54  _cameraOrigin = Vec3<double>::Zero();
55  loadFiles();
56  }
57  size_t addCheetah3(Vec4<float> color, bool useOld);
58  size_t addMiniCheetah(Vec4<float> color, bool useOld);
59  void buildDrawList();
60  void loadFiles();
61  size_t addCheckerboard(Checkerboard &checkerBoard);
62  size_t addDebugSphere(float radius);
63  void addBox(double depth, double width, double height,
64  const Vec3<double> &pos, const Mat3<double> &ori,
65  bool transparent);
66  void addMesh(double grid_size, const Vec3<double> &left_corner,
67  const DMat<double> &height_map, bool transparent);
68 
72  void resize(size_t nUniqueObject, size_t nTotalObjects) {
73  _nUnique = nUniqueObject;
74  _nTotal = nTotalObjects;
75  _vertexData.resize(nUniqueObject);
76  _normalData.resize(nUniqueObject);
77  _colorData.resize(nUniqueObject);
78  _offsetXforms.resize(nUniqueObject);
79  _objectMap.resize(nTotalObjects);
80  }
81 
85  size_t getNumObjectsToDraw() { return _nTotal; }
86 
93  size_t getGLDrawArrayOffset(size_t i) {
94  return _glArrayOffsets.at(_objectMap.at(i));
95  }
96 
100  size_t getGLDrawArraySize(size_t i) {
101  return _glArraySizes.at(_objectMap.at(i));
102  }
103 
108  float *getVertexArray() { return _glVertexData.data(); }
109 
114  float *getNormalArray() { return _glNormalData.data(); }
115 
116  size_t getSizeOfAllData() { return _glVertexData.size(); }
117 
121  float *getColorArray() { return _glColorData.data(); }
122 
128  QMatrix4x4 &getModelBaseTransform(size_t i) { return _modelOffsets[i]; }
129 
134  QMatrix4x4 &getModelKinematicTransform(size_t i) {
135  return _kinematicXform[i];
136  }
137 
142  float getGLDataSizeMB() {
143  size_t bytes =
144  _glColorData.size() + _glNormalData.size() + _glVertexData.size();
145  bytes = bytes * sizeof(float);
146  return (float)bytes / float(1 << 20);
147  }
148 
153  bool needsReload() {
154  if (_reloadNeeded) {
155  _reloadNeeded = false;
156  return true;
157  }
158  return false;
159  }
160 
169  template <typename T>
171  bool updateOrigin = false) {
172  for (size_t modelID = 5, graphicsID = id; modelID < model.getNumBodies();
173  modelID++, graphicsID++) {
174  _kinematicXform.at(graphicsID) =
175  spatialTransformToQT(model.getModel()._Xa.at(modelID));
176  }
177 
178  if (updateOrigin) {
179  _cameraOrigin = model.getState().bodyPosition.template cast<T>();
180  }
181  }
182 
190  template <typename T>
192  if (_additionalInfoFirstVisit) {
193  _nTotalGC = model.getTotalNumGC();
194  _cp_touch.resize(_nTotalGC, false);
195  _cp_pos.resize(_nTotalGC);
196  _cp_force.resize(_nTotalGC);
197  std::vector<double> tmp(3);
198  for (size_t i(0); i < _nTotalGC; ++i) {
199  _cp_pos[i] = tmp;
200  _cp_force[i] = tmp;
201  }
202  _additionalInfoFirstVisit = false;
203  }
204 
205  for (size_t i(0); i < _nTotalGC; ++i) {
206  // TODO: check touch boolean
207  _cp_touch[i] = true;
208  for (size_t j(0); j < 3; ++j) {
209  _cp_pos[i][j] = model.getModel()._pGC[i][j];
210  _cp_force[i][j] = model.getContactForce(i)[j];
211  }
212  }
213  }
214 
221  template <typename T>
223  size_t id) {
224  // Mat4<T> H = sxformToHomogeneous(model.getLocation());
225  _kinematicXform.at(id) = spatialTransformToQT(model.getLocation());
226  }
227 
228  template <typename T>
229  void updateCheckerboard(T height, size_t id) {
230  QMatrix4x4 H;
231  H.setToIdentity();
232  H.translate(0., 0., height);
233  _kinematicXform.at(id) = H;
234  }
235 
236  template <typename T>
237  void updateDebugSphereLocation(Vec3<T> &position, size_t id) {
238  QMatrix4x4 H;
239  H.setToIdentity();
240  H.translate(position[0], position[1], position[2]);
241  _kinematicXform.at(id) = H;
242  }
243 
247  static void setSolidColor(std::vector<float> &data, size_t size, float r,
248  float g, float b) {
249  data.clear();
250  data.resize(size);
251 
252  if ((size % 3) != 0) {
253  throw std::runtime_error("setSolidColor invalid size");
254  }
255 
256  for (size_t i = 0; i < size / 3; i++) {
257  data[i * 3 + 0] = r;
258  data[i * 3 + 1] = g;
259  data[i * 3 + 2] = b;
260  }
261  }
262 
263  /* Get Functions */
264  const size_t &getTotalNumGC() { return _nTotalGC; }
265  const std::vector<double> &getGCPos(size_t idx) { return _cp_pos[idx]; }
266  const std::vector<double> &getGCForce(size_t idx) { return _cp_force[idx]; }
267  const std::vector<BoxInfo> &getBoxInfoList() { return _box_list; }
268 
269  const DMat<double> &getHeightMap() { return _height_map; }
271  return _height_map_left_corner;
272  }
273  const double &getHeightMapMax() { return _height_map_max; }
274  const double &getHeightMapMin() { return _height_map_min; }
275  const double &getGridSize() { return _grid_size; }
276 
277  const Vec3<double> &getCameraOrigin() { return _cameraOrigin; }
279  std::vector<QMatrix4x4> _kinematicXform;
280 
281  private:
282  size_t _nUnique = 0, _nTotal = 0;
283  std::vector<std::vector<float>> _vertexData;
284  std::vector<std::vector<float>> _normalData;
285  std::vector<std::vector<float>> _colorData;
287  _offsetXforms; // these are NOT coordinate transformations!
288  std::string _baseFileName = "../resources/";
289 
290  std::vector<size_t> _objectMap;
291 
292  std::vector<size_t> _glArrayOffsets;
293  std::vector<size_t> _glArraySizes;
294 
295  std::vector<float> _glVertexData;
296  std::vector<float> _glNormalData;
297  std::vector<float> _glColorData;
298 
299  std::vector<QMatrix4x4> _modelOffsets;
300 
301  bool _reloadNeeded = false;
302  bool _additionalInfoFirstVisit = true;
303 
304  size_t _nTotalGC = 0;
305  std::vector<bool> _cp_touch;
306  std::vector<std::vector<double>> _cp_pos;
307  std::vector<std::vector<double>> _cp_force;
308  std::vector<BoxInfo> _box_list;
309 
310  double _grid_size;
313  double _height_map_max, _height_map_min;
314 
316 
317  size_t _cheetah3LoadIndex = 0, _miniCheetahLoadIndex = 0,
318  _sphereLoadIndex = 0, _cubeLoadIndex = 0;
319 };
320 
321 #endif // PROJECT_DRAWLIST_H
VisualizationData * _visualizationData
Definition: DrawList.h:51
std::vector< size_t > _glArrayOffsets
Definition: DrawList.h:292
Utility functions that exist only in the simulator.
std::vector< std::vector< float > > _normalData
Definition: DrawList.h:284
double height
Definition: DrawList.h:33
float getGLDataSizeMB()
Definition: DrawList.h:142
double depth
Definition: DrawList.h:33
std::vector< std::vector< float > > _colorData
Definition: DrawList.h:285
typename Eigen::Matrix< T, 3, 3 > Mat3
Definition: cppTypes.h:54
typename Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > DMat
Definition: cppTypes.h:106
float * getNormalArray()
Definition: DrawList.h:114
std::vector< bool > _cp_touch
Definition: DrawList.h:305
std::vector< size_t > _glArraySizes
Definition: DrawList.h:293
static void setSolidColor(std::vector< float > &data, size_t size, float r, float g, float b)
Definition: DrawList.h:247
typename Eigen::Matrix< T, 3, 1 > Vec3
Definition: cppTypes.h:26
const Vec3< double > & getCameraOrigin()
Definition: DrawList.h:277
const Vec3< T > & getContactForce(size_t idx)
void updateRobotFromModel(DynamicsSimulator< T > &model, size_t id, bool updateOrigin=false)
Definition: DrawList.h:170
DrawList()
Definition: DrawList.h:53
std::vector< std::vector< double > > _cp_force
Definition: DrawList.h:307
const std::vector< double > & getGCPos(size_t idx)
Definition: DrawList.h:265
float * getColorArray()
Definition: DrawList.h:121
vectorAligned< SolidColor > _instanceColor
Definition: DrawList.h:278
const Vec3< double > & getHeightMapLeftCorner()
Definition: DrawList.h:270
const size_t & getTotalNumGC()
std::vector< BoxInfo > _box_list
Definition: DrawList.h:308
std::vector< QMatrix4x4 > _kinematicXform
Definition: DrawList.h:279
void resize(size_t nUniqueObject, size_t nTotalObjects)
Definition: DrawList.h:72
Vec3< double > _height_map_left_corner
Definition: DrawList.h:311
Collision logic for an infinite plane.
size_t getSizeOfAllData()
Definition: DrawList.h:116
std::vector< float > _glColorData
Definition: DrawList.h:297
size_t getNumObjectsToDraw()
Definition: DrawList.h:85
Vec3< double > _cameraOrigin
Definition: DrawList.h:315
QMatrix4x4 & getModelKinematicTransform(size_t i)
Definition: DrawList.h:134
const double & getHeightMapMax()
Definition: DrawList.h:273
float * getVertexArray()
Definition: DrawList.h:108
const size_t & getTotalNumGC()
Definition: DrawList.h:264
vectorAligned< Mat4< float > > _offsetXforms
Definition: DrawList.h:287
typename Eigen::Matrix< T, 4, 1 > Vec4
Definition: cppTypes.h:30
DMat< double > _height_map
Definition: DrawList.h:312
float frame[16]
Definition: DrawList.h:34
const FBModelState< T > & getState() const
void updateCheckerboard(T height, size_t id)
Definition: DrawList.h:229
double _height_map_min
Definition: DrawList.h:313
std::vector< float > _glNormalData
Definition: DrawList.h:296
size_t getGLDrawArraySize(size_t i)
Definition: DrawList.h:100
typename std::vector< T, Eigen::aligned_allocator< T >> vectorAligned
Definition: cppTypes.h:118
std::vector< size_t > _objectMap
Definition: DrawList.h:290
bool needsReload()
Definition: DrawList.h:153
Utility to load .obj files, containing 3D models of robots.
void updateDebugSphereLocation(Vec3< T > &position, size_t id)
Definition: DrawList.h:237
double width
Definition: DrawList.h:33
const std::vector< BoxInfo > & getBoxInfoList()
Definition: DrawList.h:267
double _grid_size
Definition: DrawList.h:310
size_t getGLDrawArrayOffset(size_t i)
Definition: DrawList.h:93
Rigid Body Dynamics Simulator with Collisions.
const double & getGridSize()
Definition: DrawList.h:275
void updateAdditionalInfo(DynamicsSimulator< T > &model)
Definition: DrawList.h:191
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Vec4< float > rgba
Definition: DrawList.h:45
std::vector< float > _glVertexData
Definition: DrawList.h:295
bool useSolidColor
Definition: DrawList.h:46
std::vector< std::vector< double > > _cp_pos
Definition: DrawList.h:306
QMatrix4x4 & getModelBaseTransform(size_t i)
Definition: DrawList.h:128
const FloatingBaseModel< T > & getModel()
Some colors commonly used in the simulator.
Implementation of Rigid Body Floating Base model data structure.
void updateCheckerboardFromCollisionPlane(CollisionPlane< T > &model, size_t id)
Definition: DrawList.h:222
const std::vector< double > & getGCForce(size_t idx)
Definition: DrawList.h:266
const DMat< double > & getHeightMap()
Definition: DrawList.h:269
const double & getHeightMapMin()
Definition: DrawList.h:274
Utility functions for manipulating spatial quantities.
std::vector< QMatrix4x4 > _modelOffsets
Definition: DrawList.h:299
QMatrix4x4 spatialTransformToQT(const Eigen::MatrixBase< T > &X)
Definition: sim_utilities.h:23
std::vector< std::vector< float > > _vertexData
Definition: DrawList.h:283