ConstraintForceSolver.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
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 // Is LCP solved by Iterative or Pivoting method ?
00049 // #define USE_PIVOTING_LCP
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 // settings
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 // normal setting
00084 static const double ALLOWED_PENETRATION_DEPTH = 0.0001;
00085 //static const double PENETRATION_A = 500.0;
00086 //static const double PENETRATION_B = 80.0;
00087 static const double DEFAULT_NEGATIVE_VELOCITY_RATIO_FOR_PENETRATION = 10.0;
00088 
00089 // test for mobile robots with wheels
00090 //static const double ALLOWED_PENETRATION_DEPTH = 0.005;
00091 //static const double PENETRATION_A = 500.0;
00092 //static const double PENETRATION_B = 80.0;
00093 //static const double NEGATIVE_VELOCITY_RATIO_FOR_PENETRATION = 10.0;
00094 //static const bool ENABLE_CONTACT_POINT_THINNING = false;
00095 
00096 // experimental options
00097 static const bool PROPORTIONAL_DYNAMIC_FRICTION = false;
00098 static const bool ENABLE_RANDOM_STATIC_FRICTION_BASE = false;
00099 
00100 
00101 // debug options
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; // position error in the case of a connection point
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         // Mlcp * solution + b   _|_  solution
00247 
00248         rmdmatrix Mlcp;
00249 
00250         // constant acceleration term when no external force is applied
00251         dvector an0;
00252         dvector at0;
00253 
00254         // constant vector of LCP
00255         dvector b;
00256 
00257         // contact force solution: normal forces at contact points
00258         dvector solution;
00259 
00260         // random number generator
00261         variate_generator<boost::mt19937, uniform_real<> > randomAngle;
00262 
00263         // for special version of gauss sidel iterative solver
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         // for PATH solver
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                         //os << boost::format(" %6.3f ") % M(i, j);
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         // generate two vectors orthogonal to the joint axis
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 // initialize extra joints for making closed links
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             // generate two vectors orthogonal to the joint axis
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         //os << setprecision(50);
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                 // checkLCPResult(Mlcp, b, solution);
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             // dense contact points are eliminated
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             // check velocities
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                         // tentative
00880                         // invalid depths should be fixed
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                 // proportional dynamic friction near zero velocity
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     if(error.squaredNorm() > (0.04 * 0.04)){
00999         return false;
01000     }
01001     */
01002 
01003     // check velocities
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     // clear skip check numbers
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     // add the number of contact points to skip check numbers of the links from a contact target to the root
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     // calculate accelerations with no constraint force
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     // extract accelerations
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             // apply test normal force
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             // apply test friction force
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     // reset
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             // reset
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             // set constant vector of LCP
01629 
01630             // constraints for normal acceleration
01631 
01632             if(linkPair.isNonContactConstraint){
01633                 // connection constraint
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                 // contact constraint
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                     // constraints for tangent acceleration
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                         // set mu (coefficients of friction)
01671                         Mlcp(block3 + globalFrictionIndex, globalIndex) = constraint.mu;
01672                     } else {
01673                         // for iterative solver
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            // if(!linkPair->link[j]->isRoot() || linkPair->link[j]->jointType != Link::FIXED_JOINT){
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 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53