Choreonoid  1.1
ForwardDynamics.h
説明を見る。
1 
6 #ifndef CNOID_BODY_FORWARD_DYNAMICS_H_INCLUDED
7 #define CNOID_BODY_FORWARD_DYNAMICS_H_INCLUDED
8 
9 #include "Body.h"
10 #include <boost/shared_ptr.hpp>
11 #include <boost/intrusive_ptr.hpp>
12 #include "exportdecl.h"
13 
14 namespace cnoid
15 {
16  class AccelSensor;
17 
24 
25  public:
26  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27 
29  virtual ~ForwardDynamics();
30 
31  void setGravityAcceleration(const Vector3& g);
32  void setEulerMethod();
33  void setRungeKuttaMethod();
34  void setTimeStep(double timeStep);
35  void enableSensors(bool on);
36 
37  virtual void initialize() = 0;
38  virtual void calcNextState() = 0;
39 
40  protected:
41 
42  virtual void initializeSensors();
43  virtual void updateSensorsFinal();
44 
55  static void SE3exp(Vector3& out_p, Matrix3& out_R,
56  const Vector3& p0, const Matrix3& R0,
57  const Vector3& w, const Vector3& vo, double dt);
58 
61  double timeStep;
63 
64  enum { EULER_METHOD, RUNGEKUTTA_METHOD } integrationMode;
65 
66  private:
67 
68  void updateAccelSensor(AccelSensor* sensor);
69  void initializeAccelSensors();
70 
71  // varialbes for calculating sensor values
72  // preview control gain matrices for force sensors
73  Matrix2 A;
74  Vector2 B;
75  };
76 
77  typedef boost::shared_ptr<ForwardDynamics> ForwardDynamicsPtr;
78 
79 };
80 
81 #endif