Classes | Public Types | Public Member Functions | Public Attributes | Static Public Attributes | List of all members
hrp::CFSImpl Class Reference

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

◆ ConstraintPointArray

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

Definition at line 159 of file ConstraintForceSolver.cpp.

◆ ExtraJointLinkPairPtr

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

Definition at line 229 of file ConstraintForceSolver.cpp.

◆ LinkDataArray

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

Definition at line 174 of file ConstraintForceSolver.cpp.

◆ LinkPairArray

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

Definition at line 219 of file ConstraintForceSolver.cpp.

◆ LinkPairPtr

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

Definition at line 218 of file ConstraintForceSolver.cpp.

◆ rmdmatrix

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::CFSImpl ( WorldBase &  world)

Definition at line 377 of file ConstraintForceSolver.cpp.

◆ ~CFSImpl()

CFSImpl::~CFSImpl ( )

Definition at line 392 of file ConstraintForceSolver.cpp.

Member Function Documentation

◆ addCollisionCheckLinkPair()

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.

◆ addConstraintForceToLink()

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

Definition at line 1699 of file ConstraintForceSolver.cpp.

◆ addConstraintForceToLinks()

void CFSImpl::addConstraintForceToLinks ( )

Definition at line 1685 of file ConstraintForceSolver.cpp.

◆ addExtraJoint()

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.

◆ calcABMForceElementsWithTestForce()

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

Definition at line 1321 of file ConstraintForceSolver.cpp.

◆ calcAccelsABM()

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

Definition at line 1346 of file ConstraintForceSolver.cpp.

◆ calcAccelsMM()

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

Definition at line 1405 of file ConstraintForceSolver.cpp.

◆ checkLCPResult()

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

Definition at line 2055 of file ConstraintForceSolver.cpp.

◆ checkMCPResult()

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

Definition at line 2085 of file ConstraintForceSolver.cpp.

◆ clearExternalForces()

void CFSImpl::clearExternalForces ( )
inline

Definition at line 635 of file ConstraintForceSolver.cpp.

◆ clearSingularPointConstraintsOfClosedLoopConnections()

void CFSImpl::clearSingularPointConstraintsOfClosedLoopConnections ( )

Definition at line 1600 of file ConstraintForceSolver.cpp.

◆ copySymmetricElementsOfAccelerationMatrix()

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.

◆ debugPutMatrix()

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

Definition at line 359 of file ConstraintForceSolver.cpp.

◆ debugPutVector()

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

Definition at line 364 of file ConstraintForceSolver.cpp.

◆ extractRelAccelsFromLinkPairCase1()

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.

◆ extractRelAccelsFromLinkPairCase2()

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.

◆ extractRelAccelsFromLinkPairCase3()

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.

◆ extractRelAccelsOfConstraintPoints()

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

Definition at line 1438 of file ConstraintForceSolver.cpp.

◆ initABMForceElementsWithNoExtForce()

void CFSImpl::initABMForceElementsWithNoExtForce ( BodyData &  bodyData)

Definition at line 1280 of file ConstraintForceSolver.cpp.

◆ initBody()

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

Definition at line 490 of file ConstraintForceSolver.cpp.

◆ initExtraJoints()

void CFSImpl::initExtraJoints ( int  bodyIndex)

Definition at line 510 of file ConstraintForceSolver.cpp.

◆ initialize()

void CFSImpl::initialize ( void  )

Definition at line 566 of file ConstraintForceSolver.cpp.

◆ initMatrices()

void CFSImpl::initMatrices ( )

Definition at line 1078 of file ConstraintForceSolver.cpp.

◆ putContactPoints()

void CFSImpl::putContactPoints ( )

Definition at line 1032 of file ConstraintForceSolver.cpp.

◆ putMatrix()

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

Definition at line 338 of file ConstraintForceSolver.cpp.

◆ putVector()

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

Definition at line 354 of file ConstraintForceSolver.cpp.

◆ setAccelCalcSkipInformation()

void CFSImpl::setAccelCalcSkipInformation ( )

Definition at line 1110 of file ConstraintForceSolver.cpp.

◆ setAccelerationMatrix()

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.

◆ setConstantVectorAndMuBlock()

void CFSImpl::setConstantVectorAndMuBlock ( )

Definition at line 1613 of file ConstraintForceSolver.cpp.

◆ setConstraintPoints()

void CFSImpl::setConstraintPoints ( CollisionSequence &  collisions)

Definition at line 745 of file ConstraintForceSolver.cpp.

◆ setContactConstraintPoints()

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

Definition at line 832 of file ConstraintForceSolver.cpp.

◆ setDefaultAccelerationVector()

void CFSImpl::setDefaultAccelerationVector ( )

Definition at line 1144 of file ConstraintForceSolver.cpp.

◆ setExtraJointConstraintPoints()

void CFSImpl::setExtraJointConstraintPoints ( ExtraJointLinkPairPtr &  linkPair)

Definition at line 986 of file ConstraintForceSolver.cpp.

◆ setFrictionVectors()

void CFSImpl::setFrictionVectors ( ConstraintPoint &  constraintPoint)

Definition at line 944 of file ConstraintForceSolver.cpp.

◆ solve()

void CFSImpl::solve ( CollisionSequence &  corbaCollisionSequence)

Definition at line 646 of file ConstraintForceSolver.cpp.

◆ solveImpactConstraints()

void CFSImpl::solveImpactConstraints ( )

Definition at line 1071 of file ConstraintForceSolver.cpp.

