matrix-inertia.h
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
10 #ifndef __SOT_SOTMATRIXINERTIA_H__
11 #define __SOT_SOTMATRIXINERTIA_H__
12 
13 #include <ostream>
14 
15 #include "dynamic-graph/linear-algebra.h"
16 
17 #include <sot/core/matrix-geometry.hh>
18 
19 /* --------------------------------------------------------------------- */
20 /* --- API ------------------------------------------------------------- */
21 /* --------------------------------------------------------------------- */
22 
23 #if defined(WIN32)
24 #if defined(matrix_inertia_EXPORTS)
25 #define SOTMATRIXINERTIA_EXPORT __declspec(dllexport)
26 #else
27 #define SOTMATRIXINERTIA_EXPORT __declspec(dllimport)
28 #endif
29 #else
30 #define SOTMATRIXINERTIA_EXPORT
31 #endif
32 
33 namespace dynamicgraph {
34 namespace sot {
35 namespace dg = dynamicgraph;
36 
37 /* -------------------------------------------------------------------------- */
38 /* -------------------------------------------------------------------------- */
39 /* -------------------------------------------------------------------------- */
41  public:
42  private:
43  MatrixInertia(void) {}
44 
45  void initParents(void);
46  void initDofTable(void);
47 
48  public:
49  MatrixInertia(CjrlHumanoidDynamicRobot* aHDR);
50  ~MatrixInertia(void);
51  void init(CjrlHumanoidDynamicRobot* aHDR);
52 
53  public:
54  void update(void);
55  void computeInertiaMatrix();
56  void getInertiaMatrix(double* A);
57  const maal::boost::Matrix& getInertiaMatrix(void);
58  size_t getDoF() { return joints_.size(); }
59 
60  private:
61  CjrlHumanoidDynamicRobot* aHDR_;
62  dynamicsJRLJapan::HumanoidDynamicMultiBody* aHDMB_;
63  std::vector<CjrlJoint*> joints_;
64  std::vector<int> parentIndex_;
65 
66  std::vector<dynamicgraph::Matrix> Ic;
67  std::vector<dynamicgraph::Vector> phi;
68  std::vector<MatrixTwist> iVpi;
69  std::vector<MatrixForce> iVpiT;
70  dynamicgraph::Matrix inertia_;
71 };
72 
73 } /* namespace sot */
74 } /* namespace dynamicgraph */
75 
76 #endif // __SOT_SOTMATRIXINERTIA_H__
#define SOTMATRIXINERTIA_EXPORT
Definition: matrix-inertia.h:30
size_t getDoF()
Definition: matrix-inertia.h:58
Definition: matrix-inertia.h:40
Definition: angle-estimator.h:42