Choreonoid  1.5
ForwardDynamicsCBM.h
Go to the documentation of this file.
1 
6 #ifndef CNOID_BODY_FORWARD_DYNAMICS_CBM_H
7 #define CNOID_BODY_FORWARD_DYNAMICS_CBM_H
8 
9 #include "ForwardDynamics.h"
10 #include <Eigen/StdVector>
11 #include <boost/dynamic_bitset.hpp>
12 #include <boost/shared_ptr.hpp>
13 #include "exportdecl.h"
14 
15 namespace cnoid
16 {
17 class DyLink;
18 class ForceSensorDevice;
19 
28 {
29 public:
31 
34 
35  void setHighGainModeForAllJoints();
36  void setHighGainMode(int linkIndex, bool on = true);
37 
38  virtual void initialize();
39  virtual void calcNextState();
40 
41  void initializeAccelSolver();
42  void sumExternalForces();
43  void solveUnknownAccels();
44  void solveUnknownAccels(const Vector3& fext, const Vector3& tauext);
45  void solveUnknownAccels(DyLink* 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  boost::dynamic_bitset<> highGainModeLinkFlag;
71  std::vector<DyLink*> torqueModeJoints;
72  std::vector<DyLink*> highGainModeJoints;
73 
74  //int rootDof; // dof of dv and dw (0 or 6)
75  int unknown_rootDof;
76  int given_rootDof;
77 
78  bool isNoUnknownAccelMode;
79 
80  VectorXd qGiven;
81  VectorXd dqGiven;
82  VectorXd ddqGiven;
83  Vector3 pGiven;
84  Matrix3 RGiven;
85  Vector3 voGiven;
86  Vector3 wGiven;
87 
88  bool accelSolverInitialized;
89  bool ddqGivenCopied;
90 
91  VectorXd qGivenPrev;
92  VectorXd dqGivenPrev;
93  Vector3 pGivenPrev;
94  Matrix3 RGivenPrev;
95  Vector3 voGivenPrev;
96  Vector3 wGivenPrev;
97 
98  Vector3 fextTotal;
99  Vector3 tauextTotal;
100 
101  Vector3 root_w_x_v;
102 
103  // buffers for the unit vector method
104  VectorXd ddqorg;
105  VectorXd uorg;
106  Vector3 dvoorg;
107  Vector3 dworg;
108 
109  struct ForceSensorInfo {
110  bool hasSensor;
111  bool hasSensorsAbove;
112  Vector3 f;
113  Vector3 tau;
114  ForceSensorInfo()
115  : hasSensor(false),
116  hasSensorsAbove(false),
117  f(Vector3::Zero()),
118  tau(Vector3::Zero())
119  { }
120  };
121 
122  std::vector<ForceSensorInfo, Eigen::aligned_allocator<ForceSensorInfo> > forceSensorInfo;
123 
124  // Buffers for the Runge Kutta Method
125  Position T0;
126  Vector3 vo0;
127  Vector3 w0;
128  std::vector<double> q0;
129  std::vector<double> dq0;
130 
131  Vector3 vo;
132  Vector3 w;
133  Vector3 dvo;
134  Vector3 dw;
135  std::vector<double> dq;
136  std::vector<double> ddq;
137 
138  virtual void initializeSensors();
139 
140  void calcMotionWithEulerMethod();
141  void calcMotionWithRungeKuttaMethod();
142  void integrateRungeKuttaOneStep(double r, double dt);
143  void preserveHighGainModeJointState();
144  void calcPositionAndVelocityFK();
145  void calcMassMatrix();
146  void setColumnOfMassMatrix(MatrixXd& M, int column);
147  void calcInverseDynamics(DyLink* link, Vector3& out_f, Vector3& out_tau);
148  void calcd1(DyLink* link, Vector3& out_f, Vector3& out_tau);
149  inline void calcAccelFKandForceSensorValues();
150  void calcAccelFKandForceSensorValues(DyLink* link, Vector3& out_f, Vector3& out_tau);
151  void updateForceSensorInfo(DyLink* link, bool hasSensorsAbove);
152  void updateForceSensors();
153 };
154 
155 typedef boost::shared_ptr<ForwardDynamicsCBM> ForwardDynamicsCBMPtr;
156 
157 };
158 
159 #endif
Definition: DyBody.h:115
Vector6 calcInverseDynamics(Link *link)
Definition: InverseDynamics.cpp:73
Definition: ForwardDynamicsCBM.h:27
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: ForwardDynamicsCBM.h:30
Eigen::Transform< double, 3, Eigen::AffineCompact > Position
Definition: EigenTypes.h:73
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
Eigen::Vector3d Vector3
Definition: EigenTypes.h:58
#define CNOID_EXPORT
Definition: Util/exportdecl.h:37
boost::shared_ptr< ForwardDynamicsCBM > ForwardDynamicsCBMPtr
Definition: ForwardDynamicsCBM.h:155
void calcMassMatrix(Body *body, const Vector3 &g, Eigen::MatrixXd &out_M)
Definition: MassMatrix.cpp:43
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:57
Definition: ForwardDynamics.h:24