Classes | Public Member Functions | Private Member Functions | Private Attributes | List of all members
hrp::ForwardDynamicsMM Class Reference

#include <ForwardDynamicsCBM.h>

Inheritance diagram for hrp::ForwardDynamicsMM:
Inheritance graph
[legend]

Classes

struct  ForceSensorInfo
 

Public Member Functions

virtual void calcNextState ()
 
 ForwardDynamicsMM (BodyPtr body)
 
virtual void initialize ()
 
void initializeAccelSolver ()
 
void solveUnknownAccels (const Vector3 &fext, const Vector3 &tauext)
 
void solveUnknownAccels (Link *link, const Vector3 &fext, const Vector3 &tauext, const Vector3 &rootfext, const Vector3 &roottauext)
 
void solveUnknownAccels ()
 
void sumExternalForces ()
 
 ~ForwardDynamicsMM ()
 
- Public Member Functions inherited from hrp::ForwardDynamics
void enableSensors (bool on)
 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW ForwardDynamics (BodyPtr body)
 
void setEulerMethod ()
 
void setGravityAcceleration (const Vector3 &g)
 
void setRungeKuttaMethod ()
 
void setTimeStep (double timeStep)
 
virtual ~ForwardDynamics ()
 

Private Member Functions

void calcAccelFKandForceSensorValues ()
 
void calcAccelFKandForceSensorValues (Link *link, Vector3 &out_f, Vector3 &out_tau)
 
void calcd1 (Link *link, Vector3 &out_f, Vector3 &out_tau)
 
void calcInverseDynamics (Link *link, Vector3 &out_f, Vector3 &out_tau)
 
void calcMassMatrix ()
 
void calcMotionWithEulerMethod ()
 
void calcMotionWithRungeKuttaMethod ()
 
void calcPositionAndVelocityFK ()
 
virtual void initializeSensors ()
 
void integrateRungeKuttaOneStep (double r, double dt)
 
void preserveHighGainModeJointState ()
 
void setColumnOfMassMatrix (dmatrix &M, int column)
 
void updateForceSensorInfo (Link *link, bool hasSensorsAbove)
 

Private Attributes

bool accelSolverInitialized
 
dmatrix b1
 
dvector c1
 
dmatrix d1
 
std::vector< double > ddq
 
dvector ddqGiven
 
bool ddqGivenCopied
 
dvector ddqorg
 
std::vector< double > dq
 
std::vector< double > dq0
 
dvector dqGiven
 
dvector dqGivenPrev
 
Vector3 dvo
 
Vector3 dvoorg
 
Vector3 dw
 
Vector3 dworg
 
Vector3 fextTotal
 
std::vector< ForceSensorInfo > forceSensorInfo
 
int given_rootDof
 
std::vector< Link * > highGainModeJoints
 
bool isNoUnknownAccelMode
 
dmatrix M11
 
dmatrix M12
 
Vector3 p0
 
Vector3 pGiven
 
Vector3 pGivenPrev
 
std::vector< double > q0
 
dvector qGiven
 
dvector qGivenPrev
 
Matrix33 R0
 
Matrix33 RGiven
 
Matrix33 RGivenPrev
 
Vector3 root_w_x_v
 
Vector3 tauextTotal
 
std::vector< Link * > torqueModeJoints
 
int unknown_rootDof
 
dvector uorg
 
Vector3 vo
 
Vector3 vo0
 
Vector3 voGiven
 
Vector3 voGivenPrev
 
Vector3 w
 
Vector3 w0
 
Vector3 wGiven
 
Vector3 wGivenPrev
 

Additional Inherited Members

- Protected Types inherited from hrp::ForwardDynamics
enum  { EULER_METHOD, RUNGEKUTTA_METHOD }
 
- Protected Member Functions inherited from hrp::ForwardDynamics
virtual void updateSensorsFinal ()
 
- Static Protected Member Functions inherited from hrp::ForwardDynamics
static void SE3exp (Vector3 &out_p, Matrix33 &out_R, const Vector3 &p0, const Matrix33 &R0, const Vector3 &w, const Vector3 &vo, double dt)
 update position/orientation using spatial velocity More...
 
