6 #ifndef CNOID_BODY_FORWARD_DYNAMICS_CBM_H 7 #define CNOID_BODY_FORWARD_DYNAMICS_CBM_H 10 #include <Eigen/StdVector> 11 #include <boost/dynamic_bitset.hpp> 12 #include <boost/shared_ptr.hpp> 18 class ForceSensorDevice;
35 void setHighGainModeForAllJoints();
36 void setHighGainMode(
int linkIndex,
bool on =
true);
38 virtual void initialize();
39 virtual void calcNextState();
41 void initializeAccelSolver();
42 void sumExternalForces();
43 void solveUnknownAccels();
44 void solveUnknownAccels(
const Vector3& fext,
const Vector3& tauext);
70 boost::dynamic_bitset<> highGainModeLinkFlag;
71 std::vector<DyLink*> torqueModeJoints;
72 std::vector<DyLink*> highGainModeJoints;
78 bool isNoUnknownAccelMode;
88 bool accelSolverInitialized;
109 struct ForceSensorInfo {
111 bool hasSensorsAbove;
116 hasSensorsAbove(false),
122 std::vector<ForceSensorInfo, Eigen::aligned_allocator<ForceSensorInfo> > forceSensorInfo;
128 std::vector<double> q0;
129 std::vector<double> dq0;
135 std::vector<double> dq;
136 std::vector<double> ddq;
138 virtual void initializeSensors();
140 void calcMotionWithEulerMethod();
141 void calcMotionWithRungeKuttaMethod();
142 void integrateRungeKuttaOneStep(
double r,
double dt);
143 void preserveHighGainModeJointState();
144 void calcPositionAndVelocityFK();
146 void setColumnOfMassMatrix(MatrixXd& M,
int column);
149 inline void calcAccelFKandForceSensorValues();
151 void updateForceSensorInfo(
DyLink* link,
bool hasSensorsAbove);
152 void updateForceSensors();
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