00001
00002
00003
00004
00005
00006
00007
00008
00009
00016 #ifdef __WIN32__
00017 #define NOMINMAX
00018 #define _USE_MATH_DEFINES // for M_PI
00019 #endif
00020
00021 #include "World.h"
00022 #include "Body.h"
00023 #include "Link.h"
00024 #include "LinkTraverse.h"
00025 #include "ForwardDynamicsCBM.h"
00026 #include "ConstraintForceSolver.h"
00027
00028 #include <hrpUtil/EigenTypes.h>
00029 #include <hrpCorba/OpenHRPCommon.hh>
00030 #include <hrpCollision/ColdetModelPair.h>
00031
00032 #include <limits>
00033 #include <boost/format.hpp>
00034 #include <boost/tuple/tuple.hpp>
00035 #include <boost/random.hpp>
00036
00037 #include <fstream>
00038 #include <iomanip>
00039 #include <boost/lexical_cast.hpp>
00040
00041 using namespace std;
00042 using namespace boost;
00043 using namespace hrp;
00044 using namespace OpenHRP;
00045
00046
00047
00048
00049
00050 #ifdef USE_PIVOTING_LCP
00051 #include "SimpleLCP_Path.h"
00052 static const bool usePivotingLCP = true;
00053 #else
00054 static const bool usePivotingLCP = false;
00055 #endif
00056
00057
00058
00059
00060 static const double VEL_THRESH_OF_DYNAMIC_FRICTION = 1.0e-4;
00061
00062 static const bool ENABLE_STATIC_FRICTION = true;
00063 static const bool ONLY_STATIC_FRICTION_FORMULATION = (true && ENABLE_STATIC_FRICTION);
00064 static const bool STATIC_FRICTION_BY_TWO_CONSTRAINTS = true;
00065 static const bool IGNORE_CURRENT_VELOCITY_IN_STATIC_FRICTION = false;
00066
00067 static const bool ENABLE_TRUE_FRICTION_CONE =
00068 (true && ONLY_STATIC_FRICTION_FORMULATION && STATIC_FRICTION_BY_TWO_CONSTRAINTS);
00069
00070 static const bool SKIP_REDUNDANT_ACCEL_CALC = true;
00071 static const bool ASSUME_SYMMETRIC_MATRIX = false;
00072
00073 static const int DEFAULT_MAX_NUM_GAUSS_SEIDEL_ITERATION = 500;
00074 static const int DEFAULT_NUM_GAUSS_SEIDEL_ITERATION_BLOCK = 10;
00075 static const int DEFAULT_NUM_GAUSS_SEIDEL_INITIAL_ITERATION = 0;
00076 static const double DEFAULT_GAUSS_SEIDEL_MAX_REL_ERROR = 1.0e-3;
00077
00078 static const double THRESH_TO_SWITCH_REL_ERROR = 1.0e-8;
00079 static const bool USE_PREVIOUS_LCP_SOLUTION = true;
00080
00081 static const bool ALLOW_SUBTLE_PENETRATION_FOR_STABILITY = true;
00082
00083
00084 static const double ALLOWED_PENETRATION_DEPTH = 0.0001;
00085
00086
00087 static const double DEFAULT_NEGATIVE_VELOCITY_RATIO_FOR_PENETRATION = 10.0;
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097 static const bool PROPORTIONAL_DYNAMIC_FRICTION = false;
00098 static const bool ENABLE_RANDOM_STATIC_FRICTION_BASE = false;
00099
00100
00101
00102 static const bool CFS_DEBUG = false;
00103 static const bool CFS_DEBUG_VERBOSE = false;
00104 static const bool CFS_DEBUG_VERBOSE_2 = false;
00105 static const bool CFS_DEBUG_VERBOSE_3 = false;
00106 static const bool CFS_DEBUG_LCPCHECK = false;
00107 static const bool CFS_MCP_DEBUG = false;
00108 static const bool CFS_MCP_DEBUG_SHOW_ITERATION_STOP = false;
00109
00110 static const bool CFS_PUT_NUM_CONTACT_POINTS = false;
00111
00112
00113 namespace hrp
00114 {
00115 class CFSImpl
00116 {
00117 public:
00118
00119 CFSImpl(WorldBase& world);
00120 ~CFSImpl();
00121
00122 bool addCollisionCheckLinkPair
00123 (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, double muStatic, double muDynamic, double culling_thresh, double restitution, double epsilon);
00124 bool addExtraJoint
00125 (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, const double* link1LocalPos, const double* link2LocalPos, const short jointType, const double* jointAxis );
00126
00127 void initialize(void);
00128 void solve(CollisionSequence& corbaCollisionSequence);
00129 inline void clearExternalForces();
00130
00131 #undef PI
00132 #if defined(M_PI) && defined (M_PI_2)
00133 #define PI M_PI
00134 #define PI_2 M_PI_2
00135 #else
00136 static const double PI;
00137 static const double PI_2;
00138 #endif
00139
00140 WorldBase& world;
00141
00142 bool isConstraintForceOutputMode;
00143 bool useBuiltinCollisionDetector;
00144
00145 struct ConstraintPoint {
00146 int globalIndex;
00147 Vector3 point;
00148 Vector3 normalTowardInside[2];
00149 Vector3 defaultAccel[2];
00150 double normalProjectionOfRelVelocityOn0;
00151 double depth;
00152
00153 double mu;
00154 Vector3 relVelocityOn0;
00155 int globalFrictionIndex;
00156 int numFrictionVectors;
00157 Vector3 frictionVector[4][2];
00158 };
00159 typedef std::vector<ConstraintPoint> ConstraintPointArray;
00160
00161 struct LinkData
00162 {
00163 Vector3 dvo;
00164 Vector3 dw;
00165 Vector3 pf0;
00166 Vector3 ptau0;
00167 double uu;
00168 double uu0;
00169 double ddq;
00170 int numberToCheckAccelCalcSkip;
00171 int parentIndex;
00172 Link* link;
00173 };
00174 typedef std::vector<LinkData> LinkDataArray;
00175
00176 struct BodyData
00177 {
00178 BodyPtr body;
00179 bool isStatic;
00180 bool hasConstrainedLinks;
00181 bool isTestForceBeingApplied;
00182 LinkDataArray linksData;
00183
00184 Vector3 dpf;
00185 Vector3 dptau;
00186
00188 Vector3* rootLinkPosRef;
00189
00196 ForwardDynamicsMMPtr forwardDynamicsMM;
00197 };
00198
00199 std::vector<BodyData> bodiesData;
00200
00201 class LinkPair : public ColdetModelPair
00202 {
00203 public:
00204 int index;
00205 bool isSameBodyPair;
00206 int bodyIndex[2];
00207 BodyData* bodyData[2];
00208 Link* link[2];
00209 LinkData* linkData[2];
00210 ConstraintPointArray constraintPoints;
00211 bool isNonContactConstraint;
00212 double muStatic;
00213 double muDynamic;
00214 double culling_thresh;
00215 double restitution;
00216 double epsilon;
00217 };
00218 typedef intrusive_ptr<LinkPair> LinkPairPtr;
00219 typedef std::vector<LinkPairPtr> LinkPairArray;
00220
00221 LinkPairArray collisionCheckLinkPairs;
00222
00223 class ExtraJointLinkPair : public LinkPair
00224 {
00225 public:
00226 Vector3 jointPoint[2];
00227 Vector3 jointConstraintAxes[3];
00228 };
00229 typedef intrusive_ptr<ExtraJointLinkPair> ExtraJointLinkPairPtr;
00230 vector<ExtraJointLinkPairPtr> extraJointLinkPairs;;
00231
00232 std::vector<LinkPair*> constrainedLinkPairs;
00233
00234 int globalNumConstraintVectors;
00235
00236 int globalNumContactNormalVectors;
00237 int globalNumFrictionVectors;
00238
00239 int prevGlobalNumConstraintVectors;
00240 int prevGlobalNumFrictionVectors;
00241
00242 bool areThereImpacts;
00243 int numUnconverged;
00244
00245 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> rmdmatrix;
00246
00247
00248 rmdmatrix Mlcp;
00249
00250
00251 dvector an0;
00252 dvector at0;
00253
00254
00255 dvector b;
00256
00257
00258 dvector solution;
00259
00260
00261 variate_generator<boost::mt19937, uniform_real<> > randomAngle;
00262
00263
00264 std::vector<int> frictionIndexToContactIndex;
00265 dvector contactIndexToMu;
00266 dvector mcpHi;
00267
00268 int maxNumGaussSeidelIteration;
00269 int numGaussSeidelInitialIteration;
00270 double gaussSeidelMaxRelError;
00271 double negativeVelocityRatioForPenetration;
00272 double allowedPenetrationDepth;
00273
00274 int numGaussSeidelTotalLoops;
00275 int numGaussSeidelTotalCalls;
00276 int numGaussSeidelTotalLoopsMax;
00277
00278 void initBody(BodyPtr body, BodyData& bodyData);
00279 void initExtraJoints(int bodyIndex);
00280 void setConstraintPoints(CollisionSequence& collisions);
00281 void setContactConstraintPoints(LinkPair& linkPair, CollisionPointSequence& collisionPoints);
00282 void setFrictionVectors(ConstraintPoint& constraintPoint);
00283 void setExtraJointConstraintPoints(ExtraJointLinkPairPtr& linkPair);
00284 void putContactPoints();
00285 void solveImpactConstraints();
00286 void initMatrices();
00287 void setAccelCalcSkipInformation();
00288 void setDefaultAccelerationVector();
00289 void setAccelerationMatrix();
00290 void initABMForceElementsWithNoExtForce(BodyData& bodyData);
00291 void calcABMForceElementsWithTestForce(BodyData& bodyData, Link* linkToApplyForce, const Vector3& f, const Vector3& tau);
00292 void calcAccelsABM(BodyData& bodyData, int constraintIndex);
00293 void calcAccelsMM(BodyData& bodyData, int constraintIndex);
00294
00295 void extractRelAccelsOfConstraintPoints
00296 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, int testForceIndex, int constraintIndex);
00297
00298 void extractRelAccelsFromLinkPairCase1
00299 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, LinkPair& linkPair, int testForceIndex, int constraintIndex);
00300 void extractRelAccelsFromLinkPairCase2
00301 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, LinkPair& linkPair, int iTestForce, int iDefault, int testForceIndex, int constraintIndex);
00302 void extractRelAccelsFromLinkPairCase3
00303 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, LinkPair& linkPair, int testForceIndex, int constraintIndex);
00304
00305 void copySymmetricElementsOfAccelerationMatrix
00306 (Eigen::Block<rmdmatrix>& Knn, Eigen::Block<rmdmatrix>& Ktn, Eigen::Block<rmdmatrix>& Knt, Eigen::Block<rmdmatrix>& Ktt);
00307
00308 void clearSingularPointConstraintsOfClosedLoopConnections();
00309
00310 void setConstantVectorAndMuBlock();
00311 void addConstraintForceToLinks();
00312 void addConstraintForceToLink(LinkPair* linkPair, int ipair);
00313
00314 void solveMCPByProjectedGaussSeidel
00315 (const rmdmatrix& M, const dvector& b, dvector& x);
00316 void solveMCPByProjectedGaussSeidelInitial
00317 (const rmdmatrix& M, const dvector& b, dvector& x, const int numIteration);
00318 void solveMCPByProjectedGaussSeidelMain
00319 (const rmdmatrix& M, const dvector& b, dvector& x, const int numIteration);
00320
00321 void checkLCPResult(rmdmatrix& M, dvector& b, dvector& x);
00322 void checkMCPResult(rmdmatrix& M, dvector& b, dvector& x);
00323
00324 #ifdef USE_PIVOTING_LCP
00325 bool callPathLCPSolver(rmdmatrix& Mlcp, dvector& b, dvector& solution);
00326
00327
00328 std::vector<double> lb;
00329 std::vector<double> ub;
00330 std::vector<int> m_i;
00331 std::vector<int> m_j;
00332 std::vector<double> m_ij;
00333 #endif
00334
00335 ofstream os;
00336
00337 template<class TMatrix>
00338 void putMatrix(TMatrix& M, const char *name) {
00339 if(M.cols() == 1){
00340 os << "Vector " << name << M << std::endl;
00341 } else {
00342 os << "Matrix " << name << ": \n";
00343 for(int i=0; i < M.rows(); i++){
00344 for(int j=0; j < M.cols(); j++){
00345
00346 os << boost::format(" %.50g ") % M(i, j);
00347 }
00348 os << std::endl;
00349 }
00350 }
00351 }
00352
00353 template<class TVector>
00354 void putVector(const TVector& M, const char *name) {
00355 os << "Vector " << name << M << std::endl;
00356 }
00357
00358 template<class TMatrix>
00359 void debugPutMatrix(const TMatrix& M, const char *name) {
00360 if(CFS_DEBUG_VERBOSE) putMatrix(M, name);
00361 }
00362
00363 template<class TVector>
00364 void debugPutVector(const TVector& M, const char *name) {
00365 if(CFS_DEBUG_VERBOSE) putVector(M, name);
00366 }
00367 };
00368 #if !defined(M_PI) || !defined(M_PI_2)
00369
00370 const double CFSImpl::PI = 3.14159265358979323846;
00371 const double CFSImpl::PI_2 = 1.57079632679489661923;
00372 #endif
00373 };
00374
00375
00376
00377 CFSImpl::CFSImpl(WorldBase& world) :
00378 world(world),
00379 randomAngle(boost::mt19937(), uniform_real<>(0.0, 2.0 * PI))
00380 {
00381 maxNumGaussSeidelIteration = DEFAULT_MAX_NUM_GAUSS_SEIDEL_ITERATION;
00382 numGaussSeidelInitialIteration = DEFAULT_NUM_GAUSS_SEIDEL_INITIAL_ITERATION;
00383 gaussSeidelMaxRelError = DEFAULT_GAUSS_SEIDEL_MAX_REL_ERROR;
00384 negativeVelocityRatioForPenetration = DEFAULT_NEGATIVE_VELOCITY_RATIO_FOR_PENETRATION;
00385
00386 isConstraintForceOutputMode = false;
00387 useBuiltinCollisionDetector = false;
00388 allowedPenetrationDepth = ALLOWED_PENETRATION_DEPTH;
00389 }
00390
00391
00392 CFSImpl::~CFSImpl()
00393 {
00394 if(CFS_DEBUG){
00395 os.close();
00396 }
00397 }
00398
00399
00400 bool CFSImpl::addCollisionCheckLinkPair
00401 (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, double muStatic, double muDynamic, double culling_thresh, double restitution, double epsilon)
00402 {
00403 int index;
00404 int isRegistered;
00405
00406 tie(index, isRegistered) = world.getIndexOfLinkPairs(link1, link2);
00407
00408 if(index >= 0){
00409
00410 int n = collisionCheckLinkPairs.size();
00411 if(index >= n){
00412 collisionCheckLinkPairs.resize(index+1);
00413 }
00414
00415 LinkPairPtr& linkPair = collisionCheckLinkPairs[index];
00416 if(!linkPair){
00417 linkPair = new LinkPair();
00418 }
00419
00420 linkPair->set(link1->coldetModel, link2->coldetModel);
00421 linkPair->isSameBodyPair = (bodyIndex1 == bodyIndex2);
00422 linkPair->bodyIndex[0] = bodyIndex1;
00423 linkPair->link[0] = link1;
00424 linkPair->bodyIndex[1] = bodyIndex2;
00425 linkPair->link[1] = link2;
00426 linkPair->index = index;
00427 linkPair->isNonContactConstraint = false;
00428 linkPair->muStatic = muStatic;
00429 linkPair->muDynamic = muDynamic;
00430 linkPair->culling_thresh = culling_thresh;
00431 linkPair->restitution = restitution;
00432 linkPair->epsilon = epsilon;
00433 }
00434
00435 return (index >= 0 && !isRegistered);
00436 }
00437
00438 bool CFSImpl::addExtraJoint(int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, const double* link1LocalPos, const double* link2LocalPos, const short jointType, const double* jointAxis )
00439 {
00440 ExtraJointLinkPairPtr linkPair;
00441 linkPair = new ExtraJointLinkPair();
00442 linkPair->isSameBodyPair = false;
00443 linkPair->isNonContactConstraint = true;
00444 if(jointType == Body::EJ_XY){
00445
00446 Vector3 u = Vector3::Zero();
00447 int minElem = 0;
00448 Vector3 axis( jointAxis[0], jointAxis[1], jointAxis[2] );
00449 for(int k=1; k < 3; k++){
00450 if(fabs(axis(k)) < fabs(axis(minElem))){
00451 minElem = k;
00452 }
00453 }
00454 u(minElem) = 1.0;
00455 linkPair->constraintPoints.resize(2);
00456 const Vector3 t1 = axis.cross(u).normalized();
00457 linkPair->jointConstraintAxes[0] = t1;
00458 linkPair->jointConstraintAxes[1] = axis.cross(t1).normalized();
00459
00460 } else if(jointType == Body::EJ_XYZ){
00461 linkPair->constraintPoints.resize(3);
00462 linkPair->jointConstraintAxes[0] = Vector3(1.0, 0.0, 0.0);
00463 linkPair->jointConstraintAxes[1] = Vector3(0.0, 1.0, 0.0);
00464 linkPair->jointConstraintAxes[2] = Vector3(0.0, 0.0, 1.0);
00465 } else if(jointType == Body::EJ_Z){
00466 linkPair->constraintPoints.resize(1);
00467 linkPair->jointConstraintAxes[0] = Vector3(jointAxis[0], jointAxis[1], jointAxis[2]);
00468 }
00469
00470 if(linkPair){
00471 int numConstraints = linkPair->constraintPoints.size();
00472 for(int k=0; k < numConstraints; ++k){
00473 ConstraintPoint& constraint = linkPair->constraintPoints[k];
00474 constraint.numFrictionVectors = 0;
00475 constraint.globalFrictionIndex = numeric_limits<int>::max();
00476 }
00477 linkPair->bodyIndex[0] = bodyIndex1;
00478 linkPair->bodyIndex[1] = bodyIndex2;
00479 linkPair->link[0] = link1;
00480 linkPair->link[1] = link2;
00481 getVector3(linkPair->jointPoint[0], link1LocalPos);
00482 getVector3(linkPair->jointPoint[1], link2LocalPos);
00483 extraJointLinkPairs.push_back(linkPair);
00484 return true;
00485 }
00486
00487 return false;
00488 }
00489
00490 void CFSImpl::initBody(BodyPtr body, BodyData& bodyData)
00491 {
00492 body->clearExternalForces();
00493 bodyData.body = body;
00494 bodyData.linksData.resize(body->numLinks());
00495 bodyData.hasConstrainedLinks = false;
00496 bodyData.isTestForceBeingApplied = false;
00497 bodyData.isStatic = body->isStaticModel();
00498
00499 LinkDataArray& linksData = bodyData.linksData;
00500 const LinkTraverse& traverse = body->linkTraverse();
00501 for(unsigned int j=0; j < traverse.numLinks(); ++j){
00502 Link* link = traverse[j];
00503 linksData[link->index].link = link;
00504 linksData[link->index].parentIndex = link->parent ? link->parent->index : -1;
00505 }
00506 }
00507
00508
00509
00510 void CFSImpl::initExtraJoints(int bodyIndex)
00511 {
00512 BodyPtr body = world.body(bodyIndex);
00513 int numExtraJoints = body->extraJoints.size();
00514 for(int j=0; j < numExtraJoints; ++j){
00515
00516 Body::ExtraJoint& bodyExtraJoint = body->extraJoints[j];
00517 ExtraJointLinkPairPtr linkPair;
00518 if(bodyExtraJoint.type == Body::EJ_XY){
00519 linkPair = new ExtraJointLinkPair();
00520 linkPair->isSameBodyPair = true;
00521 linkPair->isNonContactConstraint = true;
00522
00523
00524 Vector3 u = Vector3::Zero();
00525 int minElem = 0;
00526 Vector3& axis = bodyExtraJoint.axis;
00527 for(int k=1; k < 3; k++){
00528 if(fabs(axis(k)) < fabs(axis(minElem))){
00529 minElem = k;
00530 }
00531 }
00532 u(minElem) = 1.0;
00533 linkPair->constraintPoints.resize(2);
00534 const Vector3 t1 = axis.cross(u).normalized();
00535 linkPair->jointConstraintAxes[0] = t1;
00536 linkPair->jointConstraintAxes[1] = axis.cross(t1).normalized();
00537
00538 } else if(bodyExtraJoint.type == Body::EJ_XYZ){
00539 linkPair->constraintPoints.resize(3);
00540 linkPair->jointConstraintAxes[0] = Vector3(1.0, 0.0, 0.0);
00541 linkPair->jointConstraintAxes[1] = Vector3(0.0, 1.0, 0.0);
00542 linkPair->jointConstraintAxes[2] = Vector3(0.0, 0.0, 1.0);
00543 } else if(bodyExtraJoint.type == Body::EJ_Z){
00544 linkPair->constraintPoints.resize(1);
00545 linkPair->jointConstraintAxes[0] = bodyExtraJoint.axis;
00546 }
00547
00548 if(linkPair){
00549 int numConstraints = linkPair->constraintPoints.size();
00550 for(int k=0; k < numConstraints; ++k){
00551 ConstraintPoint& constraint = linkPair->constraintPoints[k];
00552 constraint.numFrictionVectors = 0;
00553 constraint.globalFrictionIndex = numeric_limits<int>::max();
00554 }
00555 for(int k=0; k < 2; ++k){
00556 linkPair->bodyIndex[k] = bodyIndex;
00557 Link* link = bodyExtraJoint.link[k];
00558 linkPair->link[k] = link;
00559 linkPair->jointPoint[k] = bodyExtraJoint.point[k];
00560 }
00561 extraJointLinkPairs.push_back(linkPair);
00562 }
00563 }
00564 }
00565
00566 void CFSImpl::initialize(void)
00567 {
00568 if(CFS_DEBUG || CFS_MCP_DEBUG){
00569 static int ntest = 0;
00570 os.close();
00571 os.open((string("cfs-log-") + lexical_cast<string>(ntest++) + ".log").c_str());
00572
00573 }
00574
00575 if(CFS_MCP_DEBUG){
00576 numGaussSeidelTotalCalls = 0;
00577 numGaussSeidelTotalLoops = 0;
00578 numGaussSeidelTotalLoopsMax = 0;
00579 }
00580
00581 int numBodies = world.numBodies();
00582
00583 bodiesData.resize(numBodies);
00584
00585 for(int bodyIndex=0; bodyIndex < numBodies; ++bodyIndex){
00586
00587 BodyPtr body = world.body(bodyIndex);
00588 BodyData& bodyData = bodiesData[bodyIndex];
00589
00590 initBody(body, bodyData);
00591
00592 bodyData.forwardDynamicsMM =
00593 dynamic_pointer_cast<ForwardDynamicsMM>(world.forwardDynamics(bodyIndex));
00594
00595 if(bodyData.isStatic && !(bodyData.forwardDynamicsMM)){
00596 LinkDataArray& linksData = bodyData.linksData;
00597 for(size_t j=0; j < linksData.size(); ++j){
00598 LinkData& linkData = linksData[j];
00599 linkData.dw.setZero();
00600 linkData.dvo.setZero();
00601 }
00602 }
00603
00604 initExtraJoints(bodyIndex);
00605 }
00606
00607 int numLinkPairs = collisionCheckLinkPairs.size();
00608 for(int i=0; i < numLinkPairs; ++i){
00609 LinkPair& linkPair = *collisionCheckLinkPairs[i];
00610 for(int j=0; j < 2; ++j){
00611 BodyData& bodyData = bodiesData[linkPair.bodyIndex[j]];
00612 linkPair.bodyData[j] = &bodyData;
00613 linkPair.linkData[j] = &(bodyData.linksData[linkPair.link[j]->index]);
00614 }
00615 }
00616
00617 numLinkPairs = extraJointLinkPairs.size();
00618 for(int i=0; i < numLinkPairs; ++i){
00619 LinkPair& linkPair = *extraJointLinkPairs[i];
00620 for(int j=0; j < 2; ++j){
00621 BodyData& bodyData = bodiesData[linkPair.bodyIndex[j]];
00622 linkPair.bodyData[j] = &bodyData;
00623 linkPair.linkData[j] = &(bodyData.linksData[linkPair.link[j]->index]);
00624 }
00625 }
00626
00627 prevGlobalNumConstraintVectors = 0;
00628 prevGlobalNumFrictionVectors = 0;
00629 numUnconverged = 0;
00630
00631 randomAngle.engine().seed();
00632 }
00633
00634
00635 inline void CFSImpl::clearExternalForces()
00636 {
00637 for(size_t i=0; i < bodiesData.size(); ++i){
00638 BodyData& bodyData = bodiesData[i];
00639 if(bodyData.hasConstrainedLinks){
00640 bodyData.body->clearExternalForces();
00641 }
00642 }
00643 }
00644
00645
00646 void CFSImpl::solve(CollisionSequence& corbaCollisionSequence)
00647 {
00648 if(CFS_DEBUG)
00649 os << "Time: " << world.currentTime() << std::endl;
00650
00651 for(size_t i=0; i < bodiesData.size(); ++i){
00652 bodiesData[i].hasConstrainedLinks = false;
00653 if(useBuiltinCollisionDetector){
00654 bodiesData[i].body->updateLinkColdetModelPositions();
00655 }
00656 if(isConstraintForceOutputMode){
00657 BodyPtr& body = bodiesData[i].body;
00658 const int n = body->numLinks();
00659 for(int j=0; j < n; ++j){
00660 body->link(j)->constraintForces.clear();
00661 }
00662 }
00663 }
00664
00665 globalNumConstraintVectors = 0;
00666 globalNumFrictionVectors = 0;
00667 areThereImpacts = false;
00668 constrainedLinkPairs.clear();
00669
00670 setConstraintPoints(corbaCollisionSequence);
00671
00672 if(CFS_PUT_NUM_CONTACT_POINTS){
00673 cout << globalNumContactNormalVectors;
00674 }
00675
00676 if(globalNumConstraintVectors > 0){
00677
00678 if(CFS_DEBUG){
00679 os << "Num Collisions: " << globalNumContactNormalVectors << std::endl;
00680 }
00681 if(CFS_DEBUG_VERBOSE) putContactPoints();
00682
00683 const bool constraintsSizeChanged = ((globalNumFrictionVectors != prevGlobalNumFrictionVectors) ||
00684 (globalNumConstraintVectors != prevGlobalNumConstraintVectors));
00685
00686 if(constraintsSizeChanged){
00687 initMatrices();
00688 }
00689
00690 if(areThereImpacts){
00691 solveImpactConstraints();
00692 }
00693
00694 if(SKIP_REDUNDANT_ACCEL_CALC){
00695 setAccelCalcSkipInformation();
00696 }
00697
00698 setDefaultAccelerationVector();
00699 setAccelerationMatrix();
00700
00701 clearSingularPointConstraintsOfClosedLoopConnections();
00702
00703 setConstantVectorAndMuBlock();
00704
00705 if(CFS_DEBUG_VERBOSE){
00706 debugPutVector(an0, "an0");
00707 debugPutVector(at0, "at0");
00708 debugPutMatrix(Mlcp, "Mlcp");
00709 debugPutVector(b.head(globalNumConstraintVectors), "b1");
00710 debugPutVector(b.segment(globalNumConstraintVectors, globalNumFrictionVectors), "b2");
00711 }
00712
00713 bool isConverged;
00714 #ifdef USE_PIVOTING_LCP
00715 isConverged = callPathLCPSolver(Mlcp, b, solution);
00716 #else
00717 if(!USE_PREVIOUS_LCP_SOLUTION || constraintsSizeChanged){
00718 solution.setZero();
00719 }
00720 solveMCPByProjectedGaussSeidel(Mlcp, b, solution);
00721 isConverged = true;
00722 #endif
00723
00724 if(!isConverged){
00725 ++numUnconverged;
00726 if(CFS_DEBUG)
00727 os << "LCP didn't converge" << numUnconverged << std::endl;
00728 } else {
00729 if(CFS_DEBUG)
00730 os << "LCP converged" << std::endl;
00731 if(CFS_DEBUG_LCPCHECK){
00732
00733 checkMCPResult(Mlcp, b, solution);
00734 }
00735
00736 addConstraintForceToLinks();
00737 }
00738 }
00739
00740 prevGlobalNumConstraintVectors = globalNumConstraintVectors;
00741 prevGlobalNumFrictionVectors = globalNumFrictionVectors;
00742 }
00743
00744
00745 void CFSImpl::setConstraintPoints(CollisionSequence& collisions)
00746 {
00747 static const bool enableNormalVisualization = true;
00748
00749 CollisionPointSequence collisionPoints;
00750 CollisionPointSequence* pCollisionPoints = 0;
00751
00752 if(useBuiltinCollisionDetector && enableNormalVisualization){
00753 collisions.length(collisionCheckLinkPairs.size());
00754 }
00755
00756 for(size_t colIndex=0; colIndex < collisionCheckLinkPairs.size(); ++colIndex){
00757 pCollisionPoints = 0;
00758 LinkPair& linkPair = *collisionCheckLinkPairs[colIndex];
00759
00760 if( ! useBuiltinCollisionDetector){
00761 CollisionPointSequence& points = collisions[colIndex].points;
00762 if(points.length() > 0){
00763 pCollisionPoints = &points;
00764 }
00765 } else {
00766 if(enableNormalVisualization){
00767 Collision& collision = collisions[colIndex];
00768 collision.pair.charName1 = CORBA::string_dup(linkPair.bodyData[0]->body->name().c_str());
00769 collision.pair.charName2 = CORBA::string_dup(linkPair.bodyData[1]->body->name().c_str());
00770 collision.pair.linkName1 = CORBA::string_dup(linkPair.link[0]->name.c_str());
00771 collision.pair.linkName2 = CORBA::string_dup(linkPair.link[1]->name.c_str());
00772 pCollisionPoints = &collision.points;
00773 } else {
00774 pCollisionPoints = &collisionPoints;
00775 }
00776
00777 std::vector<collision_data>& cdata = linkPair.detectCollisions();
00778
00779 if(cdata.empty()){
00780 pCollisionPoints->length(0);
00781 pCollisionPoints = 0;
00782 } else {
00783 int npoints = 0;
00784 for(unsigned int i = 0; i < cdata.size(); i++) {
00785 for(int j = 0; j < cdata[i].num_of_i_points; j++){
00786 if(cdata[i].i_point_new[j]) npoints++;
00787 }
00788 }
00789 if(npoints == 0){
00790 pCollisionPoints->length(npoints);
00791 pCollisionPoints = 0;
00792 } else {
00793 pCollisionPoints->length(npoints);
00794 int idx = 0;
00795 for (unsigned int i = 0; i < cdata.size(); i++) {
00796 collision_data& cd = cdata[i];
00797 for(int j=0; j < cd.num_of_i_points; j++){
00798 if (cd.i_point_new[j]){
00799 CollisionPoint& point = (*pCollisionPoints)[idx];
00800 for(int k=0; k < 3; k++){
00801 point.position[k] = cd.i_points[j][k];
00802 }
00803 for(int k=0; k < 3; k++){
00804 point.normal[k] = cd.n_vector[k];
00805 }
00806 point.idepth = cd.depth;
00807 idx++;
00808 }
00809 }
00810 }
00811 }
00812 }
00813 }
00814
00815 if(pCollisionPoints){
00816 constrainedLinkPairs.push_back(&linkPair);
00817 setContactConstraintPoints(linkPair, *pCollisionPoints);
00818 linkPair.bodyData[0]->hasConstrainedLinks = true;
00819 linkPair.bodyData[1]->hasConstrainedLinks = true;
00820 }
00821 }
00822
00823 globalNumContactNormalVectors = globalNumConstraintVectors;
00824
00825 for(size_t i=0; i < extraJointLinkPairs.size(); ++i){
00826 setExtraJointConstraintPoints(extraJointLinkPairs[i]);
00827 }
00828
00829 }
00830
00831
00832 void CFSImpl::setContactConstraintPoints(LinkPair& linkPair, CollisionPointSequence& collisionPoints)
00833 {
00834 ConstraintPointArray& constraintPoints = linkPair.constraintPoints;
00835 constraintPoints.clear();
00836 int numExtractedPoints = 0;
00837 int numContactsInPair = collisionPoints.length();
00838
00839 for(int j=0; j < numContactsInPair; ++j){
00840
00841 CollisionPoint& collision = collisionPoints[j];
00842 constraintPoints.push_back(ConstraintPoint());
00843 ConstraintPoint& contact = constraintPoints.back();
00844
00845 getVector3(contact.point, collision.position);
00846 getVector3(contact.normalTowardInside[1], collision.normal);
00847 contact.normalTowardInside[0] = -contact.normalTowardInside[1];
00848 contact.depth = collision.idepth;
00849
00850 bool isNeighborhood = false;
00851
00852 if(linkPair.culling_thresh > 0.0 )
00853 {
00854
00855 for(int k=0; k < numExtractedPoints; ++k){
00856 if((constraintPoints[k].point - contact.point).norm() < linkPair.culling_thresh){
00857 isNeighborhood = true;
00858 break;
00859 }
00860 }
00861 }
00862
00863
00864 if(isNeighborhood){
00865 constraintPoints.pop_back();
00866 } else {
00867 numExtractedPoints++;
00868 contact.globalIndex = globalNumConstraintVectors++;
00869
00870
00871 Vector3 v[2];
00872 for(int k=0; k < 2; ++k){
00873 Link* link = linkPair.link[k];
00874 if(link->isRoot() && link->jointType == Link::FIXED_JOINT){
00875 v[k].setZero();
00876 } else {
00877 v[k] = link->vo + link->w.cross(contact.point);
00878 if (link->isCrawler){
00879
00880
00881 if (contact.depth > allowedPenetrationDepth*2){
00882 contact.depth = allowedPenetrationDepth*2;
00883 }
00884 Vector3 axis = link->R*link->a;
00885 Vector3 n;
00886 getVector3(n, collision.normal);
00887 Vector3 dir = axis.cross(n);
00888 if (k) dir *= -1;
00889 dir.normalize();
00890 v[k] += link->u*dir;
00891 }
00892 }
00893 }
00894 contact.relVelocityOn0 = v[1] - v[0];
00895
00896 contact.normalProjectionOfRelVelocityOn0 = contact.normalTowardInside[1].dot(contact.relVelocityOn0);
00897
00898 if( ! areThereImpacts){
00899 if(contact.normalProjectionOfRelVelocityOn0 < -1.0e-6){
00900 areThereImpacts = true;
00901 }
00902 }
00903
00904 Vector3 v_tangent(contact.relVelocityOn0 - contact.normalProjectionOfRelVelocityOn0 * contact.normalTowardInside[1]);
00905
00906 contact.globalFrictionIndex = globalNumFrictionVectors;
00907
00908 double vt_square = v_tangent.dot(v_tangent);
00909 static const double vsqrthresh = VEL_THRESH_OF_DYNAMIC_FRICTION * VEL_THRESH_OF_DYNAMIC_FRICTION;
00910 bool isSlipping = (vt_square > vsqrthresh);
00911 contact.mu = isSlipping ? linkPair.muDynamic : linkPair.muStatic;
00912
00913 if( !ONLY_STATIC_FRICTION_FORMULATION && isSlipping){
00914 contact.numFrictionVectors = 1;
00915 double vt_mag = sqrt(vt_square);
00916 Vector3 t1(v_tangent / vt_mag);
00917 Vector3 t2(contact.normalTowardInside[1].cross(t1));
00918 Vector3 t3(t2.cross(contact.normalTowardInside[1]));
00919 contact.frictionVector[0][0] = t3.normalized();
00920 contact.frictionVector[0][1] = -contact.frictionVector[0][0];
00921
00922
00923 if(PROPORTIONAL_DYNAMIC_FRICTION){
00924 vt_mag *= 10000.0;
00925 if(vt_mag < contact.mu){
00926 contact.mu = vt_mag;
00927 }
00928 }
00929 } else {
00930 if(ENABLE_STATIC_FRICTION){
00931 contact.numFrictionVectors = (STATIC_FRICTION_BY_TWO_CONSTRAINTS ? 2 : 4);
00932 setFrictionVectors(contact);
00933 } else {
00934 contact.numFrictionVectors = 0;
00935 }
00936 }
00937 globalNumFrictionVectors += contact.numFrictionVectors;
00938 }
00939 }
00940 }
00941
00942
00943
00944 void CFSImpl::setFrictionVectors(ConstraintPoint& contact)
00945 {
00946 Vector3 u(Vector3::Zero());
00947 int minAxis = 0;
00948 Vector3& normal = contact.normalTowardInside[0];
00949
00950 for(int i=1; i < 3; i++){
00951 if(fabs(normal(i)) < fabs(normal(minAxis))){
00952 minAxis = i;
00953 }
00954 }
00955 u(minAxis) = 1.0;
00956
00957 Vector3 t1(normal.cross(u));
00958 t1.normalize();
00959 Vector3 t2(normal.cross(t1));
00960 t2.normalize();
00961
00962 if(ENABLE_RANDOM_STATIC_FRICTION_BASE){
00963 double theta = randomAngle();
00964 contact.frictionVector[0][0] = cos(theta) * t1 + sin(theta) * t2;
00965 theta += PI_2;
00966 contact.frictionVector[1][0] = cos(theta) * t1 + sin(theta) * t2;
00967 } else {
00968 contact.frictionVector[0][0] = t1;
00969 contact.frictionVector[1][0] = t2;
00970 }
00971
00972 if(STATIC_FRICTION_BY_TWO_CONSTRAINTS){
00973 contact.frictionVector[0][1] = -contact.frictionVector[0][0];
00974 contact.frictionVector[1][1] = -contact.frictionVector[1][0];
00975 } else {
00976 contact.frictionVector[2][0] = -contact.frictionVector[0][0];
00977 contact.frictionVector[3][0] = -contact.frictionVector[1][0];
00978
00979 contact.frictionVector[0][1] = contact.frictionVector[2][0];
00980 contact.frictionVector[1][1] = contact.frictionVector[3][0];
00981 contact.frictionVector[2][1] = contact.frictionVector[0][0];
00982 contact.frictionVector[3][1] = contact.frictionVector[1][0];
00983 }
00984 }
00985
00986 void CFSImpl::setExtraJointConstraintPoints(ExtraJointLinkPairPtr& linkPair)
00987 {
00988 ConstraintPointArray& constraintPoints = linkPair->constraintPoints;
00989
00990 Link* link0 = linkPair->link[0];
00991 Link* link1 = linkPair->link[1];
00992
00993 Vector3 point[2];
00994 point[0].noalias() = link0->p + link0->attitude() * linkPair->jointPoint[0];
00995 point[1].noalias() = link1->p + link1->attitude() * linkPair->jointPoint[1];
00996
00997
00998
00999
01000
01001
01002
01003
01004 Vector3 v[2];
01005 for(int k=0; k < 2; ++k){
01006 Link* link = linkPair->link[k];
01007 if(link->isRoot() && link->jointType == Link::FIXED_JOINT){
01008 v[k].setZero();
01009 } else {
01010 v[k] = link->vo + link->w.cross(point[k]);
01011 }
01012 }
01013 Vector3 relVelocityOn0 = v[1] - v[0];
01014
01015 int n = linkPair->constraintPoints.size();
01016 for(int i=0; i < n; ++i){
01017 ConstraintPoint& constraint = constraintPoints[i];
01018 const Vector3 axis = link0->attitude() * linkPair->jointConstraintAxes[i];
01019 constraint.point = point[1];
01020 constraint.normalTowardInside[0] = axis;
01021 constraint.normalTowardInside[1] = -axis;
01022 constraint.depth = axis.dot(point[1] - point[0]);
01023 constraint.globalIndex = globalNumConstraintVectors++;
01024 constraint.normalProjectionOfRelVelocityOn0 = constraint.normalTowardInside[1].dot(relVelocityOn0);
01025 }
01026 linkPair->bodyData[0]->hasConstrainedLinks = true;
01027 linkPair->bodyData[1]->hasConstrainedLinks = true;
01028
01029 constrainedLinkPairs.push_back(linkPair.get());
01030 }
01031
01032 void CFSImpl::putContactPoints()
01033 {
01034 os << "Contact Points\n";
01035 for(size_t i=0; i < constrainedLinkPairs.size(); ++i){
01036 LinkPair* linkPair = constrainedLinkPairs[i];
01037 ExtraJointLinkPair* ejLinkPair = dynamic_cast<ExtraJointLinkPair*>(linkPair);
01038
01039 if(!ejLinkPair){
01040
01041 os << " " << linkPair->link[0]->name << " of " << linkPair->bodyData[0]->body->modelName();
01042 os << "<-->";
01043 os << " " << linkPair->link[1]->name << " of " << linkPair->bodyData[1]->body->modelName();
01044 os << "\n";
01045 os << " culling thresh: " << linkPair->culling_thresh << "\n";
01046
01047 ConstraintPointArray& constraintPoints = linkPair->constraintPoints;
01048 for(size_t j=0; j < constraintPoints.size(); ++j){
01049 ConstraintPoint& contact = constraintPoints[j];
01050 os << " index " << contact.globalIndex;
01051 os << " point: " << contact.point;
01052 os << " normal: " << contact.normalTowardInside[1];
01053 os << " defaultAccel[0]: " << contact.defaultAccel[0];
01054 os << " defaultAccel[1]: " << contact.defaultAccel[1];
01055 os << " normal projectionOfRelVelocityOn0" << contact.normalProjectionOfRelVelocityOn0;
01056 os << " depth" << contact.depth;
01057 os << " mu" << contact.mu;
01058 os << " rel velocity: " << contact.relVelocityOn0;
01059 os << " friction[0][0]: " << contact.frictionVector[0][0];
01060 os << " friction[0][1]: " << contact.frictionVector[0][1];
01061 os << " friction[1][0]: " << contact.frictionVector[1][0];
01062 os << " friction[1][1]: " << contact.frictionVector[1][1];
01063 os << "\n";
01064 }
01065 }
01066 }
01067 os << std::endl;
01068 }
01069
01070
01071 void CFSImpl::solveImpactConstraints()
01072 {
01073 if(CFS_DEBUG)
01074 os << "Impacts !" << std::endl;
01075 }
01076
01077
01078 void CFSImpl::initMatrices()
01079 {
01080 const int n = globalNumConstraintVectors;
01081 const int m = globalNumFrictionVectors;
01082
01083 const int dimLCP = usePivotingLCP ? (n + m + m) : (n + m);
01084
01085 Mlcp.resize(dimLCP, dimLCP);
01086 b.resize(dimLCP);
01087 solution.resize(dimLCP);
01088
01089 if(usePivotingLCP){
01090
01091 Mlcp.block(0, n + m, n, m).setZero();
01092 Mlcp.block(n + m, 0, m, n).setZero();
01093 Mlcp.block(n + m, n, m, m) = -rmdmatrix::Identity(m, m);
01094 Mlcp.block(n + m, n + m, m, m).setZero();
01095 Mlcp.block(n, n + m, m, m).setIdentity();
01096 b.tail(m).setZero();
01097
01098 } else {
01099 frictionIndexToContactIndex.resize(m);
01100 contactIndexToMu.resize(globalNumContactNormalVectors);
01101 mcpHi.resize(globalNumContactNormalVectors);
01102 }
01103
01104 an0.resize(n);
01105 at0.resize(m);
01106
01107 }
01108
01109
01110 void CFSImpl::setAccelCalcSkipInformation()
01111 {
01112
01113 for(size_t i=0; i < bodiesData.size(); ++i){
01114 BodyData& bodyData = bodiesData[i];
01115 if(bodyData.hasConstrainedLinks){
01116 LinkDataArray& linksData = bodyData.linksData;
01117 for(size_t j=0; j < linksData.size(); ++j){
01118 linksData[j].numberToCheckAccelCalcSkip = numeric_limits<int>::max();
01119 }
01120 }
01121 }
01122
01123
01124 int numLinkPairs = constrainedLinkPairs.size();
01125 for(int i=0; i < numLinkPairs; ++i){
01126 LinkPair* linkPair = constrainedLinkPairs[i];
01127 int constraintIndex = linkPair->constraintPoints.front().globalIndex;
01128 for(int j=0; j < 2; ++j){
01129 LinkDataArray& linksData = linkPair->bodyData[j]->linksData;
01130 int linkIndex = linkPair->link[j]->index;
01131 while(linkIndex >= 0){
01132 LinkData& linkData = linksData[linkIndex];
01133 if(linkData.numberToCheckAccelCalcSkip < constraintIndex){
01134 break;
01135 }
01136 linkData.numberToCheckAccelCalcSkip = constraintIndex;
01137 linkIndex = linkData.parentIndex;
01138 }
01139 }
01140 }
01141 }
01142
01143
01144 void CFSImpl::setDefaultAccelerationVector()
01145 {
01146
01147 for(size_t i=0; i < bodiesData.size(); ++i){
01148 BodyData& bodyData = bodiesData[i];
01149 if(bodyData.hasConstrainedLinks && ! bodyData.isStatic){
01150
01151 if(bodyData.forwardDynamicsMM){
01152
01153 bodyData.rootLinkPosRef = &(bodyData.body->rootLink()->p);
01154 bodyData.forwardDynamicsMM->sumExternalForces();
01155 bodyData.forwardDynamicsMM->solveUnknownAccels();
01156 calcAccelsMM(bodyData, numeric_limits<int>::max());
01157
01158 } else {
01159 initABMForceElementsWithNoExtForce(bodyData);
01160 calcAccelsABM(bodyData, numeric_limits<int>::max());
01161 }
01162 }
01163 }
01164
01165
01166 for(size_t i=0; i < constrainedLinkPairs.size(); ++i){
01167
01168 LinkPair& linkPair = *constrainedLinkPairs[i];
01169 ConstraintPointArray& constraintPoints = linkPair.constraintPoints;
01170
01171 for(size_t j=0; j < constraintPoints.size(); ++j){
01172 ConstraintPoint& constraint = constraintPoints[j];
01173
01174 for(int k=0; k < 2; ++k){
01175 if(linkPair.bodyData[k]->isStatic){
01176 constraint.defaultAccel[k].setZero();
01177 } else {
01178 Link* link = linkPair.link[k];
01179 LinkData* linkData = linkPair.linkData[k];
01180 constraint.defaultAccel[k] =
01181 linkData->dvo - constraint.point.cross(linkData->dw) +
01182 link->w.cross(link->vo + link->w.cross(constraint.point));
01183 }
01184 }
01185
01186 Vector3 relDefaultAccel(constraint.defaultAccel[1] - constraint.defaultAccel[0]);
01187 an0[constraint.globalIndex] = constraint.normalTowardInside[1].dot(relDefaultAccel);
01188
01189 for(int k=0; k < constraint.numFrictionVectors; ++k){
01190 at0[constraint.globalFrictionIndex + k] = constraint.frictionVector[k][1].dot(relDefaultAccel);
01191 }
01192 }
01193 }
01194 }
01195
01196
01197 void CFSImpl::setAccelerationMatrix()
01198 {
01199 const int n = globalNumConstraintVectors;
01200 const int m = globalNumFrictionVectors;
01201
01202 Eigen::Block<rmdmatrix> Knn = Mlcp.block(0, 0, n, n);
01203 Eigen::Block<rmdmatrix> Ktn = Mlcp.block(0, n, n, m);
01204 Eigen::Block<rmdmatrix> Knt = Mlcp.block(n, 0, m, n);
01205 Eigen::Block<rmdmatrix> Ktt = Mlcp.block(n, n, m, m);
01206
01207 for(size_t i=0; i < constrainedLinkPairs.size(); ++i){
01208
01209 LinkPair& linkPair = *constrainedLinkPairs[i];
01210 int numConstraintsInPair = linkPair.constraintPoints.size();
01211
01212 for(int j=0; j < numConstraintsInPair; ++j){
01213
01214 ConstraintPoint& constraint = linkPair.constraintPoints[j];
01215 int constraintIndex = constraint.globalIndex;
01216
01217
01218 for(int k=0; k < 2; ++k){
01219 BodyData& bodyData = *linkPair.bodyData[k];
01220 if(!bodyData.isStatic){
01221
01222 bodyData.isTestForceBeingApplied = true;
01223 const Vector3& f = constraint.normalTowardInside[k];
01224
01225 if(bodyData.forwardDynamicsMM){
01227 Vector3 arm(constraint.point - *(bodyData.rootLinkPosRef));
01228 Vector3 tau(arm.cross(f));
01229 Vector3 tauext = constraint.point.cross(f);
01230 bodyData.forwardDynamicsMM->solveUnknownAccels(linkPair.link[k], f, tauext, f, tau);
01231 calcAccelsMM(bodyData, constraintIndex);
01232 } else {
01233 Vector3 tau(constraint.point.cross(f));
01234 calcABMForceElementsWithTestForce(bodyData, linkPair.link[k], f, tau);
01235 if(!linkPair.isSameBodyPair || (k > 0)){
01236 calcAccelsABM(bodyData, constraintIndex);
01237 }
01238 }
01239 }
01240 }
01241 extractRelAccelsOfConstraintPoints(Knn, Knt, constraintIndex, constraintIndex);
01242
01243
01244 for(int l=0; l < constraint.numFrictionVectors; ++l){
01245 for(int k=0; k < 2; ++k){
01246 BodyData& bodyData = *linkPair.bodyData[k];
01247 if(!bodyData.isStatic){
01248 const Vector3& f = constraint.frictionVector[l][k];
01249
01250 if(bodyData.forwardDynamicsMM){
01252 Vector3 arm(constraint.point - *(bodyData.rootLinkPosRef));
01253 Vector3 tau(arm.cross(f));
01254 Vector3 tauext = constraint.point.cross(f);
01255 bodyData.forwardDynamicsMM->solveUnknownAccels(linkPair.link[k], f, tauext, f, tau);
01256 calcAccelsMM(bodyData, constraintIndex);
01257 } else {
01258 Vector3 tau(constraint.point.cross(f));
01259 calcABMForceElementsWithTestForce(bodyData, linkPair.link[k], f, tau);
01260 if(!linkPair.isSameBodyPair || (k > 0)){
01261 calcAccelsABM(bodyData, constraintIndex);
01262 }
01263 }
01264 }
01265 }
01266 extractRelAccelsOfConstraintPoints(Ktn, Ktt, constraint.globalFrictionIndex + l, constraintIndex);
01267 }
01268
01269 linkPair.bodyData[0]->isTestForceBeingApplied = false;
01270 linkPair.bodyData[1]->isTestForceBeingApplied = false;
01271 }
01272 }
01273
01274 if(ASSUME_SYMMETRIC_MATRIX){
01275 copySymmetricElementsOfAccelerationMatrix(Knn, Ktn, Knt, Ktt);
01276 }
01277 }
01278
01279
01280 void CFSImpl::initABMForceElementsWithNoExtForce(BodyData& bodyData)
01281 {
01282 bodyData.dpf.setZero();
01283 bodyData.dptau.setZero();
01284
01285 std::vector<LinkData>& linksData = bodyData.linksData;
01286 const LinkTraverse& traverse = bodyData.body->linkTraverse();
01287 int n = traverse.numLinks();
01288
01289 for(int i = n-1; i >= 0; --i){
01290 Link* link = traverse[i];
01291 LinkData& data = linksData[i];
01292
01293 data.pf0 = link->pf - link->fext;
01294 data.ptau0 = link->ptau - link->tauext;
01295
01296 for(Link* child = link->child; child; child = child->sibling){
01297
01298 LinkData& childData = linksData[child->index];
01299
01300 data.pf0 += childData.pf0;
01301 data.ptau0 += childData.ptau0;
01302
01303 if(child->jointType != Link::FIXED_JOINT ){
01304 double uu_dd = childData.uu0 / child->dd;
01305 data.pf0 += uu_dd * child->hhv;
01306 data.ptau0 += uu_dd * child->hhw;
01307 }
01308 }
01309
01310 if(i > 0){
01311 if(link->jointType != Link::FIXED_JOINT){
01312 data.uu0 = link->uu + link->u - (link->sv.dot(data.pf0) + link->sw.dot(data.ptau0));
01313 data.uu = data.uu0;
01314 }
01315 }
01316 }
01317 }
01318
01319
01320 void CFSImpl::calcABMForceElementsWithTestForce
01321 (BodyData& bodyData, Link* linkToApplyForce, const Vector3& f, const Vector3& tau)
01322 {
01323 std::vector<LinkData>& linksData = bodyData.linksData;
01324
01325 Vector3 dpf (-f);
01326 Vector3 dptau(-tau);
01327
01328 Link* link = linkToApplyForce;
01329 while(link->parent){
01330 if(link->jointType != Link::FIXED_JOINT){
01331 LinkData& data = linksData[link->index];
01332 double duu = -(link->sv.dot(dpf) + link->sw.dot(dptau));
01333 data.uu += duu;
01334 double duudd = duu / link->dd;
01335 dpf += duudd * link->hhv;
01336 dptau += duudd * link->hhw;
01337 }
01338 link = link->parent;
01339 }
01340
01341 bodyData.dpf += dpf;
01342 bodyData.dptau += dptau;
01343 }
01344
01345
01346 void CFSImpl::calcAccelsABM(BodyData& bodyData, int constraintIndex)
01347 {
01348 std::vector<LinkData>& linksData = bodyData.linksData;
01349 LinkData& rootData = linksData[0];
01350 Link* rootLink = rootData.link;
01351
01352 if(rootLink->jointType == Link::FREE_JOINT){
01353
01354 Eigen::Matrix<double, 6, 6> M;
01355 M << rootLink->Ivv, rootLink->Iwv.transpose(),
01356 rootLink->Iwv, rootLink->Iww;
01357
01358 Eigen::Matrix<double, 6, 1> f;
01359 f << (rootData.pf0 + bodyData.dpf),
01360 (rootData.ptau0 + bodyData.dptau);
01361 f *= -1.0;
01362
01363 Eigen::Matrix<double, 6, 1> a(M.colPivHouseholderQr().solve(f));
01364
01365 rootData.dvo = a.head<3>();
01366 rootData.dw = a.tail<3>();
01367
01368 } else {
01369 rootData.dw.setZero();
01370 rootData.dvo.setZero();
01371 }
01372
01373
01374 bodyData.dpf.setZero();
01375 bodyData.dptau.setZero();
01376
01377 int skipCheckNumber = ASSUME_SYMMETRIC_MATRIX ? constraintIndex : (numeric_limits<int>::max() - 1);
01378 int n = linksData.size();
01379 for(int linkIndex = 1; linkIndex < n; ++linkIndex){
01380
01381 LinkData& linkData = linksData[linkIndex];
01382
01383 if(!SKIP_REDUNDANT_ACCEL_CALC || linkData.numberToCheckAccelCalcSkip <= skipCheckNumber){
01384
01385 Link* link = linkData.link;
01386 LinkData& parentData = linksData[linkData.parentIndex];
01387
01388 if(link->jointType != Link::FIXED_JOINT){
01389 linkData.ddq = (linkData.uu - (link->hhv.dot(parentData.dvo) + link->hhw.dot(parentData.dw))) / link->dd;
01390 linkData.dvo = parentData.dvo + link->cv + link->sv * linkData.ddq;
01391 linkData.dw = parentData.dw + link->cw + link->sw * linkData.ddq;
01392 }else{
01393 linkData.ddq = 0.0;
01394 linkData.dvo = parentData.dvo;
01395 linkData.dw = parentData.dw;
01396 }
01397
01398
01399 linkData.uu = linkData.uu0;
01400 }
01401 }
01402 }
01403
01404
01405 void CFSImpl::calcAccelsMM(BodyData& bodyData, int constraintIndex)
01406 {
01407 std::vector<LinkData>& linksData = bodyData.linksData;
01408
01409 LinkData& rootData = linksData[0];
01410 Link* rootLink = rootData.link;
01411 rootData.dvo = rootLink->dvo;
01412 rootData.dw = rootLink->dw;
01413
01414 int skipCheckNumber = ASSUME_SYMMETRIC_MATRIX ? constraintIndex : (numeric_limits<int>::max() - 1);
01415 int n = linksData.size();
01416
01417 for(int linkIndex = 1; linkIndex < n; ++linkIndex){
01418
01419 LinkData& linkData = linksData[linkIndex];
01420
01421 if(!SKIP_REDUNDANT_ACCEL_CALC || linkData.numberToCheckAccelCalcSkip <= skipCheckNumber){
01422
01423 Link* link = linkData.link;
01424 LinkData& parentData = linksData[linkData.parentIndex];
01425 if(link->jointType != Link::FIXED_JOINT){
01426 linkData.dvo = parentData.dvo + link->cv + link->ddq * link->sv;
01427 linkData.dw = parentData.dw + link->cw + link->ddq * link->sw;
01428 }else{
01429 linkData.dvo = parentData.dvo;
01430 linkData.dw = parentData.dw;
01431 }
01432 }
01433 }
01434 }
01435
01436
01437 void CFSImpl::extractRelAccelsOfConstraintPoints
01438 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, int testForceIndex, int constraintIndex)
01439 {
01440 int maxConstraintIndexToExtract = ASSUME_SYMMETRIC_MATRIX ? constraintIndex : globalNumConstraintVectors;
01441
01442
01443 for(size_t i=0; i < constrainedLinkPairs.size(); ++i){
01444
01445 LinkPair& linkPair = *constrainedLinkPairs[i];
01446
01447 BodyData& bodyData0 = *linkPair.bodyData[0];
01448 BodyData& bodyData1 = *linkPair.bodyData[1];
01449
01450 if(bodyData0.isTestForceBeingApplied){
01451 if(bodyData1.isTestForceBeingApplied){
01452 extractRelAccelsFromLinkPairCase1(Kxn, Kxt, linkPair, testForceIndex, maxConstraintIndexToExtract);
01453 } else {
01454 extractRelAccelsFromLinkPairCase2(Kxn, Kxt, linkPair, 0, 1, testForceIndex, maxConstraintIndexToExtract);
01455 }
01456 } else {
01457 if(bodyData1.isTestForceBeingApplied){
01458 extractRelAccelsFromLinkPairCase2(Kxn, Kxt, linkPair, 1, 0, testForceIndex, maxConstraintIndexToExtract);
01459 } else {
01460 extractRelAccelsFromLinkPairCase3(Kxn, Kxt, linkPair, testForceIndex, maxConstraintIndexToExtract);
01461 }
01462 }
01463 }
01464 }
01465
01466
01467 void CFSImpl::extractRelAccelsFromLinkPairCase1
01468 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, LinkPair& linkPair, int testForceIndex, int maxConstraintIndexToExtract)
01469 {
01470 ConstraintPointArray& constraintPoints = linkPair.constraintPoints;
01471
01472 for(size_t i=0; i < constraintPoints.size(); ++i){
01473
01474 ConstraintPoint& constraint = constraintPoints[i];
01475 int constraintIndex = constraint.globalIndex;
01476
01477 if(ASSUME_SYMMETRIC_MATRIX && constraintIndex > maxConstraintIndexToExtract){
01478 break;
01479 }
01480
01481 Link* link0 = linkPair.link[0];
01482 Link* link1 = linkPair.link[1];
01483 LinkData* linkData0 = linkPair.linkData[0];
01484 LinkData* linkData1 = linkPair.linkData[1];
01485
01487 Vector3 dv0(linkData0->dvo - constraint.point.cross(linkData0->dw) + link0->w.cross(link0->vo + link0->w.cross(constraint.point)));
01488 Vector3 dv1(linkData1->dvo - constraint.point.cross(linkData1->dw) + link1->w.cross(link1->vo + link1->w.cross(constraint.point)));
01489
01490 Vector3 relAccel(dv1 - dv0);
01491
01492 Kxn(constraintIndex, testForceIndex) =
01493 constraint.normalTowardInside[1].dot(relAccel) - an0(constraintIndex);
01494
01495 for(int j=0; j < constraint.numFrictionVectors; ++j){
01496 const int index = constraint.globalFrictionIndex + j;
01497 Kxt(index, testForceIndex) = constraint.frictionVector[j][1].dot(relAccel) - at0(index);
01498 }
01499 }
01500 }
01501
01502
01503 void CFSImpl::extractRelAccelsFromLinkPairCase2
01504 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, LinkPair& linkPair, int iTestForce, int iDefault, int testForceIndex, int maxConstraintIndexToExtract)
01505 {
01506 ConstraintPointArray& constraintPoints = linkPair.constraintPoints;
01507
01508 for(size_t i=0; i < constraintPoints.size(); ++i){
01509
01510 ConstraintPoint& constraint = constraintPoints[i];
01511 int constraintIndex = constraint.globalIndex;
01512
01513 if(ASSUME_SYMMETRIC_MATRIX && constraintIndex > maxConstraintIndexToExtract){
01514 break;
01515 }
01516
01517 Link* link = linkPair.link[iTestForce];
01518 LinkData* linkData = linkPair.linkData[iTestForce];
01519
01520 Vector3 dv(linkData->dvo - constraint.point.cross(linkData->dw) + link->w.cross(link->vo + link->w.cross(constraint.point)));
01521
01522 if(CFS_DEBUG_VERBOSE_2)
01523 os << "dv " << constraintIndex << " = " << dv << "\n";
01524
01525 Vector3 relAccel(constraint.defaultAccel[iDefault] - dv);
01526
01527 Kxn(constraintIndex, testForceIndex) =
01528 constraint.normalTowardInside[iDefault].dot(relAccel) - an0(constraintIndex);
01529
01530 for(int j=0; j < constraint.numFrictionVectors; ++j){
01531 const int index = constraint.globalFrictionIndex + j;
01532 Kxt(index, testForceIndex) =
01533 constraint.frictionVector[j][iDefault].dot(relAccel) - at0(index);
01534 }
01535
01536 }
01537 }
01538
01539
01540 void CFSImpl::extractRelAccelsFromLinkPairCase3
01541 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, LinkPair& linkPair, int testForceIndex, int maxConstraintIndexToExtract)
01542 {
01543 ConstraintPointArray& constraintPoints = linkPair.constraintPoints;
01544
01545 for(size_t i=0; i < constraintPoints.size(); ++i){
01546
01547 ConstraintPoint& constraint = constraintPoints[i];
01548 int constraintIndex = constraint.globalIndex;
01549
01550 if(ASSUME_SYMMETRIC_MATRIX && constraintIndex > maxConstraintIndexToExtract){
01551 break;
01552 }
01553
01554 Kxn(constraintIndex, testForceIndex) = 0.0;
01555
01556 for(int j=0; j < constraint.numFrictionVectors; ++j){
01557 Kxt(constraint.globalFrictionIndex + j, testForceIndex) = 0.0;
01558 }
01559 }
01560 }
01561
01562
01563 void CFSImpl::copySymmetricElementsOfAccelerationMatrix
01564 (Eigen::Block<rmdmatrix>& Knn, Eigen::Block<rmdmatrix>& Ktn, Eigen::Block<rmdmatrix>& Knt, Eigen::Block<rmdmatrix>& Ktt)
01565 {
01566 for(size_t linkPairIndex=0; linkPairIndex < constrainedLinkPairs.size(); ++linkPairIndex){
01567
01568 ConstraintPointArray& constraintPoints = constrainedLinkPairs[linkPairIndex]->constraintPoints;
01569
01570 for(size_t localConstraintIndex = 0; localConstraintIndex < constraintPoints.size(); ++localConstraintIndex){
01571
01572 ConstraintPoint& constraint = constraintPoints[localConstraintIndex];
01573
01574 int constraintIndex = constraint.globalIndex;
01575 int nextConstraintIndex = constraintIndex + 1;
01576 for(int i = nextConstraintIndex; i < globalNumConstraintVectors; ++i){
01577 Knn(i, constraintIndex) = Knn(constraintIndex, i);
01578 }
01579 int frictionTopOfNextConstraint = constraint.globalFrictionIndex + constraint.numFrictionVectors;
01580 for(int i = frictionTopOfNextConstraint; i < globalNumFrictionVectors; ++i){
01581 Knt(i, constraintIndex) = Ktn(constraintIndex, i);
01582 }
01583
01584 for(int localFrictionIndex=0; localFrictionIndex < constraint.numFrictionVectors; ++localFrictionIndex){
01585
01586 int frictionIndex = constraint.globalFrictionIndex + localFrictionIndex;
01587
01588 for(int i = nextConstraintIndex; i < globalNumConstraintVectors; ++i){
01589 Ktn(i, frictionIndex) = Knt(frictionIndex, i);
01590 }
01591 for(int i = frictionTopOfNextConstraint; i < globalNumFrictionVectors; ++i){
01592 Ktt(i, frictionIndex) = Ktt(frictionIndex, i);
01593 }
01594 }
01595 }
01596 }
01597 }
01598
01599
01600 void CFSImpl::clearSingularPointConstraintsOfClosedLoopConnections()
01601 {
01602 for(int i = 0; i < Mlcp.rows(); ++i){
01603 if(Mlcp(i, i) < 1.0e-4){
01604 for(int j=0; j < Mlcp.rows(); ++j){
01605 Mlcp(j, i) = 0.0;
01606 }
01607 Mlcp(i, i) = numeric_limits<double>::max();
01608 }
01609 }
01610 }
01611
01612
01613 void CFSImpl::setConstantVectorAndMuBlock()
01614 {
01615 double dtinv = 1.0 / world.timeStep();
01616 const int block2 = globalNumConstraintVectors;
01617 const int block3 = globalNumConstraintVectors + globalNumFrictionVectors;
01618
01619 for(size_t i=0; i < constrainedLinkPairs.size(); ++i){
01620
01621 LinkPair& linkPair = *constrainedLinkPairs[i];
01622 int numConstraintsInPair = linkPair.constraintPoints.size();
01623
01624 for(int j=0; j < numConstraintsInPair; ++j){
01625 ConstraintPoint& constraint = linkPair.constraintPoints[j];
01626 int globalIndex = constraint.globalIndex;
01627
01628
01629
01630
01631
01632 if(linkPair.isNonContactConstraint){
01633
01634
01635 const double& error = constraint.depth;
01636 double v;
01637 if(error >= 0){
01638 v = 0.1 * (-1.0 + exp(-error * 20.0));
01639 } else {
01640 v = 0.1 * ( 1.0 - exp( error * 20.0));
01641 }
01642
01643 b(globalIndex) = an0(globalIndex) + (constraint.normalProjectionOfRelVelocityOn0 + v) * dtinv;
01644
01645 } else {
01646
01647 if(ALLOW_SUBTLE_PENETRATION_FOR_STABILITY){
01648 double extraNegativeVel;
01649 double newDepth = allowedPenetrationDepth - constraint.depth;
01650 extraNegativeVel = negativeVelocityRatioForPenetration * newDepth;
01651 b(globalIndex) = an0(globalIndex) + ((1 + linkPair.restitution)*constraint.normalProjectionOfRelVelocityOn0 + extraNegativeVel) * dtinv;
01652 } else {
01653 b(globalIndex) = an0(globalIndex) + (1 + linkPair.restitution)*constraint.normalProjectionOfRelVelocityOn0 * dtinv;
01654 }
01655
01656 contactIndexToMu[globalIndex] = constraint.mu;
01657
01658 int globalFrictionIndex = constraint.globalFrictionIndex;
01659 for(int k=0; k < constraint.numFrictionVectors; ++k){
01660
01661
01662 double tangentProjectionOfRelVelocity = constraint.frictionVector[k][1].dot(constraint.relVelocityOn0);
01663
01664 b(block2 + globalFrictionIndex) = at0(globalFrictionIndex);
01665 if( !IGNORE_CURRENT_VELOCITY_IN_STATIC_FRICTION || constraint.numFrictionVectors == 1){
01666 b(block2 + globalFrictionIndex) += tangentProjectionOfRelVelocity * dtinv;
01667 }
01668
01669 if(usePivotingLCP){
01670
01671 Mlcp(block3 + globalFrictionIndex, globalIndex) = constraint.mu;
01672 } else {
01673
01674 frictionIndexToContactIndex[globalFrictionIndex] = globalIndex;
01675 }
01676
01677 ++globalFrictionIndex;
01678 }
01679 }
01680 }
01681 }
01682 }
01683
01684
01685 void CFSImpl::addConstraintForceToLinks()
01686 {
01687 int n = constrainedLinkPairs.size();
01688 for(int i=0; i < n; ++i){
01689 LinkPair* linkPair = constrainedLinkPairs[i];
01690 for(int j=0; j < 2; ++j){
01691
01692 addConstraintForceToLink(linkPair, j);
01693
01694 }
01695 }
01696 }
01697
01698
01699 void CFSImpl::addConstraintForceToLink(LinkPair* linkPair, int ipair)
01700 {
01701 Vector3 f_total(Vector3::Zero());
01702 Vector3 tau_total(Vector3::Zero());
01703
01704 ConstraintPointArray& constraintPoints = linkPair->constraintPoints;
01705 int numConstraintPoints = constraintPoints.size();
01706 Link* link = linkPair->link[ipair];
01707
01708 for(int i=0; i < numConstraintPoints; ++i){
01709
01710 ConstraintPoint& constraint = constraintPoints[i];
01711 int globalIndex = constraint.globalIndex;
01712
01713 Vector3 f(solution(globalIndex) * constraint.normalTowardInside[ipair]);
01714
01715 for(int j=0; j < constraint.numFrictionVectors; ++j){
01716 f += solution(globalNumConstraintVectors + constraint.globalFrictionIndex + j) * constraint.frictionVector[j][ipair];
01717 }
01718
01719 f_total += f;
01720 tau_total += constraint.point.cross(f);
01721
01722 if(isConstraintForceOutputMode){
01723 Link::ConstraintForceArray& constraintForces = link->constraintForces;
01724 constraintForces.resize(constraintForces.size() + 1);
01725 Link::ConstraintForce& cforce = constraintForces.back();
01726 cforce.point = constraint.point;
01727 cforce.force = f;
01728 }
01729 }
01730
01731 link->fext += f_total;
01732 link->tauext += tau_total;
01733
01734
01735 if(CFS_DEBUG){
01736 os << "Constraint force to " << link->name << ": f = " << f_total << ", tau = " << tau_total << std::endl;
01737 }
01738 }
01739
01740
01741
01742 void CFSImpl::solveMCPByProjectedGaussSeidel(const rmdmatrix& M, const dvector& b, dvector& x)
01743 {
01744 static const int loopBlockSize = DEFAULT_NUM_GAUSS_SEIDEL_ITERATION_BLOCK;
01745
01746 if(numGaussSeidelInitialIteration > 0){
01747 solveMCPByProjectedGaussSeidelInitial(M, b, x, numGaussSeidelInitialIteration);
01748 }
01749
01750 int numBlockLoops = maxNumGaussSeidelIteration / loopBlockSize;
01751 if(numBlockLoops==0) numBlockLoops = 1;
01752
01753 if(CFS_MCP_DEBUG) os << "Iteration ";
01754
01755 double error = 0.0;
01756 dvector x0;
01757 int i=0;
01758 while(i < numBlockLoops){
01759 i++;
01760 solveMCPByProjectedGaussSeidelMain(M, b, x, loopBlockSize - 1);
01761
01762 x0 = x;
01763 solveMCPByProjectedGaussSeidelMain(M, b, x, 1);
01764
01765 if(true){
01766 double n = x.norm();
01767 if(n > THRESH_TO_SWITCH_REL_ERROR){
01768 error = (x - x0).norm() / x.norm();
01769 } else {
01770 error = (x - x0).norm();
01771 }
01772 } else {
01773 error = 0.0;
01774 for(int j=0; j < x.size(); ++j){
01775 double d = fabs(x(j) - x0(j));
01776 if(d > THRESH_TO_SWITCH_REL_ERROR){
01777 d /= x(j);
01778 }
01779 if(d > error){
01780 error = d;
01781 }
01782 }
01783 }
01784
01785 if(error < gaussSeidelMaxRelError){
01786 if(CFS_MCP_DEBUG_SHOW_ITERATION_STOP){
01787 os << "stopped at " << (i * loopBlockSize) << ", error = " << error << endl;
01788 }
01789 break;
01790 }
01791 }
01792
01793 if(CFS_MCP_DEBUG){
01794
01795 if(i == numBlockLoops){
01796 os << "not stopped" << ", error = " << error << endl;
01797 }
01798
01799 int n = loopBlockSize * i;
01800 numGaussSeidelTotalLoops += n;
01801 numGaussSeidelTotalCalls++;
01802 numGaussSeidelTotalLoopsMax = std::max(numGaussSeidelTotalLoopsMax, n);
01803 os << ", avarage = " << (numGaussSeidelTotalLoops / numGaussSeidelTotalCalls);
01804 os << ", max = " << numGaussSeidelTotalLoopsMax;
01805 os << endl;
01806 }
01807 }
01808
01809
01810 void CFSImpl::solveMCPByProjectedGaussSeidelInitial
01811 (const rmdmatrix& M, const dvector& b, dvector& x, const int numIteration)
01812 {
01813 const int size = globalNumConstraintVectors + globalNumFrictionVectors;
01814
01815 const double rstep = 1.0 / (numIteration * size);
01816 double r = 0.0;
01817
01818 for(int i=0; i < numIteration; ++i){
01819
01820 for(int j=0; j < globalNumContactNormalVectors; ++j){
01821
01822 double xx;
01823 if(M(j,j)==numeric_limits<double>::max())
01824 xx=0.0;
01825 else{
01826 double sum = -M(j, j) * x(j);
01827 for(int k=0; k < size; ++k){
01828 sum += M(j, k) * x(k);
01829 }
01830 xx = (-b(j) - sum) / M(j, j);
01831 }
01832 if(xx < 0.0){
01833 x(j) = 0.0;
01834 } else {
01835 x(j) = r * xx;
01836 }
01837 r += rstep;
01838 mcpHi[j] = contactIndexToMu[j] * x(j);
01839 }
01840
01841 for(int j=globalNumContactNormalVectors; j < globalNumConstraintVectors; ++j){
01842
01843 if(M(j,j)==numeric_limits<double>::max())
01844 x(j) = 0.0;
01845 else{
01846 double sum = -M(j, j) * x(j);
01847 for(int k=0; k < size; ++k){
01848 sum += M(j, k) * x(k);
01849 }
01850 x(j) = r * (-b(j) - sum) / M(j, j);
01851 }
01852 r += rstep;
01853 }
01854
01855 if(ENABLE_TRUE_FRICTION_CONE){
01856
01857 int contactIndex = 0;
01858 for(int j=globalNumConstraintVectors; j < size; ++j, ++contactIndex){
01859
01860 double fx0;
01861 if(M(j,j)==numeric_limits<double>::max())
01862 fx0 = 0.0;
01863 else{
01864 double sum = -M(j, j) * x(j);
01865 for(int k=0; k < size; ++k){
01866 sum += M(j, k) * x(k);
01867 }
01868 fx0 = (-b(j) - sum) / M(j, j);
01869 }
01870 double& fx = x(j);
01871
01872 ++j;
01873
01874 double fy0;
01875 if(M(j,j)==numeric_limits<double>::max())
01876 fy0 = 0.0;
01877 else{
01878 double sum = -M(j, j) * x(j);
01879 for(int k=0; k < size; ++k){
01880 sum += M(j, k) * x(k);
01881 }
01882 fy0 = (-b(j) - sum) / M(j, j);
01883 }
01884 double& fy = x(j);
01885
01886 const double fmax = mcpHi[contactIndex];
01887 const double fmax2 = fmax * fmax;
01888 const double fmag2 = fx0 * fx0 + fy0 * fy0;
01889
01890 if(fmag2 > fmax2){
01891 const double s = r * fmax / sqrt(fmag2);
01892 fx = s * fx0;
01893 fy = s * fy0;
01894 } else {
01895 fx = r * fx0;
01896 fy = r * fy0;
01897 }
01898 r += (rstep + rstep);
01899 }
01900
01901 } else {
01902
01903 int frictionIndex = 0;
01904 for(int j=globalNumConstraintVectors; j < size; ++j, ++frictionIndex){
01905
01906 double xx;
01907 if(M(j,j)==numeric_limits<double>::max())
01908 xx = 0.0;
01909 else{
01910 double sum = -M(j, j) * x(j);
01911 for(int k=0; k < size; ++k){
01912 sum += M(j, k) * x(k);
01913 }
01914 xx = (-b(j) - sum) / M(j, j);
01915 }
01916
01917 const int contactIndex = frictionIndexToContactIndex[frictionIndex];
01918 const double fmax = mcpHi[contactIndex];
01919 const double fmin = (STATIC_FRICTION_BY_TWO_CONSTRAINTS ? -fmax : 0.0);
01920
01921 if(xx < fmin){
01922 x(j) = fmin;
01923 } else if(xx > fmax){
01924 x(j) = fmax;
01925 } else {
01926 x(j) = xx;
01927 }
01928 x(j) *= r;
01929 r += rstep;
01930 }
01931 }
01932 }
01933 }
01934
01935
01936 void CFSImpl::solveMCPByProjectedGaussSeidelMain
01937 (const rmdmatrix& M, const dvector& b, dvector& x, const int numIteration)
01938 {
01939 const int size = globalNumConstraintVectors + globalNumFrictionVectors;
01940
01941 for(int i=0; i < numIteration; ++i){
01942
01943 for(int j=0; j < globalNumContactNormalVectors; ++j){
01944
01945 double xx;
01946 if(M(j,j)==numeric_limits<double>::max())
01947 xx=0.0;
01948 else{
01949 double sum = -M(j, j) * x(j);
01950 for(int k=0; k < size; ++k){
01951 sum += M(j, k) * x(k);
01952 }
01953 xx = (-b(j) - sum) / M(j, j);
01954 }
01955 if(xx < 0.0){
01956 x(j) = 0.0;
01957 } else {
01958 x(j) = xx;
01959 }
01960 mcpHi[j] = contactIndexToMu[j] * x(j);
01961 }
01962
01963 for(int j=globalNumContactNormalVectors; j < globalNumConstraintVectors; ++j){
01964
01965 if(M(j,j)==numeric_limits<double>::max())
01966 x(j)=0.0;
01967 else{
01968 double sum = -M(j, j) * x(j);
01969 for(int k=0; k < size; ++k){
01970 sum += M(j, k) * x(k);
01971 }
01972 x(j) = (-b(j) - sum) / M(j, j);
01973 }
01974 }
01975
01976
01977 if(ENABLE_TRUE_FRICTION_CONE){
01978
01979 int contactIndex = 0;
01980 for(int j=globalNumConstraintVectors; j < size; ++j, ++contactIndex){
01981
01982 double fx0;
01983 if(M(j,j)==numeric_limits<double>::max())
01984 fx0=0.0;
01985 else{
01986 double sum = -M(j, j) * x(j);
01987 for(int k=0; k < size; ++k){
01988 sum += M(j, k) * x(k);
01989 }
01990 fx0 = (-b(j) - sum) / M(j, j);
01991 }
01992 double& fx = x(j);
01993
01994 ++j;
01995
01996 double fy0;
01997 if(M(j,j)==numeric_limits<double>::max())
01998 fy0=0.0;
01999 else{
02000 double sum = -M(j, j) * x(j);
02001 for(int k=0; k < size; ++k){
02002 sum += M(j, k) * x(k);
02003 }
02004 fy0 = (-b(j) - sum) / M(j, j);
02005 }
02006 double& fy = x(j);
02007
02008 const double fmax = mcpHi[contactIndex];
02009 const double fmax2 = fmax * fmax;
02010 const double fmag2 = fx0 * fx0 + fy0 * fy0;
02011
02012 if(fmag2 > fmax2){
02013 const double s = fmax / sqrt(fmag2);
02014 fx = s * fx0;
02015 fy = s * fy0;
02016 } else {
02017 fx = fx0;
02018 fy = fy0;
02019 }
02020 }
02021
02022 } else {
02023
02024 int frictionIndex = 0;
02025 for(int j=globalNumConstraintVectors; j < size; ++j, ++frictionIndex){
02026
02027 double xx;
02028 if(M(j,j)==numeric_limits<double>::max())
02029 xx=0.0;
02030 else{
02031 double sum = -M(j, j) * x(j);
02032 for(int k=0; k < size; ++k){
02033 sum += M(j, k) * x(k);
02034 }
02035 xx = (-b(j) - sum) / M(j, j);
02036 }
02037
02038 const int contactIndex = frictionIndexToContactIndex[frictionIndex];
02039 const double fmax = mcpHi[contactIndex];
02040 const double fmin = (STATIC_FRICTION_BY_TWO_CONSTRAINTS ? -fmax : 0.0);
02041
02042 if(xx < fmin){
02043 x(j) = fmin;
02044 } else if(xx > fmax){
02045 x(j) = fmax;
02046 } else {
02047 x(j) = xx;
02048 }
02049 }
02050 }
02051 }
02052 }
02053
02054
02055 void CFSImpl::checkLCPResult(rmdmatrix& M, dvector& b, dvector& x)
02056 {
02057 os << "check LCP result\n";
02058 os << "-------------------------------\n";
02059
02060 dvector z = M * x + b;
02061
02062 int n = x.size();
02063 for(int i=0; i < n; ++i){
02064 os << "(" << x(i) << ", " << z(i) << ")";
02065
02066 if(x(i) < 0.0 || z(i) < 0.0 || x(i) * z(i) != 0.0){
02067 os << " - X";
02068 }
02069 os << "\n";
02070
02071 if(i == globalNumConstraintVectors){
02072 os << "-------------------------------\n";
02073 } else if(i == globalNumConstraintVectors + globalNumFrictionVectors){
02074 os << "-------------------------------\n";
02075 }
02076 }
02077
02078 os << "-------------------------------\n";
02079
02080
02081 os << std::endl;
02082 }
02083
02084
02085 void CFSImpl::checkMCPResult(rmdmatrix& M, dvector& b, dvector& x)
02086 {
02087 os << "check MCP result\n";
02088 os << "-------------------------------\n";
02089
02090 dvector z = M * x + b;
02091
02092 for(int i=0; i < globalNumConstraintVectors; ++i){
02093 os << "(" << x(i) << ", " << z(i) << ")";
02094
02095 if(x(i) < 0.0 || z(i) < -1.0e-6){
02096 os << " - X";
02097 } else if(x(i) > 0.0 && fabs(z(i)) > 1.0e-6){
02098 os << " - X";
02099 } else if(z(i) > 1.0e-6 && fabs(x(i)) > 1.0e-6){
02100 os << " - X";
02101 }
02102
02103
02104 os << "\n";
02105 }
02106
02107 os << "-------------------------------\n";
02108
02109 int j = 0;
02110 for(int i=globalNumConstraintVectors; i < globalNumConstraintVectors + globalNumFrictionVectors; ++i, ++j){
02111 os << "(" << x(i) << ", " << z(i) << ")";
02112
02113 int contactIndex = frictionIndexToContactIndex[j];
02114 double hi = contactIndexToMu[contactIndex] * x(contactIndex);
02115
02116 os << " hi = " << hi;
02117
02118 if(x(i) < 0.0 || x(i) > hi){
02119 os << " - X";
02120 } else if(x(i) == hi && z(i) > -1.0e-6){
02121 os << " - X";
02122 } else if(x(i) < hi && x(i) > 0.0 && fabs(z(i)) > 1.0e-6){
02123 os << " - X";
02124 }
02125 os << "\n";
02126 }
02127
02128 os << "-------------------------------\n";
02129
02130 os << std::endl;
02131 }
02132
02133
02134 #ifdef USE_PIVOTING_LCP
02135 bool CFSImpl::callPathLCPSolver(rmdmatrix& Mlcp, dvector& b, dvector& solution)
02136 {
02137 int size = solution.size();
02138 int square = size * size;
02139 std::vector<double> lb(size + 1, 0.0);
02140 std::vector<double> ub(size + 1, 1.0e20);
02141
02142 int m_nnz = 0;
02143 std::vector<int> m_i(square + 1);
02144 std::vector<int> m_j(square + 1);
02145 std::vector<double> m_ij(square + 1);
02146
02147 for(int i=0; i < size; ++i){
02148 solution(i) = 0.0;
02149 }
02150
02151 for(int j=0; j < size; ++j){
02152 for(int i=0; i < size; ++i){
02153 double v = Mlcp(i, j);
02154 if(v != 0.0){
02155 m_i[m_nnz] = i+1;
02156 m_j[m_nnz] = j+1;
02157 m_ij[m_nnz] = v;
02158 ++m_nnz;
02159 }
02160 }
02161 }
02162
02163 MCP_Termination status;
02164
02165 SimpleLCP(size, m_nnz, &m_i[0], &m_j[0], &m_ij[0], &b(0), &lb[0], &ub[0], &status, &solution(0));
02166
02167 return (status == MCP_Solved);
02168 }
02169 #endif
02170
02171
02172 ConstraintForceSolver::ConstraintForceSolver(WorldBase& world)
02173 {
02174 impl = new CFSImpl(world);
02175 }
02176
02177
02178 ConstraintForceSolver::~ConstraintForceSolver()
02179 {
02180 delete impl;
02181 }
02182
02183
02184 bool ConstraintForceSolver::addCollisionCheckLinkPair
02185 (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, double muStatic, double muDynamic, double culling_thresh, double restitution, double epsilon)
02186 {
02187 return impl->addCollisionCheckLinkPair(bodyIndex1, link1, bodyIndex2, link2, muStatic, muDynamic, culling_thresh, restitution, epsilon);
02188 }
02189
02190
02191 void ConstraintForceSolver::clearCollisionCheckLinkPairs()
02192 {
02193 impl->world.clearCollisionPairs();
02194 impl->collisionCheckLinkPairs.clear();
02195 }
02196
02197 bool ConstraintForceSolver::addExtraJoint(int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, const double* link1LocalPos, const double* link2LocalPos, const short jointType, const double* jointAxis )
02198 {
02199 return impl->addExtraJoint(bodyIndex1, link1, bodyIndex2, link2, link1LocalPos, link2LocalPos, jointType, jointAxis );
02200 }
02201
02202
02203 void ConstraintForceSolver::setGaussSeidelParameters(int maxNumIteration, int numInitialIteration, double maxRelError)
02204 {
02205 impl->maxNumGaussSeidelIteration = maxNumIteration;
02206 impl->numGaussSeidelInitialIteration = numInitialIteration;
02207 impl->gaussSeidelMaxRelError = maxRelError;
02208 }
02209
02210
02211 void ConstraintForceSolver::enableConstraintForceOutput(bool on)
02212 {
02213 impl->isConstraintForceOutputMode = on;
02214 }
02215
02216
02217 void ConstraintForceSolver::useBuiltinCollisionDetector(bool on)
02218 {
02219 impl->useBuiltinCollisionDetector = on;
02220 }
02221
02222 void ConstraintForceSolver::setNegativeVelocityRatioForPenetration(double ratio)
02223 {
02224 impl->negativeVelocityRatioForPenetration = ratio;
02225 }
02226
02227
02228
02229 void ConstraintForceSolver::initialize(void)
02230 {
02231 impl->initialize();
02232 }
02233
02234
02235 void ConstraintForceSolver::solve(CollisionSequence& corbaCollisionSequence)
02236 {
02237 impl->solve(corbaCollisionSequence);
02238 }
02239
02240
02241 void ConstraintForceSolver::clearExternalForces()
02242 {
02243 impl->clearExternalForces();
02244 }
02245
02246 void ConstraintForceSolver::setAllowedPenetrationDepth(double dVal)
02247 {
02248 impl->allowedPenetrationDepth = dVal;
02249 }
02250
02251 double ConstraintForceSolver::getAllowedPenetrationDepth() const
02252 {
02253 return impl->allowedPenetrationDepth;
02254 }