◆ solveMCPByProjectedGaussSeidel()

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

Definition at line 1742 of file ConstraintForceSolver.cpp.

◆ solveMCPByProjectedGaussSeidelInitial()

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

Definition at line 1811 of file ConstraintForceSolver.cpp.

◆ solveMCPByProjectedGaussSeidelMain()

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

◆ allowedPenetrationDepth

double hrp::CFSImpl::allowedPenetrationDepth

Definition at line 272 of file ConstraintForceSolver.cpp.

◆ an0

dvector hrp::CFSImpl::an0

Definition at line 251 of file ConstraintForceSolver.cpp.

◆ areThereImpacts

bool hrp::CFSImpl::areThereImpacts

Definition at line 242 of file ConstraintForceSolver.cpp.

◆ at0

dvector hrp::CFSImpl::at0

Definition at line 252 of file ConstraintForceSolver.cpp.

◆ b

dvector hrp::CFSImpl::b

Definition at line 255 of file ConstraintForceSolver.cpp.

◆ bodiesData

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

Definition at line 199 of file ConstraintForceSolver.cpp.

◆ collisionCheckLinkPairs

LinkPairArray hrp::CFSImpl::collisionCheckLinkPairs

Definition at line 221 of file ConstraintForceSolver.cpp.

◆ constrainedLinkPairs

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

Definition at line 230 of file ConstraintForceSolver.cpp.

◆ contactIndexToMu

dvector hrp::CFSImpl::contactIndexToMu

Definition at line 265 of file ConstraintForceSolver.cpp.

◆ extraJointLinkPairs

vector<ExtraJointLinkPairPtr> hrp::CFSImpl::extraJointLinkPairs

Definition at line 230 of file ConstraintForceSolver.cpp.

◆ frictionIndexToContactIndex

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

Definition at line 264 of file ConstraintForceSolver.cpp.

◆ gaussSeidelMaxRelError

double hrp::CFSImpl::gaussSeidelMaxRelError

Definition at line 270 of file ConstraintForceSolver.cpp.

◆ globalNumConstraintVectors

int hrp::CFSImpl::globalNumConstraintVectors

Definition at line 234 of file ConstraintForceSolver.cpp.

◆ globalNumContactNormalVectors

int hrp::CFSImpl::globalNumContactNormalVectors

Definition at line 236 of file ConstraintForceSolver.cpp.

◆ globalNumFrictionVectors

int hrp::CFSImpl::globalNumFrictionVectors

Definition at line 237 of file ConstraintForceSolver.cpp.

◆ isConstraintForceOutputMode

bool hrp::CFSImpl::isConstraintForceOutputMode

Definition at line 142 of file ConstraintForceSolver.cpp.

◆ maxNumGaussSeidelIteration

int hrp::CFSImpl::maxNumGaussSeidelIteration

Definition at line 268 of file ConstraintForceSolver.cpp.

◆ mcpHi

dvector hrp::CFSImpl::mcpHi

Definition at line 266 of file ConstraintForceSolver.cpp.

◆ Mlcp

rmdmatrix hrp::CFSImpl::Mlcp

Definition at line 248 of file ConstraintForceSolver.cpp.

◆ negativeVelocityRatioForPenetration

double hrp::CFSImpl::negativeVelocityRatioForPenetration

Definition at line 271 of file ConstraintForceSolver.cpp.

◆ numGaussSeidelInitialIteration

int hrp::CFSImpl::numGaussSeidelInitialIteration

Definition at line 269 of file ConstraintForceSolver.cpp.

◆ numGaussSeidelTotalCalls

int hrp::CFSImpl::numGaussSeidelTotalCalls

Definition at line 275 of file ConstraintForceSolver.cpp.

◆ numGaussSeidelTotalLoops

int hrp::CFSImpl::numGaussSeidelTotalLoops

Definition at line 274 of file ConstraintForceSolver.cpp.

◆ numGaussSeidelTotalLoopsMax

int hrp::CFSImpl::numGaussSeidelTotalLoopsMax

Definition at line 276 of file ConstraintForceSolver.cpp.

◆ numUnconverged

int hrp::CFSImpl::numUnconverged

Definition at line 243 of file ConstraintForceSolver.cpp.

◆ os

ofstream hrp::CFSImpl::os

Definition at line 335 of file ConstraintForceSolver.cpp.

◆ PI

const double hrp::CFSImpl::PI = 3.14159265358979323846
static

Definition at line 136 of file ConstraintForceSolver.cpp.

◆ PI_2

const double hrp::CFSImpl::PI_2 = 1.57079632679489661923
static

Definition at line 137 of file ConstraintForceSolver.cpp.

◆ prevGlobalNumConstraintVectors

int hrp::CFSImpl::prevGlobalNumConstraintVectors

Definition at line 239 of file ConstraintForceSolver.cpp.

◆ prevGlobalNumFrictionVectors

int hrp::CFSImpl::prevGlobalNumFrictionVectors

Definition at line 240 of file ConstraintForceSolver.cpp.

◆ randomAngle

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

Definition at line 261 of file ConstraintForceSolver.cpp.

◆ solution

dvector hrp::CFSImpl::solution

Definition at line 258 of file ConstraintForceSolver.cpp.

◆ useBuiltinCollisionDetector

bool hrp::CFSImpl::useBuiltinCollisionDetector

Definition at line 143 of file ConstraintForceSolver.cpp.

◆ world

WorldBase& hrp::CFSImpl::world

Definition at line 140 of file ConstraintForceSolver.cpp.

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

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