Classes
struct	BodyData

struct	ConstraintPoint

class	ExtraJointLinkPair

struct	LinkData

class	LinkPair

Public Types
typedef std::vector< ConstraintPoint >	ConstraintPointArray

typedef intrusive_ptr< ExtraJointLinkPair >	ExtraJointLinkPairPtr

typedef std::vector< LinkData >	LinkDataArray

typedef std::vector< LinkPairPtr >	LinkPairArray

typedef intrusive_ptr< LinkPair >	LinkPairPtr

typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor >	rmdmatrix

Public Member Functions
bool	addCollisionCheckLinkPair (int bodyIndex1, Link link1, int bodyIndex2, Link link2, double muStatic, double muDynamic, double culling_thresh, double restitution, double epsilon)

void	addConstraintForceToLink (LinkPair *linkPair, int ipair)

void	addConstraintForceToLinks ()

bool	addExtraJoint (int bodyIndex1, Link link1, int bodyIndex2, Link link2, const double link1LocalPos, const double link2LocalPos, const short jointType, const double *jointAxis)

void	calcABMForceElementsWithTestForce (BodyData &bodyData, Link *linkToApplyForce, const Vector3 &f, const Vector3 &tau)

void	calcAccelsABM (BodyData &bodyData, int constraintIndex)

void	calcAccelsMM (BodyData &bodyData, int constraintIndex)

	CFSImpl (WorldBase &world)

void	checkLCPResult (rmdmatrix &M, dvector &b, dvector &x)

void	checkMCPResult (rmdmatrix &M, dvector &b, dvector &x)

void	clearExternalForces ()

void	clearSingularPointConstraintsOfClosedLoopConnections ()

void	copySymmetricElementsOfAccelerationMatrix (Eigen::Block< rmdmatrix > &Knn, Eigen::Block< rmdmatrix > &Ktn, Eigen::Block< rmdmatrix > &Knt, Eigen::Block< rmdmatrix > &Ktt)

template<class TMatrix >
void	debugPutMatrix (const TMatrix &M, const char *name)

template<class TVector >
void	debugPutVector (const TVector &M, const char *name)

void	extractRelAccelsFromLinkPairCase1 (Eigen::Block< rmdmatrix > &Kxn, Eigen::Block< rmdmatrix > &Kxt, LinkPair &linkPair, int testForceIndex, int constraintIndex)

void	extractRelAccelsFromLinkPairCase2 (Eigen::Block< rmdmatrix > &Kxn, Eigen::Block< rmdmatrix > &Kxt, LinkPair &linkPair, int iTestForce, int iDefault, int testForceIndex, int constraintIndex)

void	extractRelAccelsFromLinkPairCase3 (Eigen::Block< rmdmatrix > &Kxn, Eigen::Block< rmdmatrix > &Kxt, LinkPair &linkPair, int testForceIndex, int constraintIndex)

void	extractRelAccelsOfConstraintPoints (Eigen::Block< rmdmatrix > &Kxn, Eigen::Block< rmdmatrix > &Kxt, int testForceIndex, int constraintIndex)

void	initABMForceElementsWithNoExtForce (BodyData &bodyData)

void	initBody (BodyPtr body, BodyData &bodyData)

void	initExtraJoints (int bodyIndex)

void	initialize (void)

void	initMatrices ()

void	putContactPoints ()

template<class TMatrix >
void	putMatrix (TMatrix &M, const char *name)

template<class TVector >
void	putVector (const TVector &M, const char *name)

void	setAccelCalcSkipInformation ()

void	setAccelerationMatrix ()

void	setConstantVectorAndMuBlock ()

void	setConstraintPoints (CollisionSequence &collisions)

void	setContactConstraintPoints (LinkPair &linkPair, CollisionPointSequence &collisionPoints)

void	setDefaultAccelerationVector ()

void	setExtraJointConstraintPoints (ExtraJointLinkPairPtr &linkPair)

void	setFrictionVectors (ConstraintPoint &constraintPoint)