- Protected Attributes inherited from hrp::ForwardDynamics
BodyPtr body
 
Vector3 g
 
enum hrp::ForwardDynamics:: { ... }  integrationMode
 
bool sensorsEnabled
 
double timeStep
 

Detailed Description

The ForwardDynamicsMM 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.

Definition at line 42 of file ForwardDynamicsCBM.h.

Constructor & Destructor Documentation

◆ ForwardDynamicsMM()

ForwardDynamicsMM::ForwardDynamicsMM ( BodyPtr  body)

Definition at line 54 of file ForwardDynamicsCBM.cpp.

◆ ~ForwardDynamicsMM()

ForwardDynamicsMM::~ForwardDynamicsMM ( )

Definition at line 61 of file ForwardDynamicsCBM.cpp.

Member Function Documentation

◆ calcAccelFKandForceSensorValues() [1/2]

void ForwardDynamicsMM::calcAccelFKandForceSensorValues ( )
inlineprivate

Definition at line 142 of file ForwardDynamicsCBM.cpp.

◆ calcAccelFKandForceSensorValues() [2/2]

void ForwardDynamicsMM::calcAccelFKandForceSensorValues ( Link *  link,
Vector3 &  out_f,
Vector3 &  out_tau 
)
private

Definition at line 729 of file ForwardDynamicsCBM.cpp.

◆ calcd1()

void ForwardDynamicsMM::calcd1 ( Link *  link,
Vector3 &  out_f,
Vector3 &  out_tau 
)
private

Definition at line 601 of file ForwardDynamicsCBM.cpp.

◆ calcInverseDynamics()

void ForwardDynamicsMM::calcInverseDynamics ( Link *  link,
Vector3 &  out_f,
Vector3 &  out_tau 
)
private

Definition at line 552 of file ForwardDynamicsCBM.cpp.

◆ calcMassMatrix()

void ForwardDynamicsMM::calcMassMatrix ( )
private

calculate the mass matrix using the unit vector method

Todo:
replace the unit vector method here with a more efficient method that only requires O(n) computation time

Definition at line 446 of file ForwardDynamicsCBM.cpp.

◆ calcMotionWithEulerMethod()

void ForwardDynamicsMM::calcMotionWithEulerMethod ( )
private

Definition at line 189 of file ForwardDynamicsCBM.cpp.

◆ calcMotionWithRungeKuttaMethod()

void ForwardDynamicsMM::calcMotionWithRungeKuttaMethod ( )
private

Definition at line 220 of file ForwardDynamicsCBM.cpp.

◆ calcNextState()

void ForwardDynamicsMM::calcNextState ( )
virtual

Implements hrp::ForwardDynamics.

Definition at line 149 of file ForwardDynamicsCBM.cpp.

◆ calcPositionAndVelocityFK()

void ForwardDynamicsMM::calcPositionAndVelocityFK ( )
private
Todo:
remove this equation

Definition at line 363 of file ForwardDynamicsCBM.cpp.

◆ initialize()

void ForwardDynamicsMM::initialize ( void  )
virtual

Implements hrp::ForwardDynamics.

Definition at line 67 of file ForwardDynamicsCBM.cpp.

◆ initializeAccelSolver()

void ForwardDynamicsMM::initializeAccelSolver ( )

Definition at line 625 of file ForwardDynamicsCBM.cpp.

◆ initializeSensors()

void ForwardDynamicsMM::initializeSensors ( )
privatevirtual

Reimplemented from hrp::ForwardDynamics.

Definition at line 780 of file ForwardDynamicsCBM.cpp.

◆ integrateRungeKuttaOneStep()

void ForwardDynamicsMM::integrateRungeKuttaOneStep ( double  r,
double  dt 
)
private

Definition at line 320 of file ForwardDynamicsCBM.cpp.

◆ preserveHighGainModeJointState()

void ForwardDynamicsMM::preserveHighGainModeJointState ( )
private

Definition at line 346 of file ForwardDynamicsCBM.cpp.

◆ setColumnOfMassMatrix()

void ForwardDynamicsMM::setColumnOfMassMatrix ( dmatrix &  M,
int  column 
)
private

Definition at line 534 of file ForwardDynamicsCBM.cpp.

◆ solveUnknownAccels() [1/3]

