18 #define _USE_MATH_DEFINES // for M_PI
29 #include <hrpCorba/OpenHRPCommon.hh>
33 #include <boost/format.hpp>
34 #include <boost/tuple/tuple.hpp>
35 #include <boost/random.hpp>
39 #include <boost/lexical_cast.hpp>
42 using namespace boost;
50 #ifdef USE_PIVOTING_LCP
51 #include "SimpleLCP_Path.h"
122 bool addCollisionCheckLinkPair
123 (
int bodyIndex1,
Link* link1,
int bodyIndex2,
Link* link2,
double muStatic,
double muDynamic,
double culling_thresh,
double restitution,
double epsilon);
125 (
int bodyIndex1,
Link* link1,
int bodyIndex2,
Link* link2,
const double* link1LocalPos,
const double* link2LocalPos,
const short jointType,
const double* jointAxis );
127 void initialize(
void);
128 void solve(CollisionSequence& corbaCollisionSequence);
129 inline void clearExternalForces();
132 #if defined(M_PI) && defined (M_PI_2)
136 static const double PI;
232 std::vector<LinkPair*> constrainedLinkPairs;
245 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
rmdmatrix;
279 void initExtraJoints(
int bodyIndex);
280 void setConstraintPoints(CollisionSequence& collisions);
281 void setContactConstraintPoints(
LinkPair& linkPair, CollisionPointSequence& collisionPoints);
284 void putContactPoints();
285 void solveImpactConstraints();
287 void setAccelCalcSkipInformation();
288 void setDefaultAccelerationVector();
289 void setAccelerationMatrix();
290 void initABMForceElementsWithNoExtForce(
BodyData& bodyData);
292 void calcAccelsABM(
BodyData& bodyData,
int constraintIndex);
293 void calcAccelsMM(
BodyData& bodyData,
int constraintIndex);
295 void extractRelAccelsOfConstraintPoints
296 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt,
int testForceIndex,
int constraintIndex);
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);
305 void copySymmetricElementsOfAccelerationMatrix
306 (Eigen::Block<rmdmatrix>& Knn, Eigen::Block<rmdmatrix>& Ktn, Eigen::Block<rmdmatrix>& Knt, Eigen::Block<rmdmatrix>& Ktt);
308 void clearSingularPointConstraintsOfClosedLoopConnections();
310 void setConstantVectorAndMuBlock();
311 void addConstraintForceToLinks();
312 void addConstraintForceToLink(
LinkPair* linkPair,
int ipair);
314 void solveMCPByProjectedGaussSeidel
316 void solveMCPByProjectedGaussSeidelInitial
318 void solveMCPByProjectedGaussSeidelMain
324 #ifdef USE_PIVOTING_LCP
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;
337 template<
class TMatrix>
340 os <<
"Vector " <<
name << M << std::endl;
342 os <<
"Matrix " <<
name <<
": \n";
343 for(
int i=0;
i < M.rows();
i++){
344 for(
int j=0; j < M.cols(); j++){
346 os << boost::format(
" %.50g ") % M(
i, j);
353 template<
class TVector>
355 os <<
"Vector " <<
name << M << std::endl;
358 template<
class TMatrix>
363 template<
class TVector>
368 #if !defined(M_PI) || !defined(M_PI_2)
371 const double CFSImpl::PI_2 = 1.57079632679489661923;
379 randomAngle(
boost::mt19937(), uniform_real<>(0.0, 2.0 *
PI))
401 (
int bodyIndex1,
Link* link1,
int bodyIndex2,
Link* link2,
double muStatic,
double muDynamic,
double culling_thresh,
double restitution,
double epsilon)
406 tie(index, isRegistered) = world.getIndexOfLinkPairs(link1, link2);
410 int n = collisionCheckLinkPairs.size();
412 collisionCheckLinkPairs.resize(index+1);
415 LinkPairPtr& linkPair = collisionCheckLinkPairs[index];
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;
435 return (index >= 0 && !isRegistered);
438 bool CFSImpl::addExtraJoint(
int bodyIndex1,
Link* link1,
int bodyIndex2,
Link* link2,
const double* link1LocalPos,
const double* link2LocalPos,
const short jointType,
const double* jointAxis )
442 linkPair->isSameBodyPair =
false;
443 linkPair->isNonContactConstraint =
true;
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))){
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();
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);
466 linkPair->constraintPoints.resize(1);
467 linkPair->jointConstraintAxes[0] =
Vector3(jointAxis[0], jointAxis[1], jointAxis[2]);
471 int numConstraints = linkPair->constraintPoints.size();
472 for(
int k=0; k < numConstraints; ++k){
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);
492 body->clearExternalForces();
493 bodyData.
body = body;
494 bodyData.
linksData.resize(body->numLinks());
497 bodyData.
isStatic = body->isStaticModel();
501 for(
unsigned int j=0; j < traverse.
numLinks(); ++j){
502 Link* link = traverse[j];
513 int numExtraJoints = body->extraJoints.size();
514 for(
int j=0; j < numExtraJoints; ++j){
520 linkPair->isSameBodyPair =
true;
521 linkPair->isNonContactConstraint =
true;
527 for(
int k=1; k < 3; k++){
528 if(fabs(axis(k)) < fabs(axis(minElem))){
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();
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);
544 linkPair->constraintPoints.resize(1);
545 linkPair->jointConstraintAxes[0] = bodyExtraJoint.
axis;
549 int numConstraints = linkPair->constraintPoints.size();
550 for(
int k=0; k < numConstraints; ++k){
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];
569 static int ntest = 0;
571 os.open((
string(
"cfs-log-") + lexical_cast<string>(ntest++) +
".log").c_str());
585 for(
int bodyIndex=0; bodyIndex < numBodies; ++bodyIndex){
597 for(
size_t j=0; j < linksData.size(); ++j){
599 linkData.
dw.setZero();
600 linkData.
dvo.setZero();
608 for(
int i=0;
i < numLinkPairs; ++
i){
610 for(
int j=0; j < 2; ++j){
618 for(
int i=0;
i < numLinkPairs; ++
i){
620 for(
int j=0; j < 2; ++j){
640 bodyData.
body->clearExternalForces();
654 bodiesData[
i].body->updateLinkColdetModelPositions();
658 const int n = body->numLinks();
659 for(
int j=0; j <
n; ++j){
660 body->link(j)->constraintForces.clear();
686 if(constraintsSizeChanged){
714 #ifdef USE_PIVOTING_LCP
730 os <<
"LCP converged" << std::endl;
747 static const bool enableNormalVisualization =
true;
749 CollisionPointSequence collisionPoints;
750 CollisionPointSequence* pCollisionPoints = 0;
757 pCollisionPoints = 0;
761 CollisionPointSequence& points = collisions[colIndex].points;
762 if(points.length() > 0){
763 pCollisionPoints = &points;
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;
774 pCollisionPoints = &collisionPoints;
780 pCollisionPoints->length(0);
781 pCollisionPoints = 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++;
790 pCollisionPoints->length(npoints);
791 pCollisionPoints = 0;
793 pCollisionPoints->length(npoints);
795 for (
unsigned int i = 0;
i < cdata.size();
i++) {
799 CollisionPoint& point = (*pCollisionPoints)[idx];
800 for(
int k=0; k < 3; k++){
801 point.position[k] = cd.
i_points[j][k];
803 for(
int k=0; k < 3; k++){
806 point.idepth = cd.
depth;
815 if(pCollisionPoints){
835 constraintPoints.clear();
836 int numExtractedPoints = 0;
837 int numContactsInPair = collisionPoints.length();
839 for(
int j=0; j < numContactsInPair; ++j){
841 CollisionPoint& collision = collisionPoints[j];
848 contact.
depth = collision.idepth;
850 bool isNeighborhood =
false;
855 for(
int k=0; k < numExtractedPoints; ++k){
857 isNeighborhood =
true;
865 constraintPoints.pop_back();
867 numExtractedPoints++;
872 for(
int k=0; k < 2; ++k){
877 v[k] = link->
vo + link->
w.cross(contact.
point);
908 double vt_square = v_tangent.dot(v_tangent);
910 bool isSlipping = (vt_square > vsqrthresh);
915 double vt_mag = sqrt(vt_square);
916 Vector3 t1(v_tangent / vt_mag);
925 if(vt_mag < contact.
mu){
950 for(
int i=1;
i < 3;
i++){
951 if(fabs(normal(
i)) < fabs(normal(minAxis))){
990 Link* link0 = linkPair->link[0];
991 Link* link1 = linkPair->link[1];
994 point[0].noalias() = link0->
p + link0->
attitude() * linkPair->jointPoint[0];
995 point[1].noalias() = link1->
p + link1->
attitude() * linkPair->jointPoint[1];
1005 for(
int k=0; k < 2; ++k){
1006 Link* link = linkPair->link[k];
1010 v[k] = link->
vo + link->
w.cross(point[k]);
1013 Vector3 relVelocityOn0 = v[1] - v[0];
1015 int n = linkPair->constraintPoints.size();
1016 for(
int i=0;
i <
n; ++
i){
1019 constraint.
point = point[1];
1022 constraint.
depth = axis.dot(point[1] - point[0]);
1026 linkPair->bodyData[0]->hasConstrainedLinks =
true;
1027 linkPair->bodyData[1]->hasConstrainedLinks =
true;
1034 os <<
"Contact Points\n";
1048 for(
size_t j=0; j < constraintPoints.size(); ++j){
1051 os <<
" point: " << contact.
point;
1056 os <<
" depth" << contact.
depth;
1057 os <<
" mu" << contact.
mu;
1074 os <<
"Impacts !" << std::endl;
1085 Mlcp.resize(dimLCP, dimLCP);
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();
1117 for(
size_t j=0; j < linksData.size(); ++j){
1125 for(
int i=0;
i < numLinkPairs; ++
i){
1128 for(
int j=0; j < 2; ++j){
1130 int linkIndex = linkPair->
link[j]->
index;
1131 while(linkIndex >= 0){
1132 LinkData& linkData = linksData[linkIndex];
1171 for(
size_t j=0; j < constraintPoints.size(); ++j){
1174 for(
int k=0; k < 2; ++k){
1181 linkData->
dvo - constraint.
point.cross(linkData->
dw) +
1182 link->
w.cross(link->
vo + link->
w.cross(constraint.
point));
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);
1212 for(
int j=0; j < numConstraintsInPair; ++j){
1218 for(
int k=0; k < 2; ++k){
1245 for(
int k=0; k < 2; ++k){
1282 bodyData.
dpf.setZero();
1283 bodyData.
dptau.setZero();
1285 std::vector<LinkData>& linksData = bodyData.
linksData;
1289 for(
int i =
n-1;
i >= 0; --
i){
1290 Link* link = traverse[
i];
1298 LinkData& childData = linksData[child->index];
1304 double uu_dd = childData.
uu0 / child->dd;
1305 data.pf0 += uu_dd * child->hhv;
1306 data.ptau0 += uu_dd * child->hhw;
1323 std::vector<LinkData>& linksData = bodyData.
linksData;
1328 Link* link = linkToApplyForce;
1332 double duu = -(link->
sv.dot(dpf) + link->
sw.dot(dptau));
1334 double duudd = duu / link->
dd;
1335 dpf += duudd * link->
hhv;
1336 dptau += duudd * link->
hhw;
1341 bodyData.
dpf += dpf;
1342 bodyData.
dptau += dptau;
1348 std::vector<LinkData>& linksData = bodyData.
linksData;
1354 Eigen::Matrix<double, 6, 6> M;
1355 M << rootLink->
Ivv, rootLink->
Iwv.transpose(),
1356 rootLink->
Iwv, rootLink->
Iww;
1358 Eigen::Matrix<double, 6, 1>
f;
1359 f << (rootData.
pf0 + bodyData.
dpf),
1363 Eigen::Matrix<double, 6, 1>
a(M.colPivHouseholderQr().solve(
f));
1365 rootData.
dvo =
a.head<3>();
1366 rootData.
dw =
a.tail<3>();
1369 rootData.
dw.setZero();
1370 rootData.
dvo.setZero();
1374 bodyData.
dpf.setZero();
1375 bodyData.
dptau.setZero();
1378 int n = linksData.size();
1379 for(
int linkIndex = 1; linkIndex <
n; ++linkIndex){
1381 LinkData& linkData = linksData[linkIndex];
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;
1394 linkData.
dvo = parentData.
dvo;
1395 linkData.
dw = parentData.
dw;
1399 linkData.
uu = linkData.
uu0;
1407 std::vector<LinkData>& linksData = bodyData.
linksData;
1411 rootData.
dvo = rootLink->
dvo;
1412 rootData.
dw = rootLink->
dw;
1415 int n = linksData.size();
1417 for(
int linkIndex = 1; linkIndex <
n; ++linkIndex){
1419 LinkData& linkData = linksData[linkIndex];
1426 linkData.
dvo = parentData.
dvo + link->
cv + link->
ddq * link->
sv;
1427 linkData.
dw = parentData.
dw + link->
cw + link->
ddq * link->
sw;
1429 linkData.
dvo = parentData.
dvo;
1430 linkData.
dw = parentData.
dw;
1438 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt,
int testForceIndex,
int constraintIndex)
1443 for(
size_t i=0;
i < constrainedLinkPairs.size(); ++
i){
1445 LinkPair& linkPair = *constrainedLinkPairs[
i];
1452 extractRelAccelsFromLinkPairCase1(Kxn, Kxt, linkPair, testForceIndex, maxConstraintIndexToExtract);
1454 extractRelAccelsFromLinkPairCase2(Kxn, Kxt, linkPair, 0, 1, testForceIndex, maxConstraintIndexToExtract);
1458 extractRelAccelsFromLinkPairCase2(Kxn, Kxt, linkPair, 1, 0, testForceIndex, maxConstraintIndexToExtract);
1460 extractRelAccelsFromLinkPairCase3(Kxn, Kxt, linkPair, testForceIndex, maxConstraintIndexToExtract);
1468 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt,
LinkPair& linkPair,
int testForceIndex,
int maxConstraintIndexToExtract)
1472 for(
size_t i=0;
i < constraintPoints.size(); ++
i){
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)));
1492 Kxn(constraintIndex, testForceIndex) =
1497 Kxt(index, testForceIndex) = constraint.
frictionVector[j][1].dot(relAccel) - at0(index);
1504 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt,
LinkPair& linkPair,
int iTestForce,
int iDefault,
int testForceIndex,
int maxConstraintIndexToExtract)
1508 for(
size_t i=0;
i < constraintPoints.size(); ++
i){
1517 Link* link = linkPair.
link[iTestForce];
1520 Vector3 dv(linkData->
dvo - constraint.
point.cross(linkData->
dw) + link->
w.cross(link->
vo + link->
w.cross(constraint.
point)));
1523 os <<
"dv " << constraintIndex <<
" = " << dv <<
"\n";
1527 Kxn(constraintIndex, testForceIndex) =
1532 Kxt(index, testForceIndex) =
1533 constraint.
frictionVector[j][iDefault].dot(relAccel) - at0(index);
1541 (Eigen::Block<rmdmatrix>& Kxn, Eigen::Block<rmdmatrix>& Kxt,
LinkPair& linkPair,
int testForceIndex,
int maxConstraintIndexToExtract)
1545 for(
size_t i=0;
i < constraintPoints.size(); ++
i){
1554 Kxn(constraintIndex, testForceIndex) = 0.0;
1564 (Eigen::Block<rmdmatrix>& Knn, Eigen::Block<rmdmatrix>& Ktn, Eigen::Block<rmdmatrix>& Knt, Eigen::Block<rmdmatrix>& Ktt)
1566 for(
size_t linkPairIndex=0; linkPairIndex < constrainedLinkPairs.size(); ++linkPairIndex){
1568 ConstraintPointArray& constraintPoints = constrainedLinkPairs[linkPairIndex]->constraintPoints;
1570 for(
size_t localConstraintIndex = 0; localConstraintIndex < constraintPoints.size(); ++localConstraintIndex){
1575 int nextConstraintIndex = constraintIndex + 1;
1576 for(
int i = nextConstraintIndex;
i < globalNumConstraintVectors; ++
i){
1577 Knn(
i, constraintIndex) = Knn(constraintIndex,
i);
1580 for(
int i = frictionTopOfNextConstraint;
i < globalNumFrictionVectors; ++
i){
1581 Knt(
i, constraintIndex) = Ktn(constraintIndex,
i);
1584 for(
int localFrictionIndex=0; localFrictionIndex < constraint.
numFrictionVectors; ++localFrictionIndex){
1588 for(
int i = nextConstraintIndex;
i < globalNumConstraintVectors; ++
i){
1589 Ktn(
i, frictionIndex) = Knt(frictionIndex,
i);
1591 for(
int i = frictionTopOfNextConstraint;
i < globalNumFrictionVectors; ++
i){
1592 Ktt(
i, frictionIndex) = Ktt(frictionIndex,
i);
1602 for(
int i = 0;
i <
Mlcp.rows(); ++
i){
1604 for(
int j=0; j <
Mlcp.rows(); ++j){
1624 for(
int j=0; j < numConstraintsInPair; ++j){
1638 v = 0.1 * (-1.0 + exp(-
error * 20.0));
1640 v = 0.1 * ( 1.0 - exp(
error * 20.0));
1648 double extraNegativeVel;
1664 b(block2 + globalFrictionIndex) =
at0(globalFrictionIndex);
1666 b(block2 + globalFrictionIndex) += tangentProjectionOfRelVelocity * dtinv;
1671 Mlcp(block3 + globalFrictionIndex, globalIndex) = constraint.
mu;
1677 ++globalFrictionIndex;
1688 for(
int i=0;
i <
n; ++
i){
1690 for(
int j=0; j < 2; ++j){
1701 Vector3 f_total(Vector3::Zero());
1702 Vector3 tau_total(Vector3::Zero());
1705 int numConstraintPoints = constraintPoints.size();
1706 Link* link = linkPair->
link[ipair];
1708 for(
int i=0;
i < numConstraintPoints; ++
i){
1720 tau_total += constraint.
point.cross(
f);
1724 constraintForces.resize(constraintForces.size() + 1);
1731 link->
fext += f_total;
1732 link->
tauext += tau_total;
1736 os <<
"Constraint force to " << link->
name <<
": f = " << f_total <<
", tau = " << tau_total << std::endl;
1751 if(numBlockLoops==0) numBlockLoops = 1;
1758 while(
i < numBlockLoops){
1766 double n = x.norm();
1774 for(
int j=0; j < x.size(); ++j){
1775 double d = fabs(x(j) - x0(j));
1787 os <<
"stopped at " << (
i * loopBlockSize) <<
", error = " <<
error << endl;
1795 if(
i == numBlockLoops){
1796 os <<
"not stopped" <<
", error = " <<
error << endl;
1799 int n = loopBlockSize *
i;
1813 const int size = globalNumConstraintVectors + globalNumFrictionVectors;
1815 const double rstep = 1.0 / (numIteration *
size);
1818 for(
int i=0;
i < numIteration; ++
i){
1820 for(
int j=0; j < globalNumContactNormalVectors; ++j){
1826 double sum = -M(j, j) * x(j);
1827 for(
int k=0; k <
size; ++k){
1828 sum += M(j, k) * x(k);
1830 xx = (-
b(j) - sum) / M(j, j);
1838 mcpHi[j] = contactIndexToMu[j] * x(j);
1841 for(
int j=globalNumContactNormalVectors; j < globalNumConstraintVectors; ++j){
1846 double sum = -M(j, j) * x(j);
1847 for(
int k=0; k <
size; ++k){
1848 sum += M(j, k) * x(k);
1850 x(j) = r * (-
b(j) - sum) / M(j, j);
1857 int contactIndex = 0;
1858 for(
int j=globalNumConstraintVectors; j <
size; ++j, ++contactIndex){
1864 double sum = -M(j, j) * x(j);
1865 for(
int k=0; k <
size; ++k){
1866 sum += M(j, k) * x(k);
1868 fx0 = (-
b(j) - sum) / M(j, j);
1878 double sum = -M(j, j) * x(j);
1879 for(
int k=0; k <
size; ++k){
1880 sum += M(j, k) * x(k);
1882 fy0 = (-
b(j) - sum) / M(j, j);
1886 const double fmax = mcpHi[contactIndex];
1887 const double fmax2 = fmax * fmax;
1888 const double fmag2 = fx0 * fx0 + fy0 * fy0;
1891 const double s = r * fmax / sqrt(fmag2);
1898 r += (rstep + rstep);
1903 int frictionIndex = 0;
1904 for(
int j=globalNumConstraintVectors; j <
size; ++j, ++frictionIndex){
1910 double sum = -M(j, j) * x(j);
1911 for(
int k=0; k <
size; ++k){
1912 sum += M(j, k) * x(k);
1914 xx = (-
b(j) - sum) / M(j, j);
1917 const int contactIndex = frictionIndexToContactIndex[frictionIndex];
1918 const double fmax = mcpHi[contactIndex];
1923 }
else if(xx > fmax){
1939 const int size = globalNumConstraintVectors + globalNumFrictionVectors;
1941 for(
int i=0;
i < numIteration; ++
i){
1943 for(
int j=0; j < globalNumContactNormalVectors; ++j){
1949 double sum = -M(j, j) * x(j);
1950 for(
int k=0; k <
size; ++k){
1951 sum += M(j, k) * x(k);
1953 xx = (-
b(j) - sum) / M(j, j);
1960 mcpHi[j] = contactIndexToMu[j] * x(j);
1963 for(
int j=globalNumContactNormalVectors; j < globalNumConstraintVectors; ++j){
1968 double sum = -M(j, j) * x(j);
1969 for(
int k=0; k <
size; ++k){
1970 sum += M(j, k) * x(k);
1972 x(j) = (-
b(j) - sum) / M(j, j);
1979 int contactIndex = 0;
1980 for(
int j=globalNumConstraintVectors; j <
size; ++j, ++contactIndex){
1986 double sum = -M(j, j) * x(j);
1987 for(
int k=0; k <
size; ++k){
1988 sum += M(j, k) * x(k);
1990 fx0 = (-
b(j) - sum) / M(j, j);
2000 double sum = -M(j, j) * x(j);
2001 for(
int k=0; k <
size; ++k){
2002 sum += M(j, k) * x(k);
2004 fy0 = (-
b(j) - sum) / M(j, j);
2008 const double fmax = mcpHi[contactIndex];
2009 const double fmax2 = fmax * fmax;
2010 const double fmag2 = fx0 * fx0 + fy0 * fy0;
2013 const double s = fmax / sqrt(fmag2);
2024 int frictionIndex = 0;
2025 for(
int j=globalNumConstraintVectors; j <
size; ++j, ++frictionIndex){
2031 double sum = -M(j, j) * x(j);
2032 for(
int k=0; k <
size; ++k){
2033 sum += M(j, k) * x(k);
2035 xx = (-
b(j) - sum) / M(j, j);
2038 const int contactIndex = frictionIndexToContactIndex[frictionIndex];
2039 const double fmax = mcpHi[contactIndex];
2044 }
else if(xx > fmax){
2057 os <<
"check LCP result\n";
2058 os <<
"-------------------------------\n";
2063 for(
int i=0;
i <
n; ++
i){
2064 os <<
"(" << x(
i) <<
", " << z(
i) <<
")";
2066 if(x(
i) < 0.0 || z(
i) < 0.0 || x(
i) * z(
i) != 0.0){
2072 os <<
"-------------------------------\n";
2074 os <<
"-------------------------------\n";
2078 os <<
"-------------------------------\n";
2087 os <<
"check MCP result\n";
2088 os <<
"-------------------------------\n";
2093 os <<
"(" << x(
i) <<
", " << z(
i) <<
")";
2095 if(x(
i) < 0.0 || z(
i) < -1.0e-6){
2097 }
else if(x(
i) > 0.0 && fabs(z(
i)) > 1.0e-6){
2099 }
else if(z(
i) > 1.0e-6 && fabs(x(
i)) > 1.0e-6){
2107 os <<
"-------------------------------\n";
2111 os <<
"(" << x(
i) <<
", " << z(
i) <<
")";
2116 os <<
" hi = " << hi;
2118 if(x(
i) < 0.0 || x(
i) > hi){
2120 }
else if(x(
i) == hi && z(
i) > -1.0e-6){
2122 }
else if(x(
i) < hi && x(
i) > 0.0 && fabs(z(
i)) > 1.0e-6){
2128 os <<
"-------------------------------\n";
2134 #ifdef USE_PIVOTING_LCP
2135 bool CFSImpl::callPathLCPSolver(rmdmatrix& Mlcp,
dvector&
b,
dvector& solution)
2139 std::vector<double> lb(
size + 1, 0.0);
2140 std::vector<double> ub(
size + 1, 1.0e20);
2143 std::vector<int> m_i(square + 1);
2144 std::vector<int> m_j(square + 1);
2145 std::vector<double> m_ij(square + 1);
2151 for(
int j=0; j <
size; ++j){
2153 double v =
Mlcp(
i, j);
2163 MCP_Termination status;
2165 SimpleLCP(
size, m_nnz, &m_i[0], &m_j[0], &m_ij[0], &
b(0), &lb[0], &ub[0], &status, &
solution(0));
2167 return (status == MCP_Solved);
2185 (
int bodyIndex1,
Link* link1,
int bodyIndex2,
Link* link2,
double muStatic,
double muDynamic,
double culling_thresh,
double restitution,
double epsilon)
2187 return impl->addCollisionCheckLinkPair(bodyIndex1, link1, bodyIndex2, link2, muStatic, muDynamic, culling_thresh, restitution, epsilon);
2199 return impl->
addExtraJoint(bodyIndex1, link1, bodyIndex2, link2, link1LocalPos, link2LocalPos, jointType, jointAxis );