Choreonoid  1.1
ForwardDynamicsCBM.h
[詳解]
1 
6 #ifndef CNOID_BODY_FORWARD_DYNAMICS_MM_H_INCLUDED
7 #define CNOID_BODY_FORWARD_DYNAMICS_MM_H_INCLUDED
8 
9 #include "ForwardDynamics.h"
10 #include <vector>
11 #include <boost/shared_ptr.hpp>
12 #include <boost/intrusive_ptr.hpp>
13 #include "exportdecl.h"
14 
15 namespace cnoid
16 {
17  class Link;
18 
19  class Body;
20  typedef boost::intrusive_ptr<Body> BodyPtr;
21 
22  class LinkTraverse;
23  class AccelSensor;
24  class ForceSensor;
25 
34 
35  public:
36 
37  ForwardDynamicsMM(BodyPtr body);
39 
40  virtual void initialize();
41  virtual void calcNextState();
42 
43  void initializeAccelSolver();
44  void solveUnknownAccels(const Vector3& fext, const Vector3& tauext);
45  void solveUnknownAccels(Link* link, const Vector3& fext, const Vector3& tauext, const Vector3& rootfext, const Vector3& roottauext);
46 
47  private:
48 
49  /*
50  Elements of the motion equation
51 
52  | | | | dv | | b1 | | 0 | | totalfext |
53  | M11 | M12 | | dw | | | | 0 | | totaltauext |
54  | | | * |ddq (unkown)| + | | + | d1 | = | u (known) |
55  |-----+-----| |------------| |----| |----| |----------------|
56  | M21 | M22 | | given ddq | | b2 | | d2 | | u2 |
57 
58  |fext |
59  d1 = trans(s)| |
60  |tauext|
61 
62  */
63 
64  MatrixXd M11;
65  MatrixXd M12;
66  MatrixXd b1;
67  VectorXd d1;
68  VectorXd c1;
69 
70  std::vector<Link*> torqueModeJoints;
71  std::vector<Link*> highGainModeJoints;
72 
73  //int rootDof; // dof of dv and dw (0 or 6)
74  int unknown_rootDof;
75  int given_rootDof;
76 
77  bool isNoUnknownAccelMode;
78 
79  VectorXd qGiven;
80  VectorXd dqGiven;
81  VectorXd ddqGiven;
82  Vector3 pGiven;
83  Matrix3 RGiven;
84  Vector3 voGiven;
85  Vector3 wGiven;
86 
87  bool accelSolverInitialized;
88  bool ddqGivenCopied;
89 
90  VectorXd qGivenPrev;
91  VectorXd dqGivenPrev;
92  Vector3 pGivenPrev;
93  Matrix3 RGivenPrev;
94  Vector3 voGivenPrev;
95  Vector3 wGivenPrev;
96 
97  Vector3 fextTotal;
98  Vector3 tauextTotal;
99 
100  Vector3 root_w_x_v;
101 
102  // buffers for the unit vector method
103  VectorXd ddqorg;
104  VectorXd uorg;
105  Vector3 dvoorg;
106  Vector3 dworg;
107 
108  struct ForceSensorInfo {
109  ForceSensor* sensor;
110  bool hasSensorsAbove;
111  };
112 
113  std::vector<ForceSensorInfo> forceSensorInfo;
114 
115  // Buffers for the Runge Kutta Method
116  Vector3 p0;
117  Matrix3 R0;
118  Vector3 vo0;
119  Vector3 w0;
120  std::vector<double> q0;
121  std::vector<double> dq0;
122 
123  Vector3 vo;
124  Vector3 w;
125  Vector3 dvo;
126  Vector3 dw;
127  std::vector<double> dq;
128  std::vector<double> ddq;
129 
130  virtual void initializeSensors();
131 
132  void calcMotionWithEulerMethod();
133  void calcMotionWithRungeKuttaMethod();
134  void integrateRungeKuttaOneStep(double r, double dt);
135  void preserveHighGainModeJointState();
136  void calcPositionAndVelocityFK();
137  void calcMassMatrix();
138  void setColumnOfMassMatrix(MatrixXd& M, int column);
139  void calcInverseDynamics(Link* link, Vector3& out_f, Vector3& out_tau);
140  void calcd1(Link* link, Vector3& out_f, Vector3& out_tau);
141  void sumExternalForces();
142  inline void solveUnknownAccels();
143  inline void calcAccelFKandForceSensorValues();
144  void calcAccelFKandForceSensorValues(Link* link, Vector3& out_f, Vector3& out_tau);
145  void updateForceSensorInfo(Link* link, bool hasSensorsAbove);
146  };
147 
148  typedef boost::shared_ptr<ForwardDynamicsMM> ForwardDynamicsMMPtr;
149 
150 };
151 
152 #endif
boost::shared_ptr< ForwardDynamicsMM > ForwardDynamicsMMPtr
Definition: ForwardDynamicsCBM.h:148
boost::intrusive_ptr< Body > BodyPtr
Definition: Body.h:22
Definition: ForwardDynamicsCBM.h:33
Definition: Sensor.h:60
void calcInverseDynamics(const BodyPtr &body, Link *ptr, Vector3 &out_f, Vector3 &out_tau)
Definition: InverseDynamics.cpp:17
Definition: EasyScanner.h:16
Eigen::Vector3d Vector3
Definition: EigenTypes.h:26
#define CNOID_EXPORT
Definition: Util/exportdecl.h:13
void calcMassMatrix(const BodyPtr &body, Eigen::MatrixXd &out_M)
Definition: MassMatrix.cpp:44
bool initialize()
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:25
Definition: ForwardDynamics.h:23