void	solve (CollisionSequence &corbaCollisionSequence)

void	solveImpactConstraints ()

void	solveMCPByProjectedGaussSeidel (const rmdmatrix &M, const dvector &b, dvector &x)

void	solveMCPByProjectedGaussSeidelInitial (const rmdmatrix &M, const dvector &b, dvector &x, const int numIteration)

void	solveMCPByProjectedGaussSeidelMain (const rmdmatrix &M, const dvector &b, dvector &x, const int numIteration)

	~CFSImpl ()

Public Attributes
double	allowedPenetrationDepth

dvector	an0

bool	areThereImpacts

dvector	at0

dvector	b

std::vector< BodyData >	bodiesData

LinkPairArray	collisionCheckLinkPairs

std::vector< LinkPair * >	constrainedLinkPairs

dvector	contactIndexToMu

vector< ExtraJointLinkPairPtr >	extraJointLinkPairs

std::vector< int >	frictionIndexToContactIndex

double	gaussSeidelMaxRelError

int	globalNumConstraintVectors

int	globalNumContactNormalVectors

int	globalNumFrictionVectors

bool	isConstraintForceOutputMode

int	maxNumGaussSeidelIteration

dvector	mcpHi

rmdmatrix	Mlcp

double	negativeVelocityRatioForPenetration

int	numGaussSeidelInitialIteration

int	numGaussSeidelTotalCalls

int	numGaussSeidelTotalLoops

int	numGaussSeidelTotalLoopsMax

int	numUnconverged

ofstream	os

int	prevGlobalNumConstraintVectors

int	prevGlobalNumFrictionVectors

variate_generator< boost::mt19937, uniform_real<> >	randomAngle

dvector	solution

bool	useBuiltinCollisionDetector

WorldBase &	world

Static Public Attributes
static const double	PI = 3.14159265358979323846

static const double	PI_2 = 1.57079632679489661923

Detailed Description

Definition at line 115 of file ConstraintForceSolver.cpp.

Member Typedef Documentation

typedef std::vector<ConstraintPoint> hrp::CFSImpl::ConstraintPointArray

Definition at line 159 of file ConstraintForceSolver.cpp.

typedef intrusive_ptr<ExtraJointLinkPair> hrp::CFSImpl::ExtraJointLinkPairPtr

Definition at line 229 of file ConstraintForceSolver.cpp.

typedef std::vector<LinkData> hrp::CFSImpl::LinkDataArray

Definition at line 174 of file ConstraintForceSolver.cpp.

typedef std::vector<LinkPairPtr> hrp::CFSImpl::LinkPairArray

Definition at line 219 of file ConstraintForceSolver.cpp.

typedef intrusive_ptr<LinkPair> hrp::CFSImpl::LinkPairPtr

Definition at line 218 of file ConstraintForceSolver.cpp.

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> hrp::CFSImpl::rmdmatrix

Definition at line 245 of file ConstraintForceSolver.cpp.

Constructor & Destructor Documentation

CFSImpl::CFSImpl ( WorldBase & world )

Definition at line 377 of file ConstraintForceSolver.cpp.

CFSImpl::~CFSImpl ( )

Definition at line 392 of file ConstraintForceSolver.cpp.

Member Function Documentation

bool CFSImpl::addCollisionCheckLinkPair	(	int	bodyIndex1,
		Link *	link1,
		int	bodyIndex2,
		Link *	link2,
		double	muStatic,
		double	muDynamic,
		double	culling_thresh,
		double	restitution,
		double	epsilon
	)

Definition at line 401 of file ConstraintForceSolver.cpp.

void CFSImpl::addConstraintForceToLink	(	LinkPair *	linkPair,
		int	ipair
	)

Definition at line 1699 of file ConstraintForceSolver.cpp.

void CFSImpl::addConstraintForceToLinks ( )

Definition at line 1685 of file ConstraintForceSolver.cpp.