void ForwardDynamicsMM::solveUnknownAccels ( const Vector3 &  fext,
const Vector3 &  tauext 
)

Definition at line 696 of file ForwardDynamicsCBM.cpp.

◆ solveUnknownAccels() [2/3]

void ForwardDynamicsMM::solveUnknownAccels ( Link *  link,
const Vector3 &  fext,
const Vector3 &  tauext,
const Vector3 &  rootfext,
const Vector3 &  roottauext 
)

Definition at line 667 of file ForwardDynamicsCBM.cpp.

◆ solveUnknownAccels() [3/3]

void ForwardDynamicsMM::solveUnknownAccels ( )

Definition at line 133 of file ForwardDynamicsCBM.cpp.

◆ sumExternalForces()

void ForwardDynamicsMM::sumExternalForces ( )

Definition at line 586 of file ForwardDynamicsCBM.cpp.

◆ updateForceSensorInfo()

void ForwardDynamicsMM::updateForceSensorInfo ( Link *  link,
bool  hasSensorsAbove 
)
private

Definition at line 806 of file ForwardDynamicsCBM.cpp.

Member Data Documentation

◆ accelSolverInitialized

bool hrp::ForwardDynamicsMM::accelSolverInitialized
private

Definition at line 98 of file ForwardDynamicsCBM.h.

◆ b1

dmatrix hrp::ForwardDynamicsMM::b1
private

Definition at line 77 of file ForwardDynamicsCBM.h.

◆ c1

dvector hrp::ForwardDynamicsMM::c1
private

Definition at line 79 of file ForwardDynamicsCBM.h.

◆ d1

dmatrix hrp::ForwardDynamicsMM::d1
private

Definition at line 78 of file ForwardDynamicsCBM.h.

◆ ddq

std::vector<double> hrp::ForwardDynamicsMM::ddq
private

Definition at line 139 of file ForwardDynamicsCBM.h.

◆ ddqGiven

dvector hrp::ForwardDynamicsMM::ddqGiven
private

Definition at line 92 of file ForwardDynamicsCBM.h.

◆ ddqGivenCopied

bool hrp::ForwardDynamicsMM::ddqGivenCopied
private

Definition at line 99 of file ForwardDynamicsCBM.h.

◆ ddqorg

dvector hrp::ForwardDynamicsMM::ddqorg
private

Definition at line 114 of file ForwardDynamicsCBM.h.

◆ dq

std::vector<double> hrp::ForwardDynamicsMM::dq
private

Definition at line 138 of file ForwardDynamicsCBM.h.

◆ dq0

std::vector<double> hrp::ForwardDynamicsMM::dq0
private

Definition at line 132 of file ForwardDynamicsCBM.h.

◆ dqGiven

dvector hrp::ForwardDynamicsMM::dqGiven
private

Definition at line 91 of file ForwardDynamicsCBM.h.

◆ dqGivenPrev

dvector hrp::ForwardDynamicsMM::dqGivenPrev
private

Definition at line 102 of file ForwardDynamicsCBM.h.

◆ dvo

Vector3 hrp::ForwardDynamicsMM::dvo
private

Definition at line 136 of file ForwardDynamicsCBM.h.

◆ dvoorg

Vector3 hrp::ForwardDynamicsMM::dvoorg
private

Definition at line 116 of file ForwardDynamicsCBM.h.

◆ dw

Vector3 hrp::ForwardDynamicsMM::dw
private

Definition at line 137 of file ForwardDynamicsCBM.h.

◆ dworg

Vector3 hrp::ForwardDynamicsMM::dworg
private

Definition at line 117 of file ForwardDynamicsCBM.h.

◆ fextTotal

Vector3 hrp::ForwardDynamicsMM::fextTotal
private

Definition at line 108 of file ForwardDynamicsCBM.h.

◆ forceSensorInfo

std::vector<ForceSensorInfo> hrp::ForwardDynamicsMM::forceSensorInfo
private

Definition at line 124 of file ForwardDynamicsCBM.h.

◆ given_rootDof

int hrp::ForwardDynamicsMM::given_rootDof
private

Definition at line 86 of file ForwardDynamicsCBM.h.

◆ highGainModeJoints

