Classes | Public Types | Public Member Functions | Public Attributes | Static Public Attributes
hrp::CFSImpl Class Reference

List of all members.

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< LinkDataLinkDataArray
typedef std::vector< LinkPairPtrLinkPairArray
typedef intrusive_ptr< LinkPairLinkPairPtr
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< BodyDatabodiesData
LinkPairArray collisionCheckLinkPairs
std::vector< LinkPair * > constrainedLinkPairs
dvector contactIndexToMu
vector< ExtraJointLinkPairPtrextraJointLinkPairs
std::vector< intfrictionIndexToContactIndex
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
WorldBaseworld

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

Definition at line 159 of file ConstraintForceSolver.cpp.

Definition at line 229 of file ConstraintForceSolver.cpp.

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

Definition at line 174 of file ConstraintForceSolver.cpp.

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

Definition at line 377 of file ConstraintForceSolver.cpp.

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.

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.

Definition at line 2055 of file ConstraintForceSolver.cpp.

Definition at line 2085 of file ConstraintForceSolver.cpp.

Definition at line 635 of file ConstraintForceSolver.cpp.

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.

Definition at line 1280 of file ConstraintForceSolver.cpp.

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

Definition at line 490 of file ConstraintForceSolver.cpp.

Definition at line 510 of file ConstraintForceSolver.cpp.

Definition at line 566 of file ConstraintForceSolver.cpp.

Definition at line 1078 of file ConstraintForceSolver.cpp.

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.

Definition at line 1110 of file ConstraintForceSolver.cpp.

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.

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.

Definition at line 1144 of file ConstraintForceSolver.cpp.

Definition at line 986 of file ConstraintForceSolver.cpp.

Definition at line 944 of file ConstraintForceSolver.cpp.

void CFSImpl::solve ( CollisionSequence &  corbaCollisionSequence)

Definition at line 646 of file ConstraintForceSolver.cpp.

Definition at line 1071 of file ConstraintForceSolver.cpp.

Definition at line 1742 of file ConstraintForceSolver.cpp.

Definition at line 1811 of file ConstraintForceSolver.cpp.

Definition at line 1937 of file ConstraintForceSolver.cpp.


Member Data Documentation

Definition at line 272 of file ConstraintForceSolver.cpp.

Definition at line 251 of file ConstraintForceSolver.cpp.

Definition at line 242 of file ConstraintForceSolver.cpp.

Definition at line 252 of file ConstraintForceSolver.cpp.

Definition at line 255 of file ConstraintForceSolver.cpp.

Definition at line 199 of file ConstraintForceSolver.cpp.

Definition at line 221 of file ConstraintForceSolver.cpp.

Definition at line 230 of file ConstraintForceSolver.cpp.

Definition at line 265 of file ConstraintForceSolver.cpp.

Definition at line 230 of file ConstraintForceSolver.cpp.

Definition at line 264 of file ConstraintForceSolver.cpp.

Definition at line 270 of file ConstraintForceSolver.cpp.

Definition at line 234 of file ConstraintForceSolver.cpp.

Definition at line 236 of file ConstraintForceSolver.cpp.

Definition at line 237 of file ConstraintForceSolver.cpp.

Definition at line 142 of file ConstraintForceSolver.cpp.

Definition at line 268 of file ConstraintForceSolver.cpp.

Definition at line 266 of file ConstraintForceSolver.cpp.

Definition at line 248 of file ConstraintForceSolver.cpp.

Definition at line 271 of file ConstraintForceSolver.cpp.

Definition at line 269 of file ConstraintForceSolver.cpp.

Definition at line 275 of file ConstraintForceSolver.cpp.

Definition at line 274 of file ConstraintForceSolver.cpp.

Definition at line 276 of file ConstraintForceSolver.cpp.

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.

Definition at line 239 of file ConstraintForceSolver.cpp.

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.

Definition at line 258 of file ConstraintForceSolver.cpp.

Definition at line 143 of file ConstraintForceSolver.cpp.

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 Apr 11 2019 03:30:22