6 #ifndef CNOID_BODY_FORWARD_DYNAMICS_MM_H_INCLUDED
7 #define CNOID_BODY_FORWARD_DYNAMICS_MM_H_INCLUDED
11 #include <boost/shared_ptr.hpp>
12 #include <boost/intrusive_ptr.hpp>
20 typedef boost::intrusive_ptr<Body>
BodyPtr;
41 virtual void calcNextState();
43 void initializeAccelSolver();
44 void solveUnknownAccels(
const Vector3& fext,
const Vector3& tauext);
70 std::vector<Link*> torqueModeJoints;
71 std::vector<Link*> highGainModeJoints;
77 bool isNoUnknownAccelMode;
87 bool accelSolverInitialized;
108 struct ForceSensorInfo {
110 bool hasSensorsAbove;
113 std::vector<ForceSensorInfo> forceSensorInfo;
120 std::vector<double> q0;
121 std::vector<double> dq0;
127 std::vector<double> dq;
128 std::vector<double> ddq;
130 virtual void initializeSensors();
132 void calcMotionWithEulerMethod();
133 void calcMotionWithRungeKuttaMethod();
134 void integrateRungeKuttaOneStep(
double r,
double dt);
135 void preserveHighGainModeJointState();
136 void calcPositionAndVelocityFK();
138 void setColumnOfMassMatrix(MatrixXd& M,
int column);
141 void sumExternalForces();
142 inline void solveUnknownAccels();
143 inline void calcAccelFKandForceSensorValues();
145 void updateForceSensorInfo(
Link* link,
bool hasSensorsAbove);