bool CFSImpl::addExtraJoint	(	int	bodyIndex1,
		Link *	link1,
		int	bodyIndex2,
		Link *	link2,
		const double *	link1LocalPos,
		const double *	link2LocalPos,
		const short	jointType,
		const double *	jointAxis
	)

Definition at line 438 of file ConstraintForceSolver.cpp.

void CFSImpl::calcABMForceElementsWithTestForce	(	BodyData &	bodyData,
		Link *	linkToApplyForce,
		const Vector3 &	f,
		const Vector3 &	tau
	)

Definition at line 1321 of file ConstraintForceSolver.cpp.

void CFSImpl::calcAccelsABM	(	BodyData &	bodyData,
		int	constraintIndex
	)

Definition at line 1346 of file ConstraintForceSolver.cpp.

void CFSImpl::calcAccelsMM	(	BodyData &	bodyData,
		int	constraintIndex
	)

Definition at line 1405 of file ConstraintForceSolver.cpp.

void CFSImpl::checkLCPResult	(	rmdmatrix &	M,
		dvector &	b,
		dvector &	x
	)

Definition at line 2055 of file ConstraintForceSolver.cpp.

void CFSImpl::checkMCPResult	(	rmdmatrix &	M,
		dvector &	b,
		dvector &	x
	)

Definition at line 2085 of file ConstraintForceSolver.cpp.

void CFSImpl::clearExternalForces ( )

inline

Definition at line 635 of file ConstraintForceSolver.cpp.

void CFSImpl::clearSingularPointConstraintsOfClosedLoopConnections ( )

Definition at line 1600 of file ConstraintForceSolver.cpp.

void CFSImpl::copySymmetricElementsOfAccelerationMatrix	(	Eigen::Block< rmdmatrix > &	Knn,
		Eigen::Block< rmdmatrix > &	Ktn,
		Eigen::Block< rmdmatrix > &	Knt,
		Eigen::Block< rmdmatrix > &	Ktt
	)

Definition at line 1564 of file ConstraintForceSolver.cpp.

template<class TMatrix >

void hrp::CFSImpl::debugPutMatrix	(	const TMatrix &	M,
		const char *	name
	)

inline

Definition at line 359 of file ConstraintForceSolver.cpp.

template<class TVector >

void hrp::CFSImpl::debugPutVector	(	const TVector &	M,
		const char *	name
	)

inline

Definition at line 364 of file ConstraintForceSolver.cpp.

void CFSImpl::extractRelAccelsFromLinkPairCase1	(	Eigen::Block< rmdmatrix > &	Kxn,
		Eigen::Block< rmdmatrix > &	Kxt,
		LinkPair &	linkPair,
		int	testForceIndex,
		int	constraintIndex
	)

Todo:: Can the follwoing equations be simplified ?

Definition at line 1468 of file ConstraintForceSolver.cpp.

void CFSImpl::extractRelAccelsFromLinkPairCase2	(	Eigen::Block< rmdmatrix > &	Kxn,
		Eigen::Block< rmdmatrix > &	Kxt,
		LinkPair &	linkPair,
		int	iTestForce,
		int	iDefault,
		int	testForceIndex,
		int	constraintIndex
	)

Definition at line 1504 of file ConstraintForceSolver.cpp.

void CFSImpl::extractRelAccelsFromLinkPairCase3	(	Eigen::Block< rmdmatrix > &	Kxn,
		Eigen::Block< rmdmatrix > &	Kxt,
		LinkPair &	linkPair,
		int	testForceIndex,
		int	constraintIndex
	)

Definition at line 1541 of file ConstraintForceSolver.cpp.

void CFSImpl::extractRelAccelsOfConstraintPoints	(	Eigen::Block< rmdmatrix > &	Kxn,
		Eigen::Block< rmdmatrix > &	Kxt,
		int	testForceIndex,
		int	constraintIndex
	)

Definition at line 1438 of file ConstraintForceSolver.cpp.

void CFSImpl::initABMForceElementsWithNoExtForce ( BodyData & bodyData )

Definition at line 1280 of file ConstraintForceSolver.cpp.

