#include <ForwardDynamicsCBM.h>

Inheritance diagram for hrp::ForwardDynamicsMM:

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 ()
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

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 ( BodyPtr body )

Definition at line 54 of file ForwardDynamicsCBM.cpp.

ForwardDynamicsMM::~ForwardDynamicsMM ( )

Definition at line 61 of file ForwardDynamicsCBM.cpp.

Member Function Documentation

void ForwardDynamicsMM::calcAccelFKandForceSensorValues ( ) [inline, private]

Definition at line 142 of file ForwardDynamicsCBM.cpp.

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

Definition at line 726 of file ForwardDynamicsCBM.cpp.

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

Definition at line 601 of file ForwardDynamicsCBM.cpp.

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

Definition at line 552 of file ForwardDynamicsCBM.cpp.

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.

void ForwardDynamicsMM::calcMotionWithEulerMethod ( ) [private]

Definition at line 189 of file ForwardDynamicsCBM.cpp.

void ForwardDynamicsMM::calcMotionWithRungeKuttaMethod ( ) [private]

Definition at line 220 of file ForwardDynamicsCBM.cpp.

void ForwardDynamicsMM::calcNextState ( ) [virtual]

Implements hrp::ForwardDynamics.

Definition at line 149 of file ForwardDynamicsCBM.cpp.

void ForwardDynamicsMM::calcPositionAndVelocityFK ( ) [private]

Todo:: remove this equation

Definition at line 363 of file ForwardDynamicsCBM.cpp.

void ForwardDynamicsMM::initialize ( void ) [virtual]

Implements hrp::ForwardDynamics.

Definition at line 67 of file ForwardDynamicsCBM.cpp.

void ForwardDynamicsMM::initializeAccelSolver ( )

Definition at line 625 of file ForwardDynamicsCBM.cpp.

void ForwardDynamicsMM::initializeSensors ( ) [private, virtual]

Reimplemented from hrp::ForwardDynamics.

Definition at line 777 of file ForwardDynamicsCBM.cpp.

void ForwardDynamicsMM::integrateRungeKuttaOneStep	(	double	r,
		double	dt
	)		`[private]`

Definition at line 320 of file ForwardDynamicsCBM.cpp.

void ForwardDynamicsMM::preserveHighGainModeJointState ( ) [private]

Definition at line 346 of file ForwardDynamicsCBM.cpp.

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

Definition at line 534 of file ForwardDynamicsCBM.cpp.

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

Definition at line 696 of file ForwardDynamicsCBM.cpp.

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