Choreonoid  1.1
ForwardDynamicsABM.h
[詳解]
1 
6 #ifndef CNOID_BODY_FORWARD_DYNAMICS_ABM_H_INCLUDED
7 #define CNOID_BODY_FORWARD_DYNAMICS_ABM_H_INCLUDED
8 
9 #include "ForwardDynamics.h"
10 #include <boost/intrusive_ptr.hpp>
11 #include <vector>
12 #include "exportdecl.h"
13 
14 namespace cnoid
15 {
16  class LinkTraverse;
17  class AccelSensor;
18  class ForceSensor;
19 
24 
25  public:
26 
29 
30  virtual void initialize();
31  virtual void calcNextState();
32 
33  private:
34 
35  void calcMotionWithEulerMethod();
36  void integrateRungeKuttaOneStep(double r, double dt);
37  void calcMotionWithRungeKuttaMethod();
38 
42  void calcABMPhase1();
43 
47  void calcABMPhase2();
48  void calcABMPhase2Part1();
49  void calcABMPhase2Part2();
50 
54  void calcABMPhase3();
55 
56  inline void calcABMFirstHalf();
57  inline void calcABMLastHalf();
58 
59  void updateForceSensors();
60  void updateForceSensor(ForceSensor* sensor);
61 
62  // Buffers for the Runge Kutta Method
63  Vector3 p0;
64  Matrix3 R0;
65  Vector3 vo0;
66  Vector3 w0;
67  std::vector<double> q0;
68  std::vector<double> dq0;
69 
70  Vector3 vo;
71  Vector3 w;
72  Vector3 dvo;
73  Vector3 dw;
74  std::vector<double> dq;
75  std::vector<double> ddq;
76 
77  };
78 
79 };
80 
81 #endif
boost::intrusive_ptr< Body > BodyPtr
Definition: Body.h:22
Definition: Sensor.h:60
Definition: ForwardDynamicsABM.h:23
Eigen::Vector3d Vector3
Definition: EigenTypes.h:26
#define CNOID_EXPORT
Definition: Util/exportdecl.h:13
bool initialize()
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:25
Definition: ForwardDynamics.h:23