void CFSImpl::initBody	(	BodyPtr	body,
		BodyData &	bodyData
	)

Definition at line 490 of file ConstraintForceSolver.cpp.

void CFSImpl::initExtraJoints ( int bodyIndex )

Definition at line 510 of file ConstraintForceSolver.cpp.

void CFSImpl::initialize ( void )

Definition at line 566 of file ConstraintForceSolver.cpp.

void CFSImpl::initMatrices ( )

Definition at line 1078 of file ConstraintForceSolver.cpp.

void CFSImpl::putContactPoints ( )

Definition at line 1032 of file ConstraintForceSolver.cpp.

template<class TMatrix >

void hrp::CFSImpl::putMatrix	(	TMatrix &	M,
		const char *	name
	)

inline

Definition at line 338 of file ConstraintForceSolver.cpp.

template<class TVector >

void hrp::CFSImpl::putVector	(	const TVector &	M,
		const char *	name
	)

inline

Definition at line 354 of file ConstraintForceSolver.cpp.

void CFSImpl::setAccelCalcSkipInformation ( )

Definition at line 1110 of file ConstraintForceSolver.cpp.

void CFSImpl::setAccelerationMatrix ( )

Todo:: This code does not work correctly when the links are in the same body. Fix it.

Todo:: This code does not work correctly when the links are in the same body. Fix it.

Definition at line 1197 of file ConstraintForceSolver.cpp.

void CFSImpl::setConstantVectorAndMuBlock ( )

Definition at line 1613 of file ConstraintForceSolver.cpp.

void CFSImpl::setConstraintPoints ( CollisionSequence & collisions )

Definition at line 745 of file ConstraintForceSolver.cpp.

void CFSImpl::setContactConstraintPoints	(	LinkPair &	linkPair,
		CollisionPointSequence &	collisionPoints
	)

Definition at line 832 of file ConstraintForceSolver.cpp.

void CFSImpl::setDefaultAccelerationVector ( )

Definition at line 1144 of file ConstraintForceSolver.cpp.

void CFSImpl::setExtraJointConstraintPoints ( ExtraJointLinkPairPtr & linkPair )

Definition at line 986 of file ConstraintForceSolver.cpp.

void CFSImpl::setFrictionVectors ( ConstraintPoint & constraintPoint )

Definition at line 944 of file ConstraintForceSolver.cpp.

void CFSImpl::solve ( CollisionSequence & corbaCollisionSequence )

Definition at line 646 of file ConstraintForceSolver.cpp.

void CFSImpl::solveImpactConstraints ( )

Definition at line 1071 of file ConstraintForceSolver.cpp.

void CFSImpl::solveMCPByProjectedGaussSeidel	(	const rmdmatrix &	M,
		const dvector &	b,
		dvector &	x
	)

Definition at line 1742 of file ConstraintForceSolver.cpp.

void CFSImpl::solveMCPByProjectedGaussSeidelInitial	(	const rmdmatrix &	M,
		const dvector &	b,
		dvector &	x,
		const int	numIteration
	)

Definition at line 1811 of file ConstraintForceSolver.cpp.

void CFSImpl::solveMCPByProjectedGaussSeidelMain	(	const rmdmatrix &	M,
		const dvector &	b,
		dvector &	x,
		const int	numIteration
	)

Definition at line 1937 of file ConstraintForceSolver.cpp.

Member Data Documentation

double hrp::CFSImpl::allowedPenetrationDepth

Definition at line 272 of file ConstraintForceSolver.cpp.

dvector hrp::CFSImpl::an0

Definition at line 251 of file ConstraintForceSolver.cpp.

bool hrp::CFSImpl::areThereImpacts

Definition at line 242 of file ConstraintForceSolver.cpp.

dvector hrp::CFSImpl::at0

Definition at line 252 of file ConstraintForceSolver.cpp.

dvector hrp::CFSImpl::b

Definition at line 255 of file ConstraintForceSolver.cpp.