std::vector<Link*> hrp::ForwardDynamicsMM::highGainModeJoints
private

Definition at line 82 of file ForwardDynamicsCBM.h.

◆ isNoUnknownAccelMode

bool hrp::ForwardDynamicsMM::isNoUnknownAccelMode
private

Definition at line 88 of file ForwardDynamicsCBM.h.

◆ M11

dmatrix hrp::ForwardDynamicsMM::M11
private

Definition at line 75 of file ForwardDynamicsCBM.h.

◆ M12

dmatrix hrp::ForwardDynamicsMM::M12
private

Definition at line 76 of file ForwardDynamicsCBM.h.

◆ p0

Vector3 hrp::ForwardDynamicsMM::p0
private

Definition at line 127 of file ForwardDynamicsCBM.h.

◆ pGiven

Vector3 hrp::ForwardDynamicsMM::pGiven
private

Definition at line 93 of file ForwardDynamicsCBM.h.

◆ pGivenPrev

Vector3 hrp::ForwardDynamicsMM::pGivenPrev
private

Definition at line 103 of file ForwardDynamicsCBM.h.

◆ q0

std::vector<double> hrp::ForwardDynamicsMM::q0
private

Definition at line 131 of file ForwardDynamicsCBM.h.

◆ qGiven

dvector hrp::ForwardDynamicsMM::qGiven
private

Definition at line 90 of file ForwardDynamicsCBM.h.

◆ qGivenPrev

dvector hrp::ForwardDynamicsMM::qGivenPrev
private

Definition at line 101 of file ForwardDynamicsCBM.h.

◆ R0

Matrix33 hrp::ForwardDynamicsMM::R0
private

Definition at line 128 of file ForwardDynamicsCBM.h.

◆ RGiven

Matrix33 hrp::ForwardDynamicsMM::RGiven
private

Definition at line 94 of file ForwardDynamicsCBM.h.

◆ RGivenPrev

Matrix33 hrp::ForwardDynamicsMM::RGivenPrev
private

Definition at line 104 of file ForwardDynamicsCBM.h.

◆ root_w_x_v

Vector3 hrp::ForwardDynamicsMM::root_w_x_v
private

Definition at line 111 of file ForwardDynamicsCBM.h.

◆ tauextTotal

Vector3 hrp::ForwardDynamicsMM::tauextTotal
private

Definition at line 109 of file ForwardDynamicsCBM.h.

◆ torqueModeJoints

std::vector<Link*> hrp::ForwardDynamicsMM::torqueModeJoints
private

Definition at line 81 of file ForwardDynamicsCBM.h.

◆ unknown_rootDof

int hrp::ForwardDynamicsMM::unknown_rootDof
private

Definition at line 85 of file ForwardDynamicsCBM.h.

◆ uorg

dvector hrp::ForwardDynamicsMM::uorg
private

Definition at line 115 of file ForwardDynamicsCBM.h.

◆ vo

Vector3 hrp::ForwardDynamicsMM::vo
private

Definition at line 134 of file ForwardDynamicsCBM.h.

◆ vo0

Vector3 hrp::ForwardDynamicsMM::vo0
private

Definition at line 129 of file ForwardDynamicsCBM.h.

◆ voGiven

Vector3 hrp::ForwardDynamicsMM::voGiven
private

Definition at line 95 of file ForwardDynamicsCBM.h.

◆ voGivenPrev

Vector3 hrp::ForwardDynamicsMM::voGivenPrev
private

Definition at line 105 of file ForwardDynamicsCBM.h.

◆ w

Vector3 hrp::ForwardDynamicsMM::w
private

Definition at line 135 of file ForwardDynamicsCBM.h.

◆ w0

Vector3 hrp::ForwardDynamicsMM::w0
private

Definition at line 130 of file ForwardDynamicsCBM.h.

◆ wGiven

Vector3 hrp::ForwardDynamicsMM::wGiven
private

Definition at line 96 of file ForwardDynamicsCBM.h.

◆ wGivenPrev

Vector3 hrp::ForwardDynamicsMM::wGivenPrev
private

Definition at line 106 of file ForwardDynamicsCBM.h.

The documentation for this class was generated from the following files:

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:08