ConstraintForceSolver.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
9 
16 #ifdef __WIN32__
17 #define NOMINMAX
18 #define _USE_MATH_DEFINES // for M_PI
19 #endif
20 
21 #include "World.h"
22 #include "Body.h"
23 #include "Link.h"
24 #include "LinkTraverse.h"
25 #include "ForwardDynamicsCBM.h"
26 #include "ConstraintForceSolver.h"
27 
28 #include <hrpUtil/EigenTypes.h>
29 #include <hrpCorba/OpenHRPCommon.hh>
31 
32 #include <limits>
33 #include <boost/format.hpp>
34 #include <boost/tuple/tuple.hpp>
35 #include <boost/random.hpp>
36 
37 #include <fstream>
38 #include <iomanip>
39 #include <boost/lexical_cast.hpp>
40 
41 using namespace std;
42 using namespace boost;
43 using namespace hrp;
44 using namespace OpenHRP;
45 
46 
47 
48 // Is LCP solved by Iterative or Pivoting method ?
49 // #define USE_PIVOTING_LCP
50 #ifdef USE_PIVOTING_LCP
51 #include "SimpleLCP_Path.h"
52 static const bool usePivotingLCP = true;
53 #else
54 static const bool usePivotingLCP = false;
55 #endif
56 
57 
58 // settings
59 
60 static const double VEL_THRESH_OF_DYNAMIC_FRICTION = 1.0e-4;
61 
62 static const bool ENABLE_STATIC_FRICTION = true;
64 static const bool STATIC_FRICTION_BY_TWO_CONSTRAINTS = true;
66 
67 static const bool ENABLE_TRUE_FRICTION_CONE =
69 
70 static const bool SKIP_REDUNDANT_ACCEL_CALC = true;
71 static const bool ASSUME_SYMMETRIC_MATRIX = false;
72 
76 static const double DEFAULT_GAUSS_SEIDEL_MAX_REL_ERROR = 1.0e-3;
77 
78 static const double THRESH_TO_SWITCH_REL_ERROR = 1.0e-8;
79 static const bool USE_PREVIOUS_LCP_SOLUTION = true;
80 
81 static const bool ALLOW_SUBTLE_PENETRATION_FOR_STABILITY = true;
82 
83 // normal setting
84 static const double ALLOWED_PENETRATION_DEPTH = 0.0001;
85 //static const double PENETRATION_A = 500.0;
86 //static const double PENETRATION_B = 80.0;
88 
89 // test for mobile robots with wheels
90 //static const double ALLOWED_PENETRATION_DEPTH = 0.005;
91 //static const double PENETRATION_A = 500.0;
92 //static const double PENETRATION_B = 80.0;
93 //static const double NEGATIVE_VELOCITY_RATIO_FOR_PENETRATION = 10.0;
94 //static const bool ENABLE_CONTACT_POINT_THINNING = false;
95 
96 // experimental options
97 static const bool PROPORTIONAL_DYNAMIC_FRICTION = false;
98 static const bool ENABLE_RANDOM_STATIC_FRICTION_BASE = false;
99 
100 
101 // debug options
102 static const bool CFS_DEBUG = false;
103 static const bool CFS_DEBUG_VERBOSE = false;
104 static const bool CFS_DEBUG_VERBOSE_2 = false;
105 static const bool CFS_DEBUG_VERBOSE_3 = false;
106 static const bool CFS_DEBUG_LCPCHECK = false;
107 static const bool CFS_MCP_DEBUG = false;
108 static const bool CFS_MCP_DEBUG_SHOW_ITERATION_STOP = false;
109 
110 static const bool CFS_PUT_NUM_CONTACT_POINTS = false;
111 
112 
113 namespace hrp
114 {
115  class CFSImpl
116  {
117  public:
118 
119  CFSImpl(WorldBase& world);
120  ~CFSImpl();
121 
122  bool addCollisionCheckLinkPair
123  (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, double muStatic, double muDynamic, double culling_thresh, double restitution, double epsilon);
124  bool addExtraJoint
125  (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, const double* link1LocalPos, const double* link2LocalPos, const short jointType, const double* jointAxis );
126 
127  void initialize(void);
128  void solve(CollisionSequence& corbaCollisionSequence);
129  inline void clearExternalForces();
130 
131 #undef PI
132 #if defined(M_PI) && defined (M_PI_2)
133 #define PI M_PI
134 #define PI_2 M_PI_2
135 #else
136  static const double PI;
137  static const double PI_2;
138 #endif
139 
141 
144 
148  Vector3 normalTowardInside[2];
149  Vector3 defaultAccel[2];
151  double depth; // position error in the case of a connection point
152 
153  double mu;
157  Vector3 frictionVector[4][2];
158  };
159  typedef std::vector<ConstraintPoint> ConstraintPointArray;
160 
161  struct LinkData
162  {
167  double uu;
168  double uu0;
169  double ddq;
173  };
174  typedef std::vector<LinkData> LinkDataArray;
175 
176  struct BodyData
177  {
179  bool isStatic;
182  LinkDataArray linksData;
183 
186 
189 
197  };
198 
199  std::vector<BodyData> bodiesData;
200 
201  class LinkPair : public ColdetModelPair
202  {
203  public:
204  int index;
206  int bodyIndex[2];
207  BodyData* bodyData[2];
208  Link* link[2];
209  LinkData* linkData[2];
210  ConstraintPointArray constraintPoints;
212  double muStatic;
213  double muDynamic;
215  double restitution;
216  double epsilon;
217  };
218  typedef intrusive_ptr<LinkPair> LinkPairPtr;
219  typedef std::vector<LinkPairPtr> LinkPairArray;
220 
221  LinkPairArray collisionCheckLinkPairs;
222 
224  {
225  public:
226  Vector3 jointPoint[2];
227  Vector3 jointConstraintAxes[3];
228  };
229  typedef intrusive_ptr<ExtraJointLinkPair> ExtraJointLinkPairPtr;
230  vector<ExtraJointLinkPairPtr> extraJointLinkPairs;;
231 
232  std::vector<LinkPair*> constrainedLinkPairs;
233 
235 
238 
241 
244 
245  typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> rmdmatrix;
246  // Mlcp * solution + b _|_ solution
247 
248  rmdmatrix Mlcp;
249 
250  // constant acceleration term when no external force is applied
253 
254  // constant vector of LCP
256 
257  // contact force solution: normal forces at contact points
259 
260  // random number generator
261  variate_generator<boost::mt19937, uniform_real<> > randomAngle;
262 
263  // for special version of gauss sidel iterative solver
264  std::vector<int> frictionIndexToContactIndex;
267 
273 
277 
278  void initBody(BodyPtr body, BodyData& bodyData);
279  void initExtraJoints(int bodyIndex);
280  void setConstraintPoints(CollisionSequence& collisions);
281  void setContactConstraintPoints(LinkPair& linkPair, CollisionPointSequence& collisionPoints);
282  void setFrictionVectors(ConstraintPoint& constraintPoint);
283  void setExtraJointConstraintPoints(ExtraJointLinkPairPtr& linkPair);
284  void putContactPoints();
285  void solveImpactConstraints();
286  void initMatrices();
287  void setAccelCalcSkipInformation();
288  void setDefaultAccelerationVector();
289  void setAccelerationMatrix();
290  void initABMForceElementsWithNoExtForce(BodyData& bodyData);
291  void calcABMForceElementsWithTestForce(BodyData& bodyData, Link* linkToApplyForce, const Vector3& f, const Vector3& tau);
292  void calcAccelsABM(BodyData& bodyData, int constraintIndex);
293  void calcAccelsMM(BodyData& bodyData, int constraintIndex);
294 
295  void extractRelAccelsOfConstraintPoints
296  (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, int testForceIndex, int constraintIndex);
297 
298  void extractRelAccelsFromLinkPairCase1
299  (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, LinkPair& linkPair, int testForceIndex, int constraintIndex);
300  void extractRelAccelsFromLinkPairCase2
301  (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, LinkPair& linkPair, int iTestForce, int iDefault, int testForceIndex, int constraintIndex);
302  void extractRelAccelsFromLinkPairCase3
303  (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, LinkPair& linkPair, int testForceIndex, int constraintIndex);
304 
305  void copySymmetricElementsOfAccelerationMatrix
306  (Eigen::Block<rmdmatrix>& Knn, Eigen::Block<rmdmatrix>& Ktn, Eigen::Block<rmdmatrix>& Knt, Eigen::Block<rmdmatrix>& Ktt);
307 
308  void clearSingularPointConstraintsOfClosedLoopConnections();
309 
310  void setConstantVectorAndMuBlock();
311  void addConstraintForceToLinks();
312  void addConstraintForceToLink(LinkPair* linkPair, int ipair);
313 
314  void solveMCPByProjectedGaussSeidel
315  (const rmdmatrix& M, const dvector& b, dvector& x);
316  void solveMCPByProjectedGaussSeidelInitial
317  (const rmdmatrix& M, const dvector& b, dvector& x, const int numIteration);
318  void solveMCPByProjectedGaussSeidelMain
319  (const rmdmatrix& M, const dvector& b, dvector& x, const int numIteration);
320 
321  void checkLCPResult(rmdmatrix& M, dvector& b, dvector& x);
322  void checkMCPResult(rmdmatrix& M, dvector& b, dvector& x);
323 
324 #ifdef USE_PIVOTING_LCP
325  bool callPathLCPSolver(rmdmatrix& Mlcp, dvector& b, dvector& solution);
326 
327  // for PATH solver
328  std::vector<double> lb;
329  std::vector<double> ub;
330  std::vector<int> m_i;
331  std::vector<int> m_j;
332  std::vector<double> m_ij;
333 #endif
334 
335  ofstream os;
336 
337  template<class TMatrix>
338  void putMatrix(TMatrix& M, const char *name) {
339  if(M.cols() == 1){
340  os << "Vector " << name << M << std::endl;
341  } else {
342  os << "Matrix " << name << ": \n";
343  for(int i=0; i < M.rows(); i++){
344  for(int j=0; j < M.cols(); j++){
345  //os << boost::format(" %6.3f ") % M(i, j);
346  os << boost::format(" %.50g ") % M(i, j);
347  }
348  os << std::endl;
349  }
350  }
351  }
352 
353  template<class TVector>
354  void putVector(const TVector& M, const char *name) {
355  os << "Vector " << name << M << std::endl;
356  }
357 
358  template<class TMatrix>
359  void debugPutMatrix(const TMatrix& M, const char *name) {
360  if(CFS_DEBUG_VERBOSE) putMatrix(M, name);
361  }
362 
363  template<class TVector>
364  void debugPutVector(const TVector& M, const char *name) {
365  if(CFS_DEBUG_VERBOSE) putVector(M, name);
366  }
367  };
368 #if !defined(M_PI) || !defined(M_PI_2)
369 
370  const double CFSImpl::PI = 3.14159265358979323846;
371  const double CFSImpl::PI_2 = 1.57079632679489661923;
372 #endif
373 };
374 
375 
376 
377 CFSImpl::CFSImpl(WorldBase& world) :
378  world(world),
379  randomAngle(boost::mt19937(), uniform_real<>(0.0, 2.0 * PI))
380 {
385 
389 }
390 
391 
393 {
394  if(CFS_DEBUG){
395  os.close();
396  }
397 }
398 
399 
401 (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, double muStatic, double muDynamic, double culling_thresh, double restitution, double epsilon)
402 {
403  int index;
404  int isRegistered;
405 
406  tie(index, isRegistered) = world.getIndexOfLinkPairs(link1, link2);
407 
408  if(index >= 0){
409 
410  int n = collisionCheckLinkPairs.size();
411  if(index >= n){
412  collisionCheckLinkPairs.resize(index+1);
413  }
414 
415  LinkPairPtr& linkPair = collisionCheckLinkPairs[index];
416  if(!linkPair){
417  linkPair = new LinkPair();
418  }
419 
420  linkPair->set(link1->coldetModel, link2->coldetModel);
421  linkPair->isSameBodyPair = (bodyIndex1 == bodyIndex2);
422  linkPair->bodyIndex[0] = bodyIndex1;
423  linkPair->link[0] = link1;
424  linkPair->bodyIndex[1] = bodyIndex2;
425  linkPair->link[1] = link2;
426  linkPair->index = index;
427  linkPair->isNonContactConstraint = false;
428  linkPair->muStatic = muStatic;
429  linkPair->muDynamic = muDynamic;
430  linkPair->culling_thresh = culling_thresh;
431  linkPair->restitution = restitution;
432  linkPair->epsilon = epsilon;
433  }
434 
435  return (index >= 0 && !isRegistered);
436 }
437 
438 bool CFSImpl::addExtraJoint(int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, const double* link1LocalPos, const double* link2LocalPos, const short jointType, const double* jointAxis )
439 {
440  ExtraJointLinkPairPtr linkPair;
441  linkPair = new ExtraJointLinkPair();
442  linkPair->isSameBodyPair = false;
443  linkPair->isNonContactConstraint = true;
444  if(jointType == Body::EJ_XY){
445  // generate two vectors orthogonal to the joint axis
446  Vector3 u = Vector3::Zero();
447  int minElem = 0;
448  Vector3 axis( jointAxis[0], jointAxis[1], jointAxis[2] );
449  for(int k=1; k < 3; k++){
450  if(fabs(axis(k)) < fabs(axis(minElem))){
451  minElem = k;
452  }
453  }
454  u(minElem) = 1.0;
455  linkPair->constraintPoints.resize(2);
456  const Vector3 t1 = axis.cross(u).normalized();
457  linkPair->jointConstraintAxes[0] = t1;
458  linkPair->jointConstraintAxes[1] = axis.cross(t1).normalized();
459 
460  } else if(jointType == Body::EJ_XYZ){
461  linkPair->constraintPoints.resize(3);
462  linkPair->jointConstraintAxes[0] = Vector3(1.0, 0.0, 0.0);
463  linkPair->jointConstraintAxes[1] = Vector3(0.0, 1.0, 0.0);
464  linkPair->jointConstraintAxes[2] = Vector3(0.0, 0.0, 1.0);
465  } else if(jointType == Body::EJ_Z){
466  linkPair->constraintPoints.resize(1);
467  linkPair->jointConstraintAxes[0] = Vector3(jointAxis[0], jointAxis[1], jointAxis[2]);
468  }
469 
470  if(linkPair){
471  int numConstraints = linkPair->constraintPoints.size();
472  for(int k=0; k < numConstraints; ++k){
473  ConstraintPoint& constraint = linkPair->constraintPoints[k];
474  constraint.numFrictionVectors = 0;
476  }
477  linkPair->bodyIndex[0] = bodyIndex1;
478  linkPair->bodyIndex[1] = bodyIndex2;
479  linkPair->link[0] = link1;
480  linkPair->link[1] = link2;
481  getVector3(linkPair->jointPoint[0], link1LocalPos);
482  getVector3(linkPair->jointPoint[1], link2LocalPos);
483  extraJointLinkPairs.push_back(linkPair);
484  return true;
485  }
486 
487  return false;
488 }
489 
490 void CFSImpl::initBody(BodyPtr body, BodyData& bodyData)
491 {
492  body->clearExternalForces();
493  bodyData.body = body;
494  bodyData.linksData.resize(body->numLinks());
495  bodyData.hasConstrainedLinks = false;
496  bodyData.isTestForceBeingApplied = false;
497  bodyData.isStatic = body->isStaticModel();
498 
499  LinkDataArray& linksData = bodyData.linksData;
500  const LinkTraverse& traverse = body->linkTraverse();
501  for(unsigned int j=0; j < traverse.numLinks(); ++j){
502  Link* link = traverse[j];
503  linksData[link->index].link = link;
504  linksData[link->index].parentIndex = link->parent ? link->parent->index : -1;
505  }
506 }
507 
508 
509 // initialize extra joints for making closed links
510 void CFSImpl::initExtraJoints(int bodyIndex)
511 {
512  BodyPtr body = world.body(bodyIndex);
513  int numExtraJoints = body->extraJoints.size();
514  for(int j=0; j < numExtraJoints; ++j){
515 
516  Body::ExtraJoint& bodyExtraJoint = body->extraJoints[j];
517  ExtraJointLinkPairPtr linkPair;
518  if(bodyExtraJoint.type == Body::EJ_XY){
519  linkPair = new ExtraJointLinkPair();
520  linkPair->isSameBodyPair = true;
521  linkPair->isNonContactConstraint = true;
522 
523  // generate two vectors orthogonal to the joint axis
524  Vector3 u = Vector3::Zero();
525  int minElem = 0;
526  Vector3& axis = bodyExtraJoint.axis;
527  for(int k=1; k < 3; k++){
528  if(fabs(axis(k)) < fabs(axis(minElem))){
529  minElem = k;
530  }
531  }
532  u(minElem) = 1.0;
533  linkPair->constraintPoints.resize(2);
534  const Vector3 t1 = axis.cross(u).normalized();
535  linkPair->jointConstraintAxes[0] = t1;
536  linkPair->jointConstraintAxes[1] = axis.cross(t1).normalized();
537 
538  } else if(bodyExtraJoint.type == Body::EJ_XYZ){
539  linkPair->constraintPoints.resize(3);
540  linkPair->jointConstraintAxes[0] = Vector3(1.0, 0.0, 0.0);
541  linkPair->jointConstraintAxes[1] = Vector3(0.0, 1.0, 0.0);
542  linkPair->jointConstraintAxes[2] = Vector3(0.0, 0.0, 1.0);
543  } else if(bodyExtraJoint.type == Body::EJ_Z){
544  linkPair->constraintPoints.resize(1);
545  linkPair->jointConstraintAxes[0] = bodyExtraJoint.axis;
546  }
547 
548  if(linkPair){
549  int numConstraints = linkPair->constraintPoints.size();
550  for(int k=0; k < numConstraints; ++k){
551  ConstraintPoint& constraint = linkPair->constraintPoints[k];
552  constraint.numFrictionVectors = 0;
554  }
555  for(int k=0; k < 2; ++k){
556  linkPair->bodyIndex[k] = bodyIndex;
557  Link* link = bodyExtraJoint.link[k];
558  linkPair->link[k] = link;
559  linkPair->jointPoint[k] = bodyExtraJoint.point[k];
560  }
561  extraJointLinkPairs.push_back(linkPair);
562  }
563  }
564 }
565 
567 {
568  if(CFS_DEBUG || CFS_MCP_DEBUG){
569  static int ntest = 0;
570  os.close();
571  os.open((string("cfs-log-") + lexical_cast<string>(ntest++) + ".log").c_str());
572  //os << setprecision(50);
573  }
574 
575  if(CFS_MCP_DEBUG){
579  }
580 
581  int numBodies = world.numBodies();
582 
583  bodiesData.resize(numBodies);
584 
585  for(int bodyIndex=0; bodyIndex < numBodies; ++bodyIndex){
586 
587  BodyPtr body = world.body(bodyIndex);
588  BodyData& bodyData = bodiesData[bodyIndex];
589 
590  initBody(body, bodyData);
591 
592  bodyData.forwardDynamicsMM =
593  dynamic_pointer_cast<ForwardDynamicsMM>(world.forwardDynamics(bodyIndex));
594 
595  if(bodyData.isStatic && !(bodyData.forwardDynamicsMM)){
596  LinkDataArray& linksData = bodyData.linksData;
597  for(size_t j=0; j < linksData.size(); ++j){
598  LinkData& linkData = linksData[j];
599  linkData.dw.setZero();
600  linkData.dvo.setZero();
601  }
602  }
603 
604  initExtraJoints(bodyIndex);
605  }
606 
607  int numLinkPairs = collisionCheckLinkPairs.size();
608  for(int i=0; i < numLinkPairs; ++i){
609  LinkPair& linkPair = *collisionCheckLinkPairs[i];
610  for(int j=0; j < 2; ++j){
611  BodyData& bodyData = bodiesData[linkPair.bodyIndex[j]];
612  linkPair.bodyData[j] = &bodyData;
613  linkPair.linkData[j] = &(bodyData.linksData[linkPair.link[j]->index]);
614  }
615  }
616 
617  numLinkPairs = extraJointLinkPairs.size();
618  for(int i=0; i < numLinkPairs; ++i){
619  LinkPair& linkPair = *extraJointLinkPairs[i];
620  for(int j=0; j < 2; ++j){
621  BodyData& bodyData = bodiesData[linkPair.bodyIndex[j]];
622  linkPair.bodyData[j] = &bodyData;
623  linkPair.linkData[j] = &(bodyData.linksData[linkPair.link[j]->index]);
624  }
625  }
626 
629  numUnconverged = 0;
630 
631  randomAngle.engine().seed();
632 }
633 
634 
636 {
637  for(size_t i=0; i < bodiesData.size(); ++i){
638  BodyData& bodyData = bodiesData[i];
639  if(bodyData.hasConstrainedLinks){
640  bodyData.body->clearExternalForces();
641  }
642  }
643 }
644 
645 
646 void CFSImpl::solve(CollisionSequence& corbaCollisionSequence)
647 {
648  if(CFS_DEBUG)
649  os << "Time: " << world.currentTime() << std::endl;
650 
651  for(size_t i=0; i < bodiesData.size(); ++i){
652  bodiesData[i].hasConstrainedLinks = false;
654  bodiesData[i].body->updateLinkColdetModelPositions();
655  }
657  BodyPtr& body = bodiesData[i].body;
658  const int n = body->numLinks();
659  for(int j=0; j < n; ++j){
660  body->link(j)->constraintForces.clear();
661  }
662  }
663  }
664 
667  areThereImpacts = false;
668  constrainedLinkPairs.clear();
669 
670  setConstraintPoints(corbaCollisionSequence);
671 
674  }
675 
677 
678  if(CFS_DEBUG){
679  os << "Num Collisions: " << globalNumContactNormalVectors << std::endl;
680  }
682 
683  const bool constraintsSizeChanged = ((globalNumFrictionVectors != prevGlobalNumFrictionVectors) ||
685 
686  if(constraintsSizeChanged){
687  initMatrices();
688  }
689 
690  if(areThereImpacts){
692  }
693 
696  }
697 
700 
702 
704 
705  if(CFS_DEBUG_VERBOSE){
706  debugPutVector(an0, "an0");
707  debugPutVector(at0, "at0");
708  debugPutMatrix(Mlcp, "Mlcp");
711  }
712 
713  bool isConverged;
714 #ifdef USE_PIVOTING_LCP
715  isConverged = callPathLCPSolver(Mlcp, b, solution);
716 #else
717  if(!USE_PREVIOUS_LCP_SOLUTION || constraintsSizeChanged){
718  solution.setZero();
719  }
721  isConverged = true;
722 #endif
723 
724  if(!isConverged){
725  ++numUnconverged;
726  if(CFS_DEBUG)
727  os << "LCP didn't converge" << numUnconverged << std::endl;
728  } else {
729  if(CFS_DEBUG)
730  os << "LCP converged" << std::endl;
731  if(CFS_DEBUG_LCPCHECK){
732  // checkLCPResult(Mlcp, b, solution);
734  }
735 
737  }
738  }
739 
742 }
743 
744 
745 void CFSImpl::setConstraintPoints(CollisionSequence& collisions)
746 {
747  static const bool enableNormalVisualization = true;
748 
749  CollisionPointSequence collisionPoints;
750  CollisionPointSequence* pCollisionPoints = 0;
751 
752  if(useBuiltinCollisionDetector && enableNormalVisualization){
753  collisions.length(collisionCheckLinkPairs.size());
754  }
755 
756  for(size_t colIndex=0; colIndex < collisionCheckLinkPairs.size(); ++colIndex){
757  pCollisionPoints = 0;
758  LinkPair& linkPair = *collisionCheckLinkPairs[colIndex];
759 
761  CollisionPointSequence& points = collisions[colIndex].points;
762  if(points.length() > 0){
763  pCollisionPoints = &points;
764  }
765  } else {
766  if(enableNormalVisualization){
767  Collision& collision = collisions[colIndex];
768  collision.pair.charName1 = CORBA::string_dup(linkPair.bodyData[0]->body->name().c_str());
769  collision.pair.charName2 = CORBA::string_dup(linkPair.bodyData[1]->body->name().c_str());
770  collision.pair.linkName1 = CORBA::string_dup(linkPair.link[0]->name.c_str());
771  collision.pair.linkName2 = CORBA::string_dup(linkPair.link[1]->name.c_str());
772  pCollisionPoints = &collision.points;
773  } else {
774  pCollisionPoints = &collisionPoints;
775  }
776 
777  std::vector<collision_data>& cdata = linkPair.detectCollisions();
778 
779  if(cdata.empty()){
780  pCollisionPoints->length(0);
781  pCollisionPoints = 0;
782  } else {
783  int npoints = 0;
784  for(unsigned int i = 0; i < cdata.size(); i++) {
785  for(int j = 0; j < cdata[i].num_of_i_points; j++){
786  if(cdata[i].i_point_new[j]) npoints++;
787  }
788  }
789  if(npoints == 0){
790  pCollisionPoints->length(npoints);
791  pCollisionPoints = 0;
792  } else {
793  pCollisionPoints->length(npoints);
794  int idx = 0;
795  for (unsigned int i = 0; i < cdata.size(); i++) {
796  collision_data& cd = cdata[i];
797  for(int j=0; j < cd.num_of_i_points; j++){
798  if (cd.i_point_new[j]){
799  CollisionPoint& point = (*pCollisionPoints)[idx];
800  for(int k=0; k < 3; k++){
801  point.position[k] = cd.i_points[j][k];
802  }
803  for(int k=0; k < 3; k++){
804  point.normal[k] = cd.n_vector[k];
805  }
806  point.idepth = cd.depth;
807  idx++;
808  }
809  }
810  }
811  }
812  }
813  }
814 
815  if(pCollisionPoints){
816  constrainedLinkPairs.push_back(&linkPair);
817  setContactConstraintPoints(linkPair, *pCollisionPoints);
818  linkPair.bodyData[0]->hasConstrainedLinks = true;
819  linkPair.bodyData[1]->hasConstrainedLinks = true;
820  }
821  }
822 
824 
825  for(size_t i=0; i < extraJointLinkPairs.size(); ++i){
827  }
828 
829 }
830 
831 
832 void CFSImpl::setContactConstraintPoints(LinkPair& linkPair, CollisionPointSequence& collisionPoints)
833 {
834  ConstraintPointArray& constraintPoints = linkPair.constraintPoints;
835  constraintPoints.clear();
836  int numExtractedPoints = 0;
837  int numContactsInPair = collisionPoints.length();
838 
839  for(int j=0; j < numContactsInPair; ++j){
840 
841  CollisionPoint& collision = collisionPoints[j];
842  constraintPoints.push_back(ConstraintPoint());
843  ConstraintPoint& contact = constraintPoints.back();
844 
845  getVector3(contact.point, collision.position);
846  getVector3(contact.normalTowardInside[1], collision.normal);
847  contact.normalTowardInside[0] = -contact.normalTowardInside[1];
848  contact.depth = collision.idepth;
849 
850  bool isNeighborhood = false;
851 
852  if(linkPair.culling_thresh > 0.0 )
853  {
854  // dense contact points are eliminated
855  for(int k=0; k < numExtractedPoints; ++k){
856  if((constraintPoints[k].point - contact.point).norm() < linkPair.culling_thresh){
857  isNeighborhood = true;
858  break;
859  }
860  }
861  }
862 
863 
864  if(isNeighborhood){
865  constraintPoints.pop_back();
866  } else {
867  numExtractedPoints++;
869 
870  // check velocities
871  Vector3 v[2];
872  for(int k=0; k < 2; ++k){
873  Link* link = linkPair.link[k];
874  if(link->isRoot() && link->jointType == Link::FIXED_JOINT){
875  v[k].setZero();
876  } else {
877  v[k] = link->vo + link->w.cross(contact.point);
878  if (link->isCrawler){
879  // tentative
880  // invalid depths should be fixed
881  if (contact.depth > allowedPenetrationDepth*2){
882  contact.depth = allowedPenetrationDepth*2;
883  }
884  Vector3 axis = link->R*link->a;
885  Vector3 n;
886  getVector3(n, collision.normal);
887  Vector3 dir = axis.cross(n);
888  if (k) dir *= -1;
889  dir.normalize();
890  v[k] += link->u*dir;
891  }
892  }
893  }
894  contact.relVelocityOn0 = v[1] - v[0];
895 
896  contact.normalProjectionOfRelVelocityOn0 = contact.normalTowardInside[1].dot(contact.relVelocityOn0);
897 
898  if( ! areThereImpacts){
899  if(contact.normalProjectionOfRelVelocityOn0 < -1.0e-6){
900  areThereImpacts = true;
901  }
902  }
903 
904  Vector3 v_tangent(contact.relVelocityOn0 - contact.normalProjectionOfRelVelocityOn0 * contact.normalTowardInside[1]);
905 
907 
908  double vt_square = v_tangent.dot(v_tangent);
909  static const double vsqrthresh = VEL_THRESH_OF_DYNAMIC_FRICTION * VEL_THRESH_OF_DYNAMIC_FRICTION;
910  bool isSlipping = (vt_square > vsqrthresh);
911  contact.mu = isSlipping ? linkPair.muDynamic : linkPair.muStatic;
912 
913  if( !ONLY_STATIC_FRICTION_FORMULATION && isSlipping){
914  contact.numFrictionVectors = 1;
915  double vt_mag = sqrt(vt_square);
916  Vector3 t1(v_tangent / vt_mag);
917  Vector3 t2(contact.normalTowardInside[1].cross(t1));
918  Vector3 t3(t2.cross(contact.normalTowardInside[1]));
919  contact.frictionVector[0][0] = t3.normalized();
920  contact.frictionVector[0][1] = -contact.frictionVector[0][0];
921 
922  // proportional dynamic friction near zero velocity
924  vt_mag *= 10000.0;
925  if(vt_mag < contact.mu){
926  contact.mu = vt_mag;
927  }
928  }
929  } else {
932  setFrictionVectors(contact);
933  } else {
934  contact.numFrictionVectors = 0;
935  }
936  }
938  }
939  }
940 }
941 
942 
943 
945 {
946  Vector3 u(Vector3::Zero());
947  int minAxis = 0;
948  Vector3& normal = contact.normalTowardInside[0];
949 
950  for(int i=1; i < 3; i++){
951  if(fabs(normal(i)) < fabs(normal(minAxis))){
952  minAxis = i;
953  }
954  }
955  u(minAxis) = 1.0;
956 
957  Vector3 t1(normal.cross(u));
958  t1.normalize();
959  Vector3 t2(normal.cross(t1));
960  t2.normalize();
961 
963  double theta = randomAngle();
964  contact.frictionVector[0][0] = cos(theta) * t1 + sin(theta) * t2;
965  theta += PI_2;
966  contact.frictionVector[1][0] = cos(theta) * t1 + sin(theta) * t2;
967  } else {
968  contact.frictionVector[0][0] = t1;
969  contact.frictionVector[1][0] = t2;
970  }
971 
973  contact.frictionVector[0][1] = -contact.frictionVector[0][0];
974  contact.frictionVector[1][1] = -contact.frictionVector[1][0];
975  } else {
976  contact.frictionVector[2][0] = -contact.frictionVector[0][0];
977  contact.frictionVector[3][0] = -contact.frictionVector[1][0];
978 
979  contact.frictionVector[0][1] = contact.frictionVector[2][0];
980  contact.frictionVector[1][1] = contact.frictionVector[3][0];
981  contact.frictionVector[2][1] = contact.frictionVector[0][0];
982  contact.frictionVector[3][1] = contact.frictionVector[1][0];
983  }
984 }
985 
987 {
988  ConstraintPointArray& constraintPoints = linkPair->constraintPoints;
989 
990  Link* link0 = linkPair->link[0];
991  Link* link1 = linkPair->link[1];
992 
993  Vector3 point[2];
994  point[0].noalias() = link0->p + link0->attitude() * linkPair->jointPoint[0];
995  point[1].noalias() = link1->p + link1->attitude() * linkPair->jointPoint[1];
996 
997  /*
998  if(error.squaredNorm() > (0.04 * 0.04)){
999  return false;
1000  }
1001  */
1002 
1003  // check velocities
1004  Vector3 v[2];
1005  for(int k=0; k < 2; ++k){
1006  Link* link = linkPair->link[k];
1007  if(link->isRoot() && link->jointType == Link::FIXED_JOINT){
1008  v[k].setZero();
1009  } else {
1010  v[k] = link->vo + link->w.cross(point[k]);
1011  }
1012  }
1013  Vector3 relVelocityOn0 = v[1] - v[0];
1014 
1015  int n = linkPair->constraintPoints.size();
1016  for(int i=0; i < n; ++i){
1017  ConstraintPoint& constraint = constraintPoints[i];
1018  const Vector3 axis = link0->attitude() * linkPair->jointConstraintAxes[i];
1019  constraint.point = point[1];
1020  constraint.normalTowardInside[0] = axis;
1021  constraint.normalTowardInside[1] = -axis;
1022  constraint.depth = axis.dot(point[1] - point[0]);
1023  constraint.globalIndex = globalNumConstraintVectors++;
1024  constraint.normalProjectionOfRelVelocityOn0 = constraint.normalTowardInside[1].dot(relVelocityOn0);
1025  }
1026  linkPair->bodyData[0]->hasConstrainedLinks = true;
1027  linkPair->bodyData[1]->hasConstrainedLinks = true;
1028 
1029  constrainedLinkPairs.push_back(linkPair.get());
1030 }
1031 
1033 {
1034  os << "Contact Points\n";
1035  for(size_t i=0; i < constrainedLinkPairs.size(); ++i){
1036  LinkPair* linkPair = constrainedLinkPairs[i];
1037  ExtraJointLinkPair* ejLinkPair = dynamic_cast<ExtraJointLinkPair*>(linkPair);
1038 
1039  if(!ejLinkPair){
1040 
1041  os << " " << linkPair->link[0]->name << " of " << linkPair->bodyData[0]->body->modelName();
1042  os << "<-->";
1043  os << " " << linkPair->link[1]->name << " of " << linkPair->bodyData[1]->body->modelName();
1044  os << "\n";
1045  os << " culling thresh: " << linkPair->culling_thresh << "\n";
1046 
1047  ConstraintPointArray& constraintPoints = linkPair->constraintPoints;
1048  for(size_t j=0; j < constraintPoints.size(); ++j){
1049  ConstraintPoint& contact = constraintPoints[j];
1050  os << " index " << contact.globalIndex;
1051  os << " point: " << contact.point;
1052  os << " normal: " << contact.normalTowardInside[1];
1053  os << " defaultAccel[0]: " << contact.defaultAccel[0];
1054  os << " defaultAccel[1]: " << contact.defaultAccel[1];
1055  os << " normal projectionOfRelVelocityOn0" << contact.normalProjectionOfRelVelocityOn0;
1056  os << " depth" << contact.depth;
1057  os << " mu" << contact.mu;
1058  os << " rel velocity: " << contact.relVelocityOn0;
1059  os << " friction[0][0]: " << contact.frictionVector[0][0];
1060  os << " friction[0][1]: " << contact.frictionVector[0][1];
1061  os << " friction[1][0]: " << contact.frictionVector[1][0];
1062  os << " friction[1][1]: " << contact.frictionVector[1][1];
1063  os << "\n";
1064  }
1065  }
1066  }
1067  os << std::endl;
1068 }
1069 
1070 
1072 {
1073  if(CFS_DEBUG)
1074  os << "Impacts !" << std::endl;
1075 }
1076 
1077 
1079 {
1080  const int n = globalNumConstraintVectors;
1081  const int m = globalNumFrictionVectors;
1082 
1083  const int dimLCP = usePivotingLCP ? (n + m + m) : (n + m);
1084 
1085  Mlcp.resize(dimLCP, dimLCP);
1086  b.resize(dimLCP);
1087  solution.resize(dimLCP);
1088 
1089  if(usePivotingLCP){
1090 
1091  Mlcp.block(0, n + m, n, m).setZero();
1092  Mlcp.block(n + m, 0, m, n).setZero();
1093  Mlcp.block(n + m, n, m, m) = -rmdmatrix::Identity(m, m);
1094  Mlcp.block(n + m, n + m, m, m).setZero();
1095  Mlcp.block(n, n + m, m, m).setIdentity();
1096  b.tail(m).setZero();
1097 
1098  } else {
1099  frictionIndexToContactIndex.resize(m);
1102  }
1103 
1104  an0.resize(n);
1105  at0.resize(m);
1106 
1107 }
1108 
1109 
1111 {
1112  // clear skip check numbers
1113  for(size_t i=0; i < bodiesData.size(); ++i){
1114  BodyData& bodyData = bodiesData[i];
1115  if(bodyData.hasConstrainedLinks){
1116  LinkDataArray& linksData = bodyData.linksData;
1117  for(size_t j=0; j < linksData.size(); ++j){
1118  linksData[j].numberToCheckAccelCalcSkip = numeric_limits<int>::max();
1119  }
1120  }
1121  }
1122 
1123  // add the number of contact points to skip check numbers of the links from a contact target to the root
1124  int numLinkPairs = constrainedLinkPairs.size();
1125  for(int i=0; i < numLinkPairs; ++i){
1126  LinkPair* linkPair = constrainedLinkPairs[i];
1127  int constraintIndex = linkPair->constraintPoints.front().globalIndex;
1128  for(int j=0; j < 2; ++j){
1129  LinkDataArray& linksData = linkPair->bodyData[j]->linksData;
1130  int linkIndex = linkPair->link[j]->index;
1131  while(linkIndex >= 0){
1132  LinkData& linkData = linksData[linkIndex];
1133  if(linkData.numberToCheckAccelCalcSkip < constraintIndex){
1134  break;
1135  }
1136  linkData.numberToCheckAccelCalcSkip = constraintIndex;
1137  linkIndex = linkData.parentIndex;
1138  }
1139  }
1140  }
1141 }
1142 
1143 
1145 {
1146  // calculate accelerations with no constraint force
1147  for(size_t i=0; i < bodiesData.size(); ++i){
1148  BodyData& bodyData = bodiesData[i];
1149  if(bodyData.hasConstrainedLinks && ! bodyData.isStatic){
1150 
1151  if(bodyData.forwardDynamicsMM){
1152 
1153  bodyData.rootLinkPosRef = &(bodyData.body->rootLink()->p);
1154  bodyData.forwardDynamicsMM->sumExternalForces();
1155  bodyData.forwardDynamicsMM->solveUnknownAccels();
1157 
1158  } else {
1161  }
1162  }
1163  }
1164 
1165  // extract accelerations
1166  for(size_t i=0; i < constrainedLinkPairs.size(); ++i){
1167 
1168  LinkPair& linkPair = *constrainedLinkPairs[i];
1169  ConstraintPointArray& constraintPoints = linkPair.constraintPoints;
1170 
1171  for(size_t j=0; j < constraintPoints.size(); ++j){
1172  ConstraintPoint& constraint = constraintPoints[j];
1173 
1174  for(int k=0; k < 2; ++k){
1175  if(linkPair.bodyData[k]->isStatic){
1176  constraint.defaultAccel[k].setZero();
1177  } else {
1178  Link* link = linkPair.link[k];
1179  LinkData* linkData = linkPair.linkData[k];
1180  constraint.defaultAccel[k] =
1181  linkData->dvo - constraint.point.cross(linkData->dw) +
1182  link->w.cross(link->vo + link->w.cross(constraint.point));
1183  }
1184  }
1185 
1186  Vector3 relDefaultAccel(constraint.defaultAccel[1] - constraint.defaultAccel[0]);
1187  an0[constraint.globalIndex] = constraint.normalTowardInside[1].dot(relDefaultAccel);
1188 
1189  for(int k=0; k < constraint.numFrictionVectors; ++k){
1190  at0[constraint.globalFrictionIndex + k] = constraint.frictionVector[k][1].dot(relDefaultAccel);
1191  }
1192  }
1193  }
1194 }
1195 
1196 
1198 {
1199  const int n = globalNumConstraintVectors;
1200  const int m = globalNumFrictionVectors;
1201 
1202  Eigen::Block<rmdmatrix> Knn = Mlcp.block(0, 0, n, n);
1203  Eigen::Block<rmdmatrix> Ktn = Mlcp.block(0, n, n, m);
1204  Eigen::Block<rmdmatrix> Knt = Mlcp.block(n, 0, m, n);
1205  Eigen::Block<rmdmatrix> Ktt = Mlcp.block(n, n, m, m);
1206 
1207  for(size_t i=0; i < constrainedLinkPairs.size(); ++i){
1208 
1209  LinkPair& linkPair = *constrainedLinkPairs[i];
1210  int numConstraintsInPair = linkPair.constraintPoints.size();
1211 
1212  for(int j=0; j < numConstraintsInPair; ++j){
1213 
1214  ConstraintPoint& constraint = linkPair.constraintPoints[j];
1215  int constraintIndex = constraint.globalIndex;
1216 
1217  // apply test normal force
1218  for(int k=0; k < 2; ++k){
1219  BodyData& bodyData = *linkPair.bodyData[k];
1220  if(!bodyData.isStatic){
1221 
1222  bodyData.isTestForceBeingApplied = true;
1223  const Vector3& f = constraint.normalTowardInside[k];
1224 
1225  if(bodyData.forwardDynamicsMM){
1227  Vector3 arm(constraint.point - *(bodyData.rootLinkPosRef));
1228  Vector3 tau(arm.cross(f));
1229  Vector3 tauext = constraint.point.cross(f);
1230  bodyData.forwardDynamicsMM->solveUnknownAccels(linkPair.link[k], f, tauext, f, tau);
1231  calcAccelsMM(bodyData, constraintIndex);
1232  } else {
1233  Vector3 tau(constraint.point.cross(f));
1234  calcABMForceElementsWithTestForce(bodyData, linkPair.link[k], f, tau);
1235  if(!linkPair.isSameBodyPair || (k > 0)){
1236  calcAccelsABM(bodyData, constraintIndex);
1237  }
1238  }
1239  }
1240  }
1241  extractRelAccelsOfConstraintPoints(Knn, Knt, constraintIndex, constraintIndex);
1242 
1243  // apply test friction force
1244  for(int l=0; l < constraint.numFrictionVectors; ++l){
1245  for(int k=0; k < 2; ++k){
1246  BodyData& bodyData = *linkPair.bodyData[k];
1247  if(!bodyData.isStatic){
1248  const Vector3& f = constraint.frictionVector[l][k];
1249 
1250  if(bodyData.forwardDynamicsMM){
1252  Vector3 arm(constraint.point - *(bodyData.rootLinkPosRef));
1253  Vector3 tau(arm.cross(f));
1254  Vector3 tauext = constraint.point.cross(f);
1255  bodyData.forwardDynamicsMM->solveUnknownAccels(linkPair.link[k], f, tauext, f, tau);
1256  calcAccelsMM(bodyData, constraintIndex);
1257  } else {
1258  Vector3 tau(constraint.point.cross(f));
1259  calcABMForceElementsWithTestForce(bodyData, linkPair.link[k], f, tau);
1260  if(!linkPair.isSameBodyPair || (k > 0)){
1261  calcAccelsABM(bodyData, constraintIndex);
1262  }
1263  }
1264  }
1265  }
1266  extractRelAccelsOfConstraintPoints(Ktn, Ktt, constraint.globalFrictionIndex + l, constraintIndex);
1267  }
1268 
1269  linkPair.bodyData[0]->isTestForceBeingApplied = false;
1270  linkPair.bodyData[1]->isTestForceBeingApplied = false;
1271  }
1272  }
1273 
1275  copySymmetricElementsOfAccelerationMatrix(Knn, Ktn, Knt, Ktt);
1276  }
1277 }
1278 
1279 
1281 {
1282  bodyData.dpf.setZero();
1283  bodyData.dptau.setZero();
1284 
1285  std::vector<LinkData>& linksData = bodyData.linksData;
1286  const LinkTraverse& traverse = bodyData.body->linkTraverse();
1287  int n = traverse.numLinks();
1288 
1289  for(int i = n-1; i >= 0; --i){
1290  Link* link = traverse[i];
1291  LinkData& data = linksData[i];
1292 
1293  data.pf0 = link->pf - link->fext;
1294  data.ptau0 = link->ptau - link->tauext;
1295 
1296  for(Link* child = link->child; child; child = child->sibling){
1297 
1298  LinkData& childData = linksData[child->index];
1299 
1300  data.pf0 += childData.pf0;
1301  data.ptau0 += childData.ptau0;
1302 
1303  if(child->jointType != Link::FIXED_JOINT ){
1304  double uu_dd = childData.uu0 / child->dd;
1305  data.pf0 += uu_dd * child->hhv;
1306  data.ptau0 += uu_dd * child->hhw;
1307  }
1308  }
1309 
1310  if(i > 0){
1311  if(link->jointType != Link::FIXED_JOINT){
1312  data.uu0 = link->uu + link->u - (link->sv.dot(data.pf0) + link->sw.dot(data.ptau0));
1313  data.uu = data.uu0;
1314  }
1315  }
1316  }
1317 }
1318 
1319 
1321 (BodyData& bodyData, Link* linkToApplyForce, const Vector3& f, const Vector3& tau)
1322 {
1323  std::vector<LinkData>& linksData = bodyData.linksData;
1324 
1325  Vector3 dpf (-f);
1326  Vector3 dptau(-tau);
1327 
1328  Link* link = linkToApplyForce;
1329  while(link->parent){
1330  if(link->jointType != Link::FIXED_JOINT){
1331  LinkData& data = linksData[link->index];
1332  double duu = -(link->sv.dot(dpf) + link->sw.dot(dptau));
1333  data.uu += duu;
1334  double duudd = duu / link->dd;
1335  dpf += duudd * link->hhv;
1336  dptau += duudd * link->hhw;
1337  }
1338  link = link->parent;
1339  }
1340 
1341  bodyData.dpf += dpf;
1342  bodyData.dptau += dptau;
1343 }
1344 
1345 
1346 void CFSImpl::calcAccelsABM(BodyData& bodyData, int constraintIndex)
1347 {
1348  std::vector<LinkData>& linksData = bodyData.linksData;
1349  LinkData& rootData = linksData[0];
1350  Link* rootLink = rootData.link;
1351 
1352  if(rootLink->jointType == Link::FREE_JOINT){
1353 
1354  Eigen::Matrix<double, 6, 6> M;
1355  M << rootLink->Ivv, rootLink->Iwv.transpose(),
1356  rootLink->Iwv, rootLink->Iww;
1357 
1358  Eigen::Matrix<double, 6, 1> f;
1359  f << (rootData.pf0 + bodyData.dpf),
1360  (rootData.ptau0 + bodyData.dptau);
1361  f *= -1.0;
1362 
1363  Eigen::Matrix<double, 6, 1> a(M.colPivHouseholderQr().solve(f));
1364 
1365  rootData.dvo = a.head<3>();
1366  rootData.dw = a.tail<3>();
1367 
1368  } else {
1369  rootData.dw.setZero();
1370  rootData.dvo.setZero();
1371  }
1372 
1373  // reset
1374  bodyData.dpf.setZero();
1375  bodyData.dptau.setZero();
1376 
1377  int skipCheckNumber = ASSUME_SYMMETRIC_MATRIX ? constraintIndex : (numeric_limits<int>::max() - 1);
1378  int n = linksData.size();
1379  for(int linkIndex = 1; linkIndex < n; ++linkIndex){
1380 
1381  LinkData& linkData = linksData[linkIndex];
1382 
1383  if(!SKIP_REDUNDANT_ACCEL_CALC || linkData.numberToCheckAccelCalcSkip <= skipCheckNumber){
1384 
1385  Link* link = linkData.link;
1386  LinkData& parentData = linksData[linkData.parentIndex];
1387 
1388  if(link->jointType != Link::FIXED_JOINT){
1389  linkData.ddq = (linkData.uu - (link->hhv.dot(parentData.dvo) + link->hhw.dot(parentData.dw))) / link->dd;
1390  linkData.dvo = parentData.dvo + link->cv + link->sv * linkData.ddq;
1391  linkData.dw = parentData.dw + link->cw + link->sw * linkData.ddq;
1392  }else{
1393  linkData.ddq = 0.0;
1394  linkData.dvo = parentData.dvo;
1395  linkData.dw = parentData.dw;
1396  }
1397 
1398  // reset
1399  linkData.uu = linkData.uu0;
1400  }
1401  }
1402 }
1403 
1404 
1405 void CFSImpl::calcAccelsMM(BodyData& bodyData, int constraintIndex)
1406 {
1407  std::vector<LinkData>& linksData = bodyData.linksData;
1408 
1409  LinkData& rootData = linksData[0];
1410  Link* rootLink = rootData.link;
1411  rootData.dvo = rootLink->dvo;
1412  rootData.dw = rootLink->dw;
1413 
1414  int skipCheckNumber = ASSUME_SYMMETRIC_MATRIX ? constraintIndex : (numeric_limits<int>::max() - 1);
1415  int n = linksData.size();
1416 
1417  for(int linkIndex = 1; linkIndex < n; ++linkIndex){
1418 
1419  LinkData& linkData = linksData[linkIndex];
1420 
1421  if(!SKIP_REDUNDANT_ACCEL_CALC || linkData.numberToCheckAccelCalcSkip <= skipCheckNumber){
1422 
1423  Link* link = linkData.link;
1424  LinkData& parentData = linksData[linkData.parentIndex];
1425  if(link->jointType != Link::FIXED_JOINT){
1426  linkData.dvo = parentData.dvo + link->cv + link->ddq * link->sv;
1427  linkData.dw = parentData.dw + link->cw + link->ddq * link->sw;
1428  }else{
1429  linkData.dvo = parentData.dvo;
1430  linkData.dw = parentData.dw;
1431  }
1432  }
1433  }
1434 }
1435 
1436 
1438 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, int testForceIndex, int constraintIndex)
1439 {
1440  int maxConstraintIndexToExtract = ASSUME_SYMMETRIC_MATRIX ? constraintIndex : globalNumConstraintVectors;
1441 
1442 
1443  for(size_t i=0; i < constrainedLinkPairs.size(); ++i){
1444 
1445  LinkPair& linkPair = *constrainedLinkPairs[i];
1446 
1447  BodyData& bodyData0 = *linkPair.bodyData[0];
1448  BodyData& bodyData1 = *linkPair.bodyData[1];
1449 
1450  if(bodyData0.isTestForceBeingApplied){
1451  if(bodyData1.isTestForceBeingApplied){
1452  extractRelAccelsFromLinkPairCase1(Kxn, Kxt, linkPair, testForceIndex, maxConstraintIndexToExtract);
1453  } else {
1454  extractRelAccelsFromLinkPairCase2(Kxn, Kxt, linkPair, 0, 1, testForceIndex, maxConstraintIndexToExtract);
1455  }
1456  } else {
1457  if(bodyData1.isTestForceBeingApplied){
1458  extractRelAccelsFromLinkPairCase2(Kxn, Kxt, linkPair, 1, 0, testForceIndex, maxConstraintIndexToExtract);
1459  } else {
1460  extractRelAccelsFromLinkPairCase3(Kxn, Kxt, linkPair, testForceIndex, maxConstraintIndexToExtract);
1461  }
1462  }
1463  }
1464 }
1465 
1466 
1468 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, LinkPair& linkPair, int testForceIndex, int maxConstraintIndexToExtract)
1469 {
1470  ConstraintPointArray& constraintPoints = linkPair.constraintPoints;
1471 
1472  for(size_t i=0; i < constraintPoints.size(); ++i){
1473 
1474  ConstraintPoint& constraint = constraintPoints[i];
1475  int constraintIndex = constraint.globalIndex;
1476 
1477  if(ASSUME_SYMMETRIC_MATRIX && constraintIndex > maxConstraintIndexToExtract){
1478  break;
1479  }
1480 
1481  Link* link0 = linkPair.link[0];
1482  Link* link1 = linkPair.link[1];
1483  LinkData* linkData0 = linkPair.linkData[0];
1484  LinkData* linkData1 = linkPair.linkData[1];
1485 
1487  Vector3 dv0(linkData0->dvo - constraint.point.cross(linkData0->dw) + link0->w.cross(link0->vo + link0->w.cross(constraint.point)));
1488  Vector3 dv1(linkData1->dvo - constraint.point.cross(linkData1->dw) + link1->w.cross(link1->vo + link1->w.cross(constraint.point)));
1489 
1490  Vector3 relAccel(dv1 - dv0);
1491 
1492  Kxn(constraintIndex, testForceIndex) =
1493  constraint.normalTowardInside[1].dot(relAccel) - an0(constraintIndex);
1494 
1495  for(int j=0; j < constraint.numFrictionVectors; ++j){
1496  const int index = constraint.globalFrictionIndex + j;
1497  Kxt(index, testForceIndex) = constraint.frictionVector[j][1].dot(relAccel) - at0(index);
1498  }
1499  }
1500 }
1501 
1502 
1504 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, LinkPair& linkPair, int iTestForce, int iDefault, int testForceIndex, int maxConstraintIndexToExtract)
1505 {
1506  ConstraintPointArray& constraintPoints = linkPair.constraintPoints;
1507 
1508  for(size_t i=0; i < constraintPoints.size(); ++i){
1509 
1510  ConstraintPoint& constraint = constraintPoints[i];
1511  int constraintIndex = constraint.globalIndex;
1512 
1513  if(ASSUME_SYMMETRIC_MATRIX && constraintIndex > maxConstraintIndexToExtract){
1514  break;
1515  }
1516 
1517  Link* link = linkPair.link[iTestForce];
1518  LinkData* linkData = linkPair.linkData[iTestForce];
1519 
1520  Vector3 dv(linkData->dvo - constraint.point.cross(linkData->dw) + link->w.cross(link->vo + link->w.cross(constraint.point)));
1521 
1523  os << "dv " << constraintIndex << " = " << dv << "\n";
1524 
1525  Vector3 relAccel(constraint.defaultAccel[iDefault] - dv);
1526 
1527  Kxn(constraintIndex, testForceIndex) =
1528  constraint.normalTowardInside[iDefault].dot(relAccel) - an0(constraintIndex);
1529 
1530  for(int j=0; j < constraint.numFrictionVectors; ++j){
1531  const int index = constraint.globalFrictionIndex + j;
1532  Kxt(index, testForceIndex) =
1533  constraint.frictionVector[j][iDefault].dot(relAccel) - at0(index);
1534  }
1535 
1536  }
1537 }
1538 
1539 
1541 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt, LinkPair& linkPair, int testForceIndex, int maxConstraintIndexToExtract)
1542 {
1543  ConstraintPointArray& constraintPoints = linkPair.constraintPoints;
1544 
1545  for(size_t i=0; i < constraintPoints.size(); ++i){
1546 
1547  ConstraintPoint& constraint = constraintPoints[i];
1548  int constraintIndex = constraint.globalIndex;
1549 
1550  if(ASSUME_SYMMETRIC_MATRIX && constraintIndex > maxConstraintIndexToExtract){
1551  break;
1552  }
1553 
1554  Kxn(constraintIndex, testForceIndex) = 0.0;
1555 
1556  for(int j=0; j < constraint.numFrictionVectors; ++j){
1557  Kxt(constraint.globalFrictionIndex + j, testForceIndex) = 0.0;
1558  }
1559  }
1560 }
1561 
1562 
1564 (Eigen::Block<rmdmatrix>& Knn, Eigen::Block<rmdmatrix>& Ktn, Eigen::Block<rmdmatrix>& Knt, Eigen::Block<rmdmatrix>& Ktt)
1565 {
1566  for(size_t linkPairIndex=0; linkPairIndex < constrainedLinkPairs.size(); ++linkPairIndex){
1567 
1568  ConstraintPointArray& constraintPoints = constrainedLinkPairs[linkPairIndex]->constraintPoints;
1569 
1570  for(size_t localConstraintIndex = 0; localConstraintIndex < constraintPoints.size(); ++localConstraintIndex){
1571 
1572  ConstraintPoint& constraint = constraintPoints[localConstraintIndex];
1573 
1574  int constraintIndex = constraint.globalIndex;
1575  int nextConstraintIndex = constraintIndex + 1;
1576  for(int i = nextConstraintIndex; i < globalNumConstraintVectors; ++i){
1577  Knn(i, constraintIndex) = Knn(constraintIndex, i);
1578  }
1579  int frictionTopOfNextConstraint = constraint.globalFrictionIndex + constraint.numFrictionVectors;
1580  for(int i = frictionTopOfNextConstraint; i < globalNumFrictionVectors; ++i){
1581  Knt(i, constraintIndex) = Ktn(constraintIndex, i);
1582  }
1583 
1584  for(int localFrictionIndex=0; localFrictionIndex < constraint.numFrictionVectors; ++localFrictionIndex){
1585 
1586  int frictionIndex = constraint.globalFrictionIndex + localFrictionIndex;
1587 
1588  for(int i = nextConstraintIndex; i < globalNumConstraintVectors; ++i){
1589  Ktn(i, frictionIndex) = Knt(frictionIndex, i);
1590  }
1591  for(int i = frictionTopOfNextConstraint; i < globalNumFrictionVectors; ++i){
1592  Ktt(i, frictionIndex) = Ktt(frictionIndex, i);
1593  }
1594  }
1595  }
1596  }
1597 }
1598 
1599 
1601 {
1602  for(int i = 0; i < Mlcp.rows(); ++i){
1603  if(Mlcp(i, i) < 1.0e-4){
1604  for(int j=0; j < Mlcp.rows(); ++j){
1605  Mlcp(j, i) = 0.0;
1606  }
1608  }
1609  }
1610 }
1611 
1612 
1614 {
1615  double dtinv = 1.0 / world.timeStep();
1616  const int block2 = globalNumConstraintVectors;
1618 
1619  for(size_t i=0; i < constrainedLinkPairs.size(); ++i){
1620 
1621  LinkPair& linkPair = *constrainedLinkPairs[i];
1622  int numConstraintsInPair = linkPair.constraintPoints.size();
1623 
1624  for(int j=0; j < numConstraintsInPair; ++j){
1625  ConstraintPoint& constraint = linkPair.constraintPoints[j];
1626  int globalIndex = constraint.globalIndex;
1627 
1628  // set constant vector of LCP
1629 
1630  // constraints for normal acceleration
1631 
1632  if(linkPair.isNonContactConstraint){
1633  // connection constraint
1634 
1635  const double& error = constraint.depth;
1636  double v;
1637  if(error >= 0){
1638  v = 0.1 * (-1.0 + exp(-error * 20.0));
1639  } else {
1640  v = 0.1 * ( 1.0 - exp( error * 20.0));
1641  }
1642 
1643  b(globalIndex) = an0(globalIndex) + (constraint.normalProjectionOfRelVelocityOn0 + v) * dtinv;
1644 
1645  } else {
1646  // contact constraint
1648  double extraNegativeVel;
1649  double newDepth = allowedPenetrationDepth - constraint.depth;
1650  extraNegativeVel = negativeVelocityRatioForPenetration * newDepth;
1651  b(globalIndex) = an0(globalIndex) + ((1 + linkPair.restitution)*constraint.normalProjectionOfRelVelocityOn0 + extraNegativeVel) * dtinv;
1652  } else {
1653  b(globalIndex) = an0(globalIndex) + (1 + linkPair.restitution)*constraint.normalProjectionOfRelVelocityOn0 * dtinv;
1654  }
1655 
1656  contactIndexToMu[globalIndex] = constraint.mu;
1657 
1658  int globalFrictionIndex = constraint.globalFrictionIndex;
1659  for(int k=0; k < constraint.numFrictionVectors; ++k){
1660 
1661  // constraints for tangent acceleration
1662  double tangentProjectionOfRelVelocity = constraint.frictionVector[k][1].dot(constraint.relVelocityOn0);
1663 
1664  b(block2 + globalFrictionIndex) = at0(globalFrictionIndex);
1666  b(block2 + globalFrictionIndex) += tangentProjectionOfRelVelocity * dtinv;
1667  }
1668 
1669  if(usePivotingLCP){
1670  // set mu (coefficients of friction)
1671  Mlcp(block3 + globalFrictionIndex, globalIndex) = constraint.mu;
1672  } else {
1673  // for iterative solver
1674  frictionIndexToContactIndex[globalFrictionIndex] = globalIndex;
1675  }
1676 
1677  ++globalFrictionIndex;
1678  }
1679  }
1680  }
1681  }
1682 }
1683 
1684 
1686 {
1687  int n = constrainedLinkPairs.size();
1688  for(int i=0; i < n; ++i){
1689  LinkPair* linkPair = constrainedLinkPairs[i];
1690  for(int j=0; j < 2; ++j){
1691  // if(!linkPair->link[j]->isRoot() || linkPair->link[j]->jointType != Link::FIXED_JOINT){
1692  addConstraintForceToLink(linkPair, j);
1693  // }
1694  }
1695  }
1696 }
1697 
1698 
1700 {
1701  Vector3 f_total(Vector3::Zero());
1702  Vector3 tau_total(Vector3::Zero());
1703 
1704  ConstraintPointArray& constraintPoints = linkPair->constraintPoints;
1705  int numConstraintPoints = constraintPoints.size();
1706  Link* link = linkPair->link[ipair];
1707 
1708  for(int i=0; i < numConstraintPoints; ++i){
1709 
1710  ConstraintPoint& constraint = constraintPoints[i];
1711  int globalIndex = constraint.globalIndex;
1712 
1713  Vector3 f(solution(globalIndex) * constraint.normalTowardInside[ipair]);
1714 
1715  for(int j=0; j < constraint.numFrictionVectors; ++j){
1716  f += solution(globalNumConstraintVectors + constraint.globalFrictionIndex + j) * constraint.frictionVector[j][ipair];
1717  }
1718 
1719  f_total += f;
1720  tau_total += constraint.point.cross(f);
1721 
1723  Link::ConstraintForceArray& constraintForces = link->constraintForces;
1724  constraintForces.resize(constraintForces.size() + 1);
1725  Link::ConstraintForce& cforce = constraintForces.back();
1726  cforce.point = constraint.point;
1727  cforce.force = f;
1728  }
1729  }
1730 
1731  link->fext += f_total;
1732  link->tauext += tau_total;
1733 
1734 
1735  if(CFS_DEBUG){
1736  os << "Constraint force to " << link->name << ": f = " << f_total << ", tau = " << tau_total << std::endl;
1737  }
1738 }
1739 
1740 
1741 
1743 {
1744  static const int loopBlockSize = DEFAULT_NUM_GAUSS_SEIDEL_ITERATION_BLOCK;
1745 
1748  }
1749 
1750  int numBlockLoops = maxNumGaussSeidelIteration / loopBlockSize;
1751  if(numBlockLoops==0) numBlockLoops = 1;
1752 
1753  if(CFS_MCP_DEBUG) os << "Iteration ";
1754 
1755  double error = 0.0;
1756  dvector x0;
1757  int i=0;
1758  while(i < numBlockLoops){
1759  i++;
1760  solveMCPByProjectedGaussSeidelMain(M, b, x, loopBlockSize - 1);
1761 
1762  x0 = x;
1764 
1765  if(true){
1766  double n = x.norm();
1768  error = (x - x0).norm() / x.norm();
1769  } else {
1770  error = (x - x0).norm();
1771  }
1772  } else {
1773  error = 0.0;
1774  for(int j=0; j < x.size(); ++j){
1775  double d = fabs(x(j) - x0(j));
1777  d /= x(j);
1778  }
1779  if(d > error){
1780  error = d;
1781  }
1782  }
1783  }
1784 
1785  if(error < gaussSeidelMaxRelError){
1787  os << "stopped at " << (i * loopBlockSize) << ", error = " << error << endl;
1788  }
1789  break;
1790  }
1791  }
1792 
1793  if(CFS_MCP_DEBUG){
1794 
1795  if(i == numBlockLoops){
1796  os << "not stopped" << ", error = " << error << endl;
1797  }
1798 
1799  int n = loopBlockSize * i;
1803  os << ", avarage = " << (numGaussSeidelTotalLoops / numGaussSeidelTotalCalls);
1804  os << ", max = " << numGaussSeidelTotalLoopsMax;
1805  os << endl;
1806  }
1807 }
1808 
1809 
1811 (const rmdmatrix& M, const dvector& b, dvector& x, const int numIteration)
1812 {
1814 
1815  const double rstep = 1.0 / (numIteration * size);
1816  double r = 0.0;
1817 
1818  for(int i=0; i < numIteration; ++i){
1819 
1820  for(int j=0; j < globalNumContactNormalVectors; ++j){
1821 
1822  double xx;
1823  if(M(j,j)==numeric_limits<double>::max())
1824  xx=0.0;
1825  else{
1826  double sum = -M(j, j) * x(j);
1827  for(int k=0; k < size; ++k){
1828  sum += M(j, k) * x(k);
1829  }
1830  xx = (-b(j) - sum) / M(j, j);
1831  }
1832  if(xx < 0.0){
1833  x(j) = 0.0;
1834  } else {
1835  x(j) = r * xx;
1836  }
1837  r += rstep;
1838  mcpHi[j] = contactIndexToMu[j] * x(j);
1839  }
1840 
1841  for(int j=globalNumContactNormalVectors; j < globalNumConstraintVectors; ++j){
1842 
1843  if(M(j,j)==numeric_limits<double>::max())
1844  x(j) = 0.0;
1845  else{
1846  double sum = -M(j, j) * x(j);
1847  for(int k=0; k < size; ++k){
1848  sum += M(j, k) * x(k);
1849  }
1850  x(j) = r * (-b(j) - sum) / M(j, j);
1851  }
1852  r += rstep;
1853  }
1854 
1856 
1857  int contactIndex = 0;
1858  for(int j=globalNumConstraintVectors; j < size; ++j, ++contactIndex){
1859 
1860  double fx0;
1861  if(M(j,j)==numeric_limits<double>::max())
1862  fx0 = 0.0;
1863  else{
1864  double sum = -M(j, j) * x(j);
1865  for(int k=0; k < size; ++k){
1866  sum += M(j, k) * x(k);
1867  }
1868  fx0 = (-b(j) - sum) / M(j, j);
1869  }
1870  double& fx = x(j);
1871 
1872  ++j;
1873 
1874  double fy0;
1875  if(M(j,j)==numeric_limits<double>::max())
1876  fy0 = 0.0;
1877  else{
1878  double sum = -M(j, j) * x(j);
1879  for(int k=0; k < size; ++k){
1880  sum += M(j, k) * x(k);
1881  }
1882  fy0 = (-b(j) - sum) / M(j, j);
1883  }
1884  double& fy = x(j);
1885 
1886  const double fmax = mcpHi[contactIndex];
1887  const double fmax2 = fmax * fmax;
1888  const double fmag2 = fx0 * fx0 + fy0 * fy0;
1889 
1890  if(fmag2 > fmax2){
1891  const double s = r * fmax / sqrt(fmag2);
1892  fx = s * fx0;
1893  fy = s * fy0;
1894  } else {
1895  fx = r * fx0;
1896  fy = r * fy0;
1897  }
1898  r += (rstep + rstep);
1899  }
1900 
1901  } else {
1902 
1903  int frictionIndex = 0;
1904  for(int j=globalNumConstraintVectors; j < size; ++j, ++frictionIndex){
1905 
1906  double xx;
1907  if(M(j,j)==numeric_limits<double>::max())
1908  xx = 0.0;
1909  else{
1910  double sum = -M(j, j) * x(j);
1911  for(int k=0; k < size; ++k){
1912  sum += M(j, k) * x(k);
1913  }
1914  xx = (-b(j) - sum) / M(j, j);
1915  }
1916 
1917  const int contactIndex = frictionIndexToContactIndex[frictionIndex];
1918  const double fmax = mcpHi[contactIndex];
1919  const double fmin = (STATIC_FRICTION_BY_TWO_CONSTRAINTS ? -fmax : 0.0);
1920 
1921  if(xx < fmin){
1922  x(j) = fmin;
1923  } else if(xx > fmax){
1924  x(j) = fmax;
1925  } else {
1926  x(j) = xx;
1927  }
1928  x(j) *= r;
1929  r += rstep;
1930  }
1931  }
1932  }
1933 }
1934 
1935 
1937 (const rmdmatrix& M, const dvector& b, dvector& x, const int numIteration)
1938 {
1940 
1941  for(int i=0; i < numIteration; ++i){
1942 
1943  for(int j=0; j < globalNumContactNormalVectors; ++j){
1944 
1945  double xx;
1946  if(M(j,j)==numeric_limits<double>::max())
1947  xx=0.0;
1948  else{
1949  double sum = -M(j, j) * x(j);
1950  for(int k=0; k < size; ++k){
1951  sum += M(j, k) * x(k);
1952  }
1953  xx = (-b(j) - sum) / M(j, j);
1954  }
1955  if(xx < 0.0){
1956  x(j) = 0.0;
1957  } else {
1958  x(j) = xx;
1959  }
1960  mcpHi[j] = contactIndexToMu[j] * x(j);
1961  }
1962 
1963  for(int j=globalNumContactNormalVectors; j < globalNumConstraintVectors; ++j){
1964 
1965  if(M(j,j)==numeric_limits<double>::max())
1966  x(j)=0.0;
1967  else{
1968  double sum = -M(j, j) * x(j);
1969  for(int k=0; k < size; ++k){
1970  sum += M(j, k) * x(k);
1971  }
1972  x(j) = (-b(j) - sum) / M(j, j);
1973  }
1974  }
1975 
1976 
1978 
1979  int contactIndex = 0;
1980  for(int j=globalNumConstraintVectors; j < size; ++j, ++contactIndex){
1981 
1982  double fx0;
1983  if(M(j,j)==numeric_limits<double>::max())
1984  fx0=0.0;
1985  else{
1986  double sum = -M(j, j) * x(j);
1987  for(int k=0; k < size; ++k){
1988  sum += M(j, k) * x(k);
1989  }
1990  fx0 = (-b(j) - sum) / M(j, j);
1991  }
1992  double& fx = x(j);
1993 
1994  ++j;
1995 
1996  double fy0;
1997  if(M(j,j)==numeric_limits<double>::max())
1998  fy0=0.0;
1999  else{
2000  double sum = -M(j, j) * x(j);
2001  for(int k=0; k < size; ++k){
2002  sum += M(j, k) * x(k);
2003  }
2004  fy0 = (-b(j) - sum) / M(j, j);
2005  }
2006  double& fy = x(j);
2007 
2008  const double fmax = mcpHi[contactIndex];
2009  const double fmax2 = fmax * fmax;
2010  const double fmag2 = fx0 * fx0 + fy0 * fy0;
2011 
2012  if(fmag2 > fmax2){
2013  const double s = fmax / sqrt(fmag2);
2014  fx = s * fx0;
2015  fy = s * fy0;
2016  } else {
2017  fx = fx0;
2018  fy = fy0;
2019  }
2020  }
2021 
2022  } else {
2023 
2024  int frictionIndex = 0;
2025  for(int j=globalNumConstraintVectors; j < size; ++j, ++frictionIndex){
2026 
2027  double xx;
2028  if(M(j,j)==numeric_limits<double>::max())
2029  xx=0.0;
2030  else{
2031  double sum = -M(j, j) * x(j);
2032  for(int k=0; k < size; ++k){
2033  sum += M(j, k) * x(k);
2034  }
2035  xx = (-b(j) - sum) / M(j, j);
2036  }
2037 
2038  const int contactIndex = frictionIndexToContactIndex[frictionIndex];
2039  const double fmax = mcpHi[contactIndex];
2040  const double fmin = (STATIC_FRICTION_BY_TWO_CONSTRAINTS ? -fmax : 0.0);
2041 
2042  if(xx < fmin){
2043  x(j) = fmin;
2044  } else if(xx > fmax){
2045  x(j) = fmax;
2046  } else {
2047  x(j) = xx;
2048  }
2049  }
2050  }
2051  }
2052 }
2053 
2054 
2056 {
2057  os << "check LCP result\n";
2058  os << "-------------------------------\n";
2059 
2060  dvector z = M * x + b;
2061 
2062  int n = x.size();
2063  for(int i=0; i < n; ++i){
2064  os << "(" << x(i) << ", " << z(i) << ")";
2065 
2066  if(x(i) < 0.0 || z(i) < 0.0 || x(i) * z(i) != 0.0){
2067  os << " - X";
2068  }
2069  os << "\n";
2070 
2072  os << "-------------------------------\n";
2074  os << "-------------------------------\n";
2075  }
2076  }
2077 
2078  os << "-------------------------------\n";
2079 
2080 
2081  os << std::endl;
2082 }
2083 
2084 
2086 {
2087  os << "check MCP result\n";
2088  os << "-------------------------------\n";
2089 
2090  dvector z = M * x + b;
2091 
2092  for(int i=0; i < globalNumConstraintVectors; ++i){
2093  os << "(" << x(i) << ", " << z(i) << ")";
2094 
2095  if(x(i) < 0.0 || z(i) < -1.0e-6){
2096  os << " - X";
2097  } else if(x(i) > 0.0 && fabs(z(i)) > 1.0e-6){
2098  os << " - X";
2099  } else if(z(i) > 1.0e-6 && fabs(x(i)) > 1.0e-6){
2100  os << " - X";
2101  }
2102 
2103 
2104  os << "\n";
2105  }
2106 
2107  os << "-------------------------------\n";
2108 
2109  int j = 0;
2110  for(int i=globalNumConstraintVectors; i < globalNumConstraintVectors + globalNumFrictionVectors; ++i, ++j){
2111  os << "(" << x(i) << ", " << z(i) << ")";
2112 
2113  int contactIndex = frictionIndexToContactIndex[j];
2114  double hi = contactIndexToMu[contactIndex] * x(contactIndex);
2115 
2116  os << " hi = " << hi;
2117 
2118  if(x(i) < 0.0 || x(i) > hi){
2119  os << " - X";
2120  } else if(x(i) == hi && z(i) > -1.0e-6){
2121  os << " - X";
2122  } else if(x(i) < hi && x(i) > 0.0 && fabs(z(i)) > 1.0e-6){
2123  os << " - X";
2124  }
2125  os << "\n";
2126  }
2127 
2128  os << "-------------------------------\n";
2129 
2130  os << std::endl;
2131 }
2132 
2133 
2134 #ifdef USE_PIVOTING_LCP
2135 bool CFSImpl::callPathLCPSolver(rmdmatrix& Mlcp, dvector& b, dvector& solution)
2136 {
2137  int size = solution.size();
2138  int square = size * size;
2139  std::vector<double> lb(size + 1, 0.0);
2140  std::vector<double> ub(size + 1, 1.0e20);
2141 
2142  int m_nnz = 0;
2143  std::vector<int> m_i(square + 1);
2144  std::vector<int> m_j(square + 1);
2145  std::vector<double> m_ij(square + 1);
2146 
2147  for(int i=0; i < size; ++i){
2148  solution(i) = 0.0;
2149  }
2150 
2151  for(int j=0; j < size; ++j){
2152  for(int i=0; i < size; ++i){
2153  double v = Mlcp(i, j);
2154  if(v != 0.0){
2155  m_i[m_nnz] = i+1;
2156  m_j[m_nnz] = j+1;
2157  m_ij[m_nnz] = v;
2158  ++m_nnz;
2159  }
2160  }
2161  }
2162 
2163  MCP_Termination status;
2164 
2165  SimpleLCP(size, m_nnz, &m_i[0], &m_j[0], &m_ij[0], &b(0), &lb[0], &ub[0], &status, &solution(0));
2166 
2167  return (status == MCP_Solved);
2168 }
2169 #endif
2170 
2171 
2173 {
2174  impl = new CFSImpl(world);
2175 }
2176 
2177 
2179 {
2180  delete impl;
2181 }
2182 
2183 
2185 (int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, double muStatic, double muDynamic, double culling_thresh, double restitution, double epsilon)
2186 {
2187  return impl->addCollisionCheckLinkPair(bodyIndex1, link1, bodyIndex2, link2, muStatic, muDynamic, culling_thresh, restitution, epsilon);
2188 }
2189 
2190 
2192 {
2193  impl->world.clearCollisionPairs();
2194  impl->collisionCheckLinkPairs.clear();
2195 }
2196 
2197 bool ConstraintForceSolver::addExtraJoint(int bodyIndex1, Link* link1, int bodyIndex2, Link* link2, const double* link1LocalPos, const double* link2LocalPos, const short jointType, const double* jointAxis )
2198 {
2199  return impl->addExtraJoint(bodyIndex1, link1, bodyIndex2, link2, link1LocalPos, link2LocalPos, jointType, jointAxis );
2200 }
2201 
2202 
2203 void ConstraintForceSolver::setGaussSeidelParameters(int maxNumIteration, int numInitialIteration, double maxRelError)
2204 {
2205  impl->maxNumGaussSeidelIteration = maxNumIteration;
2206  impl->numGaussSeidelInitialIteration = numInitialIteration;
2207  impl->gaussSeidelMaxRelError = maxRelError;
2208 }
2209 
2210 
2212 {
2213  impl->isConstraintForceOutputMode = on;
2214 }
2215 
2216 
2218 {
2219  impl->useBuiltinCollisionDetector = on;
2220 }
2221 
2223 {
2224  impl->negativeVelocityRatioForPenetration = ratio;
2225 }
2226 
2227 
2228 
2230 {
2231  impl->initialize();
2232 }
2233 
2234 
2235 void ConstraintForceSolver::solve(CollisionSequence& corbaCollisionSequence)
2236 {
2237  impl->solve(corbaCollisionSequence);
2238 }
2239 
2240 
2242 {
2243  impl->clearExternalForces();
2244 }
2245 
2247 {
2248  impl->allowedPenetrationDepth = dVal;
2249 }
2250 
2252 {
2253  return impl->allowedPenetrationDepth;
2254 }
void solveMCPByProjectedGaussSeidel(const rmdmatrix &M, const dvector &b, dvector &x)
static const double ALLOWED_PENETRATION_DEPTH
void checkMCPResult(rmdmatrix &M, dvector &b, dvector &x)
The header file of the LinkTraverse class.
static const bool CFS_MCP_DEBUG
static const double PI_2
boost::shared_ptr< ForwardDynamicsMM > ForwardDynamicsMMPtr
bool addExtraJoint(int bodyIndex1, Link *link1, int bodyIndex2, Link *link2, const double *link1LocalPos, const double *link2LocalPos, const short jointType, const double *jointAxis)
static const bool ENABLE_TRUE_FRICTION_CONE
void initBody(BodyPtr body, BodyData &bodyData)
Modifications controlling boost library behavior.
Definition: ColladaUtil.h:306
* x
Definition: IceUtils.h:98
void extractRelAccelsFromLinkPairCase2(Eigen::Block< rmdmatrix > &Kxn, Eigen::Block< rmdmatrix > &Kxt, LinkPair &linkPair, int iTestForce, int iDefault, int testForceIndex, int constraintIndex)
ExtraJointType type
Definition: Body.h:250
void calcAccelsABM(BodyData &bodyData, int constraintIndex)
void checkLCPResult(rmdmatrix &M, dvector &b, dvector &x)
void setNegativeVelocityRatioForPenetration(double ratio)
static const bool ENABLE_RANDOM_STATIC_FRICTION_BASE
bool addCollisionCheckLinkPair(int bodyIndex1, Link *link1, int bodyIndex2, Link *link2, double muStatic, double muDynamic, double culling_thresh, double restitution, double epsilon)
png_uint_32 size
Definition: png.h:1521
static const bool ENABLE_STATIC_FRICTION
Link * link(int index) const
Definition: LinkTraverse.h:56
std::vector< int > frictionIndexToContactIndex
void initABMForceElementsWithNoExtForce(BodyData &bodyData)
png_infop png_charpp name
Definition: png.h:2382
static const bool ONLY_STATIC_FRICTION_FORMULATION
static const bool CFS_MCP_DEBUG_SHOW_ITERATION_STOP
void solve(OpenHRP::CollisionSequence &corbaCollisionSequence)
void error(char *msg) const
Definition: minigzip.c:87
ConstraintPointArray constraintPoints
png_uint_32 i
Definition: png.h:2735
static const bool PROPORTIONAL_DYNAMIC_FRICTION
void setConstraintPoints(CollisionSequence &collisions)
Eigen::VectorXd dvector
Definition: EigenTypes.h:14
Vector3 point[2]
Definition: Body.h:253
static const int DEFAULT_MAX_NUM_GAUSS_SEIDEL_ITERATION
static void putVector(TVector &M, char *name)
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
static const bool SKIP_REDUNDANT_ACCEL_CALC
void solve(CollisionSequence &corbaCollisionSequence)
intrusive_ptr< LinkPair > LinkPairPtr
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
static const double PI
void debugPutVector(const TVector &M, const char *name)
ForwardDynamicsPtr forwardDynamics(int index)
get forward dynamics computation method for body
BodyPtr body(int index)
get body by index
static const bool IGNORE_CURRENT_VELOCITY_IN_STATIC_FRICTION
void putVector(const TVector &M, const char *name)
static const bool ASSUME_SYMMETRIC_MATRIX
void extractRelAccelsFromLinkPairCase3(Eigen::Block< rmdmatrix > &Kxn, Eigen::Block< rmdmatrix > &Kxt, LinkPair &linkPair, int testForceIndex, int constraintIndex)
static const double THRESH_TO_SWITCH_REL_ERROR
void setExtraJointConstraintPoints(ExtraJointLinkPairPtr &linkPair)
void setFrictionVectors(ConstraintPoint &constraintPoint)
double currentTime(void) const
get current time
static const bool USE_PREVIOUS_LCP_SOLUTION
list index
static const bool CFS_DEBUG
vector< ExtraJointLinkPairPtr > extraJointLinkPairs
string a
static const double DEFAULT_NEGATIVE_VELOCITY_RATIO_FOR_PENETRATION
static const bool STATIC_FRICTION_BY_TWO_CONSTRAINTS
bool addExtraJoint(int bodyIndex1, Link *link1, int bodyIndex2, Link *link2, const double *link1LocalPos, const double *link2LocalPos, const short jointType, const double *jointAxis)
#define PI
PI.
Definition: IceTypes.h:32
Link * link[2]
Definition: Body.h:252
bool addCollisionCheckLinkPair(int bodyIndex1, Link *link1, int bodyIndex2, Link *link2, double muStatic, double muDynamic, double culling_thresh, double restitution, double epsilon)
LinkPairArray collisionCheckLinkPairs
void setAllowedPenetrationDepth(double dVal)
static const bool CFS_DEBUG_VERBOSE
void extractRelAccelsOfConstraintPoints(Eigen::Block< rmdmatrix > &Kxn, Eigen::Block< rmdmatrix > &Kxt, int testForceIndex, int constraintIndex)
static const bool CFS_DEBUG_VERBOSE_3
void setContactConstraintPoints(LinkPair &linkPair, CollisionPointSequence &collisionPoints)
void debugPutMatrix(const TMatrix &M, const char *name)
void clearSingularPointConstraintsOfClosedLoopConnections()
static void putMatrix(TMatrix &M, char *name)
intrusive_ptr< ExtraJointLinkPair > ExtraJointLinkPairPtr
double negativeVelocityRatioForPenetration
void calcABMForceElementsWithTestForce(BodyData &bodyData, Link *linkToApplyForce, const Vector3 &f, const Vector3 &tau)
static const bool usePivotingLCP
std::vector< LinkPair * > constrainedLinkPairs
void addConstraintForceToLink(LinkPair *linkPair, int ipair)
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > rmdmatrix
static const int DEFAULT_NUM_GAUSS_SEIDEL_INITIAL_ITERATION
std::vector< BodyData > bodiesData
variate_generator< boost::mt19937, uniform_real<> > randomAngle
void setGaussSeidelParameters(int maxNumIteration, int numInitialIteration, double maxRelError)
void getVector3(Vector3 &v3, const V &v, size_t top=0)
Definition: Eigen3d.h:138
static const bool ALLOW_SUBTLE_PENETRATION_FOR_STABILITY
JSAMPIMAGE data
Definition: jpeglib.h:945
static const int DEFAULT_NUM_GAUSS_SEIDEL_ITERATION_BLOCK
void solveMCPByProjectedGaussSeidelMain(const rmdmatrix &M, const dvector &b, dvector &x, const int numIteration)
void initExtraJoints(int bodyIndex)
void putMatrix(TMatrix &M, const char *name)
void solveMCPByProjectedGaussSeidelInitial(const rmdmatrix &M, const dvector &b, dvector &x, const int numIteration)
void extractRelAccelsFromLinkPairCase1(Eigen::Block< rmdmatrix > &Kxn, Eigen::Block< rmdmatrix > &Kxt, LinkPair &linkPair, int testForceIndex, int constraintIndex)
static const double VEL_THRESH_OF_DYNAMIC_FRICTION
ForwardDynamicsMMPtr forwardDynamicsMM
CFSImpl(WorldBase &world)
void calcAccelsMM(BodyData &bodyData, int constraintIndex)
unsigned int numBodies()
get the number of bodies in this world
std::vector< LinkData > LinkDataArray
std::vector< LinkPairPtr > LinkPairArray
static const double DEFAULT_GAUSS_SEIDEL_MAX_REL_ERROR
void copySymmetricElementsOfAccelerationMatrix(Eigen::Block< rmdmatrix > &Knn, Eigen::Block< rmdmatrix > &Ktn, Eigen::Block< rmdmatrix > &Knt, Eigen::Block< rmdmatrix > &Ktt)
double timeStep(void) const
get time step
static const bool CFS_PUT_NUM_CONTACT_POINTS
static const bool CFS_DEBUG_VERBOSE_2
static int max(int a, int b)
std::pair< int, bool > getIndexOfLinkPairs(Link *link1, Link *link2)
get index of link pairs
Vector3 * rootLinkPosRef
only used by the ForwardDynamicsMM mode
std::vector< collision_data > & detectCollisions()
std::vector< ConstraintPoint > ConstraintPointArray
static const bool CFS_DEBUG_LCPCHECK
unsigned int numLinks() const
Definition: LinkTraverse.h:40


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:02