std::vector<BodyData> hrp::CFSImpl::bodiesData

Definition at line 199 of file ConstraintForceSolver.cpp.

LinkPairArray hrp::CFSImpl::collisionCheckLinkPairs

Definition at line 221 of file ConstraintForceSolver.cpp.

std::vector<LinkPair*> hrp::CFSImpl::constrainedLinkPairs

Definition at line 230 of file ConstraintForceSolver.cpp.

dvector hrp::CFSImpl::contactIndexToMu

Definition at line 265 of file ConstraintForceSolver.cpp.

vector<ExtraJointLinkPairPtr> hrp::CFSImpl::extraJointLinkPairs

Definition at line 230 of file ConstraintForceSolver.cpp.

std::vector<int> hrp::CFSImpl::frictionIndexToContactIndex

Definition at line 264 of file ConstraintForceSolver.cpp.

double hrp::CFSImpl::gaussSeidelMaxRelError

Definition at line 270 of file ConstraintForceSolver.cpp.

int hrp::CFSImpl::globalNumConstraintVectors

Definition at line 234 of file ConstraintForceSolver.cpp.

int hrp::CFSImpl::globalNumContactNormalVectors

Definition at line 236 of file ConstraintForceSolver.cpp.

int hrp::CFSImpl::globalNumFrictionVectors

Definition at line 237 of file ConstraintForceSolver.cpp.

bool hrp::CFSImpl::isConstraintForceOutputMode

Definition at line 142 of file ConstraintForceSolver.cpp.

int hrp::CFSImpl::maxNumGaussSeidelIteration

Definition at line 268 of file ConstraintForceSolver.cpp.

dvector hrp::CFSImpl::mcpHi

Definition at line 266 of file ConstraintForceSolver.cpp.

rmdmatrix hrp::CFSImpl::Mlcp

Definition at line 248 of file ConstraintForceSolver.cpp.

double hrp::CFSImpl::negativeVelocityRatioForPenetration

Definition at line 271 of file ConstraintForceSolver.cpp.

int hrp::CFSImpl::numGaussSeidelInitialIteration

Definition at line 269 of file ConstraintForceSolver.cpp.

int hrp::CFSImpl::numGaussSeidelTotalCalls

Definition at line 275 of file ConstraintForceSolver.cpp.

int hrp::CFSImpl::numGaussSeidelTotalLoops

Definition at line 274 of file ConstraintForceSolver.cpp.

int hrp::CFSImpl::numGaussSeidelTotalLoopsMax

Definition at line 276 of file ConstraintForceSolver.cpp.

int hrp::CFSImpl::numUnconverged

Definition at line 243 of file ConstraintForceSolver.cpp.

ofstream hrp::CFSImpl::os

Definition at line 335 of file ConstraintForceSolver.cpp.

const double hrp::CFSImpl::PI = 3.14159265358979323846

static

Definition at line 136 of file ConstraintForceSolver.cpp.

const double hrp::CFSImpl::PI_2 = 1.57079632679489661923

static

Definition at line 137 of file ConstraintForceSolver.cpp.

int hrp::CFSImpl::prevGlobalNumConstraintVectors

Definition at line 239 of file ConstraintForceSolver.cpp.

int hrp::CFSImpl::prevGlobalNumFrictionVectors

Definition at line 240 of file ConstraintForceSolver.cpp.

variate_generator<boost::mt19937, uniform_real<> > hrp::CFSImpl::randomAngle

Definition at line 261 of file ConstraintForceSolver.cpp.

dvector hrp::CFSImpl::solution

Definition at line 258 of file ConstraintForceSolver.cpp.

bool hrp::CFSImpl::useBuiltinCollisionDetector

Definition at line 143 of file ConstraintForceSolver.cpp.

WorldBase& hrp::CFSImpl::world

Definition at line 140 of file ConstraintForceSolver.cpp.

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

ConstraintForceSolver.cpp

Classes

Public Types

Public Member Functions

Public Attributes

Static Public Attributes

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation