Choreonoid
1.5
|
#include <ForwardDynamicsCBM.h>
Public Member Functions | |
ForwardDynamicsCBM (DyBody *body) | |
~ForwardDynamicsCBM () | |
void | setHighGainModeForAllJoints () |
void | setHighGainMode (int linkIndex, bool on=true) |
virtual void | initialize () |
virtual void | calcNextState () |
void | initializeAccelSolver () |
void | sumExternalForces () |
void | solveUnknownAccels () |
void | solveUnknownAccels (const Vector3 &fext, const Vector3 &tauext) |
void | solveUnknownAccels (DyLink *link, const Vector3 &fext, const Vector3 &tauext, const Vector3 &rootfext, const Vector3 &roottauext) |
![]() | |
ForwardDynamics (DyBody *body) | |
virtual | ~ForwardDynamics () |
void | setGravityAcceleration (const Vector3 &g) |
void | setEulerMethod () |
void | setRungeKuttaMethod () |
void | setTimeStep (double timeStep) |
void | enableSensors (bool on) |
void | setOldAccelSensorCalcMode (bool on) |
Public Attributes | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | |
![]() | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | |
Additional Inherited Members | |
![]() | |
enum | { EULER_METHOD, RUNGEKUTTA_METHOD } |
![]() | |
static void | SE3exp (Position &out_T, const Position &T0, const Vector3 &w, const Vector3 &vo, double dt) |
update position/orientation using spatial velocity More... | |
![]() | |
DyBodyPtr | body |
Vector3 | g |
double | timeStep |
bool | sensorsEnabled |
BasicSensorSimulationHelper | sensorHelper |
enum cnoid::ForwardDynamics:: { ... } | integrationMode |
The ForwardDynamicsCBM class calculates the forward dynamics using the motion equation based on the generalized mass matrix. The class is mainly used for a body that has high-gain mode joints. If all the joints of a body are the torque mode, the ForwardDynamicsABM, which uses the articulated body method, is more efficient.
ForwardDynamicsCBM::ForwardDynamicsCBM | ( | DyBody * | body | ) |
ForwardDynamicsCBM::~ForwardDynamicsCBM | ( | ) |
|
virtual |
Implements cnoid::ForwardDynamics.
|
virtual |
Implements cnoid::ForwardDynamics.
void ForwardDynamicsCBM::initializeAccelSolver | ( | ) |
void ForwardDynamicsCBM::setHighGainMode | ( | int | linkIndex, |
bool | on = true |
||
) |
void ForwardDynamicsCBM::setHighGainModeForAllJoints | ( | ) |
void ForwardDynamicsCBM::solveUnknownAccels | ( | ) |
void ForwardDynamicsCBM::solveUnknownAccels | ( | DyLink * | link, |
const Vector3 & | fext, | ||
const Vector3 & | tauext, | ||
const Vector3 & | rootfext, | ||
const Vector3 & | roottauext | ||
) |
void ForwardDynamicsCBM::sumExternalForces | ( | ) |
cnoid::ForwardDynamicsCBM::EIGEN_MAKE_ALIGNED_OPERATOR_NEW |