//==============================================================================
void DantzigBoxedLcpSolver::solve(
int n,
double* A,
double* x,
double* b,
int /*nub*/,
double* lo,
double* hi,
int* findex)
{
dSolveLCP(n, A, x, b, nullptr, 0, lo, hi, findex);
}
extern "C" ODE_API int dTestSolveLCP()
{
const int n = 100;
//size_t memreq = EstimateTestSolveLCPMemoryReq(n);
//dxWorldProcessMemArena *arena = dxAllocateTemporaryWorldProcessMemArena(memreq, nullptr, nullptr);
//if (arena == nullptr) {
// return 0;
//}
//int i,nskip = dPAD(n);
int i;
int nskip = n;
#ifdef dDOUBLE
const dReal tol = REAL(1e-9);
#endif
#ifdef dSINGLE
const dReal tol = REAL(1e-4);
#endif
printf ("dTestSolveLCP()\n");
dReal *A = new dReal[n*nskip];
dReal *x = new dReal[n];
dReal *b = new dReal[n];
dReal *w = new dReal[n];
dReal *lo = new dReal[n];
dReal *hi = new dReal[n];
dReal *A2 = new dReal[n*nskip];
dReal *b2 = new dReal[n];
dReal *lo2 = new dReal[n];
dReal *hi2 = new dReal[n];
dReal *tmp1 = new dReal[n];
dReal *tmp2 = new dReal[n];
// double total_time = 0;
for (int count=0; count < 1000; count++) {
// form (A,b) = a random positive definite LCP problem
dMakeRandomMatrix (A2,n,n,1.0);
dMultiply2 (A,A2,A2,n,n,n);
dMakeRandomMatrix (x,n,1,1.0);
dMultiply0 (b,A,x,n,n,1);
for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1);
// choose `nub' in the range 0..n-1
int nub = 50; //dRandInt (n);
// make limits
for (i=0; i<nub; i++) lo[i] = -dInfinity;
for (i=0; i<nub; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = 0;
//for (i=nub; i<n; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = -dInfinity;
//for (i=nub; i<n; i++) hi[i] = 0;
for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01);
for (i=nub; i<n; i++) hi[i] = (dRandReal()*REAL(1.0))+REAL(0.01);
// set a few limits to lo=hi=0
/*
for (i=0; i<10; i++) {
int j = dRandInt (n-nub) + nub;
lo[j] = 0;
hi[j] = 0;
}
*/
// solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for
// SolveLCP() to permute. also, we'll clear the upper triangle of A2 to
// ensure that it doesn't get referenced (if it does, the answer will be
// wrong).
memcpy (A2,A,n*nskip*sizeof(dReal));
dClearUpperTriangle (A2,n);
memcpy (b2,b,n*sizeof(dReal));
memcpy (lo2,lo,n*sizeof(dReal));
memcpy (hi2,hi,n*sizeof(dReal));
dSetZero (x,n);
dSetZero (w,n);
dSolveLCP (n,A2,x,b2,w,nub,lo2,hi2,0);
// check the solution
dMultiply0 (tmp1,A,x,n,n,1);
for (i=0; i<n; i++) tmp2[i] = b[i] + w[i];
dReal diff = dMaxDifference (tmp1,tmp2,n,1);
// printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff,
// diff > tol ? "FAILED" : "passed");
if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff);
int n1=0,n2=0,n3=0;
for (i=0; i<n; i++) {
if (x[i]==lo[i] && w[i] >= 0) {
n1++; // ok
}
else if (x[i]==hi[i] && w[i] <= 0) {
n2++; // ok
}
else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) {
n3++; // ok
}
else {
dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i,
x[i],w[i],lo[i],hi[i]);
}
}
// pacifier
printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3);
}
delete[] A;
delete[] x;
delete[] b;
delete[] w;
delete[] lo ;
delete[] hi ;
delete[] A2 ;
delete[] b2 ;
delete[] lo2;
delete[] hi2;
delete[] tmp1;
delete[] tmp2;
return 1;
}
extern "C" ODE_API int dTestSolveLCP()
{
const int n = 100;
size_t memreq = EstimateTestSolveLCPMemoryReq(n);
dxWorldProcessMemArena *arena = dxAllocateTemporaryWorldProcessMemArena(memreq, NULL, NULL);
if (arena == NULL) {
return 0;
}
int i,nskip = dPAD(n);
#ifdef dDOUBLE
const dReal tol = REAL(1e-9);
#endif
#ifdef dSINGLE
const dReal tol = REAL(1e-4);
#endif
printf ("dTestSolveLCP()\n");
dReal *A = arena->AllocateArray<dReal> (n*nskip);
dReal *x = arena->AllocateArray<dReal> (n);
dReal *b = arena->AllocateArray<dReal> (n);
dReal *w = arena->AllocateArray<dReal> (n);
dReal *lo = arena->AllocateArray<dReal> (n);
dReal *hi = arena->AllocateArray<dReal> (n);
dReal *A2 = arena->AllocateArray<dReal> (n*nskip);
dReal *b2 = arena->AllocateArray<dReal> (n);
dReal *lo2 = arena->AllocateArray<dReal> (n);
dReal *hi2 = arena->AllocateArray<dReal> (n);
dReal *tmp1 = arena->AllocateArray<dReal> (n);
dReal *tmp2 = arena->AllocateArray<dReal> (n);
double total_time = 0;
for (int count=0; count < 1000; count++) {
BEGIN_STATE_SAVE(arena, saveInner) {
// form (A,b) = a random positive definite LCP problem
dMakeRandomMatrix (A2,n,n,1.0);
dMultiply2 (A,A2,A2,n,n,n);
dMakeRandomMatrix (x,n,1,1.0);
dMultiply0 (b,A,x,n,n,1);
for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1);
// choose `nub' in the range 0..n-1
int nub = 50; //dRandInt (n);
// make limits
for (i=0; i<nub; i++) lo[i] = -dInfinity;
for (i=0; i<nub; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = 0;
//for (i=nub; i<n; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = -dInfinity;
//for (i=nub; i<n; i++) hi[i] = 0;
for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01);
for (i=nub; i<n; i++) hi[i] = (dRandReal()*REAL(1.0))+REAL(0.01);
// set a few limits to lo=hi=0
/*
for (i=0; i<10; i++) {
int j = dRandInt (n-nub) + nub;
lo[j] = 0;
hi[j] = 0;
}
*/
// solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for
// SolveLCP() to permute. also, we'll clear the upper triangle of A2 to
// ensure that it doesn't get referenced (if it does, the answer will be
// wrong).
memcpy (A2,A,n*nskip*sizeof(dReal));
dClearUpperTriangle (A2,n);
memcpy (b2,b,n*sizeof(dReal));
memcpy (lo2,lo,n*sizeof(dReal));
memcpy (hi2,hi,n*sizeof(dReal));
dSetZero (x,n);
dSetZero (w,n);
dStopwatch sw;
dStopwatchReset (&sw);
dStopwatchStart (&sw);
dSolveLCP (arena,n,A2,x,b2,w,nub,lo2,hi2,0);
dStopwatchStop (&sw);
double time = dStopwatchTime(&sw);
total_time += time;
double average = total_time / double(count+1) * 1000.0;
// check the solution
dMultiply0 (tmp1,A,x,n,n,1);
for (i=0; i<n; i++) tmp2[i] = b[i] + w[i];
dReal diff = dMaxDifference (tmp1,tmp2,n,1);
// printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff,
// diff > tol ? "FAILED" : "passed");
if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff);
int n1=0,n2=0,n3=0;
for (i=0; i<n; i++) {
if (x[i]==lo[i] && w[i] >= 0) {
n1++; // ok
}
else if (x[i]==hi[i] && w[i] <= 0) {
n2++; // ok
}
else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) {
n3++; // ok
}
else {
dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i,
x[i],w[i],lo[i],hi[i]);
}
}
// pacifier
printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3);
printf ("time=%10.3f ms avg=%10.4f\n",time * 1000.0,average);
} END_STATE_SAVE(arena, saveInner);
}
bool LCPSolver::Solve(const MatrixXd& _A, const VectorXd& _b, VectorXd& _x, double mu, int numDir, bool bUseODESolver)
{
if (!bUseODESolver)
{
int err = Lemke(_A, _b, _x);
return (err == 0);
}
else
{
assert(numDir >= 4);
MatrixXd AODE;
VectorXd bODE;
transferToODEFormulation(_A, _b, AODE, bODE, numDir);
double* A, *b, *x, *w, *lo, *hi;
int n = AODE.rows();
int nContacts = n / 3;
int nSkip = dPAD(n);
A = new double[n * nSkip];
b = new double[n];
x = new double[n];
w = new double[n];
lo = new double[n];
hi = new double[n];
int* findex = new int[n];
memset(A, 0, n * nSkip * sizeof(double));
for (int i = 0; i < n; ++i)
{
for (int j = 0; j < n; ++j)
{
A[i * nSkip + j] = AODE(i, j);
}
}
for (int i = 0; i < n; ++i)
{
b[i] = -bODE[i];
x[i] = w[i] = lo[i] = 0;
hi[i] = dInfinity;
}
for (int i = 0; i < nContacts; ++i)
{
findex[i] = -1;
findex[nContacts + i * 2 + 0] = i;
findex[nContacts + i * 2 + 1] = i;
lo[nContacts + i * 2 + 0] = -mu;
lo[nContacts + i * 2 + 1] = -mu;
hi[nContacts + i * 2 + 0] = mu;
hi[nContacts + i * 2 + 1] = mu;
}
// dClearUpperTriangle (A,n);
dSolveLCP (n,A,x,b,w,0,lo,hi,findex);
VectorXd xODE = VectorXd::Zero(n);
for (int i = 0; i < n; ++i)
{
xODE[i] = x[i];
}
transferSolFromODEFormulation(xODE, _x, numDir);
// checkIfSolution(reducedA, reducedb, _x);
delete[] A;
delete[] b;
delete[] x;
delete[] w;
delete[] lo;
delete[] hi;
delete[] findex;
return 1;
}
}
//==============================================================================
void DantzigLCPSolver::solve(ConstrainedGroup* _group)
{
// If there is no constraint, then just return true.
size_t numConstraints = _group->getNumConstraints();
if (numConstraints == 0)
return;
// Build LCP terms by aggregating them from constraints
size_t n = _group->getTotalDimension();
int nSkip = dPAD(n);
double* A = new double[n * nSkip];
double* x = new double[n];
double* b = new double[n];
double* w = new double[n];
double* lo = new double[n];
double* hi = new double[n];
int* findex = new int[n];
// Set w to 0 and findex to -1
#ifndef NDEBUG
std::memset(A, 0.0, n * nSkip * sizeof(double));
#endif
std::memset(w, 0.0, n * sizeof(double));
std::memset(findex, -1, n * sizeof(int));
// Compute offset indices
size_t* offset = new size_t[n];
offset[0] = 0;
// std::cout << "offset[" << 0 << "]: " << offset[0] << std::endl;
for (size_t i = 1; i < numConstraints; ++i)
{
ConstraintBase* constraint = _group->getConstraint(i - 1);
assert(constraint->getDimension() > 0);
offset[i] = offset[i - 1] + constraint->getDimension();
// std::cout << "offset[" << i << "]: " << offset[i] << std::endl;
}
// For each constraint
ConstraintInfo constInfo;
constInfo.invTimeStep = 1.0 / mTimeStep;
ConstraintBase* constraint;
for (size_t i = 0; i < numConstraints; ++i)
{
constraint = _group->getConstraint(i);
constInfo.x = x + offset[i];
constInfo.lo = lo + offset[i];
constInfo.hi = hi + offset[i];
constInfo.b = b + offset[i];
constInfo.findex = findex + offset[i];
constInfo.w = w + offset[i];
// Fill vectors: lo, hi, b, w
constraint->getInformation(&constInfo);
// Fill a matrix by impulse tests: A
constraint->excite();
for (size_t j = 0; j < constraint->getDimension(); ++j)
{
// Adjust findex for global index
if (findex[offset[i] + j] >= 0)
findex[offset[i] + j] += offset[i];
// Apply impulse for mipulse test
constraint->applyUnitImpulse(j);
// Fill upper triangle blocks of A matrix
int index = nSkip * (offset[i] + j) + offset[i];
constraint->getVelocityChange(A + index, true);
for (size_t k = i + 1; k < numConstraints; ++k)
{
index = nSkip * (offset[i] + j) + offset[k];
_group->getConstraint(k)->getVelocityChange(A + index, false);
}
// Filling symmetric part of A matrix
for (size_t k = 0; k < i; ++k)
{
for (size_t l = 0; l < _group->getConstraint(k)->getDimension(); ++l)
{
int index1 = nSkip * (offset[i] + j) + offset[k] + l;
int index2 = nSkip * (offset[k] + l) + offset[i] + j;
A[index1] = A[index2];
}
}
}
assert(isSymmetric(n, A, offset[i],
offset[i] + constraint->getDimension() - 1));
constraint->unexcite();
}
assert(isSymmetric(n, A));
// Print LCP formulation
// dtdbg << "Before solve:" << std::endl;
// print(n, A, x, lo, hi, b, w, findex);
// std::cout << std::endl;
// Solve LCP using ODE's Dantzig algorithm
dSolveLCP(n, A, x, b, w, 0, lo, hi, findex);
// Print LCP formulation
// dtdbg << "After solve:" << std::endl;
// print(n, A, x, lo, hi, b, w, findex);
// std::cout << std::endl;
// Apply constraint impulses
for (size_t i = 0; i < numConstraints; ++i)
{
constraint = _group->getConstraint(i);
constraint->applyImpulse(x + offset[i]);
constraint->excite();
}
delete[] offset;
delete[] A;
delete[] x;
delete[] b;
delete[] w;
delete[] lo;
delete[] hi;
delete[] findex;
}
bool ODELCPSolver::Solve(const Eigen::MatrixXd& _A,
const Eigen::VectorXd& _b,
Eigen::VectorXd* _x,
int _numContacts,
double _mu,
int _numDir,
bool _bUseODESolver) {
if (!_bUseODESolver) {
int err = Lemke(_A, _b, _x);
return (err == 0);
} else {
assert(_numDir >= 4);
DART_UNUSED(_numDir);
double* A, *b, *x, *w, *lo, *hi;
int n = _A.rows();
int nSkip = dPAD(n);
A = new double[n * nSkip];
b = new double[n];
x = new double[n];
w = new double[n];
lo = new double[n];
hi = new double[n];
int* findex = new int[n];
memset(A, 0, n * nSkip * sizeof(double));
for (int i = 0; i < n; ++i) {
for (int j = 0; j < n; ++j) {
A[i * nSkip + j] = _A(i, j);
}
}
for (int i = 0; i < n; ++i) {
b[i] = -_b[i];
x[i] = w[i] = lo[i] = 0;
hi[i] = dInfinity;
findex[i] = -1;
}
for (int i = 0; i < _numContacts; ++i) {
findex[_numContacts + i * 2 + 0] = i;
findex[_numContacts + i * 2 + 1] = i;
lo[_numContacts + i * 2 + 0] = -_mu;
lo[_numContacts + i * 2 + 1] = -_mu;
hi[_numContacts + i * 2 + 0] = _mu;
hi[_numContacts + i * 2 + 1] = _mu;
}
// dClearUpperTriangle (A,n);
dSolveLCP(n, A, x, b, w, 0, lo, hi, findex);
// for (int i = 0; i < n; i++) {
// if (w[i] < 0.0 && abs(x[i] - hi[i]) > 0.000001)
// cout << "w[" << i << "] is negative, but x is " << x[i] << endl;
// else if (w[i] > 0.0 && abs(x[i] - lo[i]) > 0.000001)
// cout << "w[" << i << "] is positive, but x is " << x[i]
// << " lo is " << lo[i] << endl;
// else if (abs(w[i]) < 0.000001 && (x[i] > hi[i] || x[i] < lo[i]))
// cout << "w[i] " << i << " is zero, but x is " << x[i] << endl;
// }
*_x = Eigen::VectorXd(n);
for (int i = 0; i < n; ++i) {
(*_x)[i] = x[i];
}
// checkIfSolution(reducedA, reducedb, _x);
delete[] A;
delete[] b;
delete[] x;
delete[] w;
delete[] lo;
delete[] hi;
delete[] findex;
return 1;
}
}
void
dInternalStepFast (dxWorld * /*world*/, dxBody * body[2], dReal * /*GI*/[2], dReal * GinvI[2], dxJoint * joint, dxJoint::Info1 info, dxJoint::Info2 Jinfo, dReal stepsize)
{
int i, j, k;
dReal stepsize1 = dRecip (stepsize);
int m = info.m;
// nothing to do if no constraints.
if (m <= 0)
return;
int nub = 0;
if (info.nub == info.m)
nub = m;
// compute A = J*invM*J'. first compute JinvM = J*invM. this has the same
// format as J so we just go through the constraints in J multiplying by
// the appropriate scalars and matrices.
dReal JinvM[2 * 6 * 8];
//dSetZero (JinvM, 2 * m * 8);
dReal *Jsrc = Jinfo.J1l;
dReal *Jdst = JinvM;
if (body[0])
{
for (j = m - 1; j >= 0; j--)
{
for (k = 0; k < 3; k++)
Jdst[k] = dMUL(Jsrc[k],body[0]->invMass);
dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[0]);
Jsrc += 8;
Jdst += 8;
}
}
if (body[1])
{
Jsrc = Jinfo.J2l;
Jdst = JinvM + 8 * m;
for (j = m - 1; j >= 0; j--)
{
for (k = 0; k < 3; k++)
Jdst[k] = dMUL(Jsrc[k],body[1]->invMass);
dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[1]);
Jsrc += 8;
Jdst += 8;
}
}
// now compute A = JinvM * J'.
int mskip = dPAD (m);
dReal A[6 * 8];
//dSetZero (A, 6 * 8);
if (body[0]) {
Multiply2_sym_p8p (A, JinvM, Jinfo.J1l, m, mskip);
if (body[1])
MultiplyAdd2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l,
m, mskip);
} else {
if (body[1])
Multiply2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l,
m, mskip);
}
// add cfm to the diagonal of A
for (i = 0; i < m; i++)
A[i * mskip + i] += dMUL(Jinfo.cfm[i],stepsize1);
// compute the right hand side `rhs'
dReal tmp1[16];
//dSetZero (tmp1, 16);
// put v/h + invM*fe into tmp1
for (i = 0; i < 2; i++)
{
if (!body[i])
continue;
for (j = 0; j < 3; j++)
tmp1[i * 8 + j] = dMUL(body[i]->facc[j],body[i]->invMass) + dMUL(body[i]->lvel[j],stepsize1);
dMULTIPLY0_331 (tmp1 + i * 8 + 4, GinvI[i], body[i]->tacc);
for (j = 0; j < 3; j++)
tmp1[i * 8 + 4 + j] += dMUL(body[i]->avel[j],stepsize1);
}
// put J*tmp1 into rhs
dReal rhs[6];
//dSetZero (rhs, 6);
if (body[0]) {
Multiply0_p81 (rhs, Jinfo.J1l, tmp1, m);
if (body[1])
MultiplyAdd0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m);
} else {
if (body[1])
Multiply0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m);
}
// complete rhs
for (i = 0; i < m; i++)
rhs[i] = dMUL(Jinfo.c[i],stepsize1) - rhs[i];
#ifdef SLOW_LCP
// solve the LCP problem and get lambda.
// this will destroy A but that's okay
dReal *lambda = (dReal *) ALLOCA (m * sizeof (dReal));
dReal *residual = (dReal *) ALLOCA (m * sizeof (dReal));
dReal lo[6], hi[6];
memcpy (lo, Jinfo.lo, m * sizeof (dReal));
memcpy (hi, Jinfo.hi, m * sizeof (dReal));
dSolveLCP (m, A, lambda, rhs, residual, nub, lo, hi, Jinfo.findex);
#endif
// compute the constraint force `cforce'
// compute cforce = J'*lambda
dJointFeedback *fb = joint->feedback;
dReal cforce[16];
//dSetZero (cforce, 16);
if (fb)
{
// the user has requested feedback on the amount of force that this
// joint is applying to the bodies. we use a slightly slower
// computation that splits out the force components and puts them
// in the feedback structure.
dReal data1[8], data2[8];
if (body[0])
{
Multiply1_8q1 (data1, Jinfo.J1l, lambda, m);
dReal *cf1 = cforce;
cf1[0] = (fb->f1[0] = data1[0]);
cf1[1] = (fb->f1[1] = data1[1]);
cf1[2] = (fb->f1[2] = data1[2]);
cf1[4] = (fb->t1[0] = data1[4]);
cf1[5] = (fb->t1[1] = data1[5]);
cf1[6] = (fb->t1[2] = data1[6]);
}
if (body[1])
{
Multiply1_8q1 (data2, Jinfo.J2l, lambda, m);
dReal *cf2 = cforce + 8;
cf2[0] = (fb->f2[0] = data2[0]);
cf2[1] = (fb->f2[1] = data2[1]);
cf2[2] = (fb->f2[2] = data2[2]);
cf2[4] = (fb->t2[0] = data2[4]);
cf2[5] = (fb->t2[1] = data2[5]);
cf2[6] = (fb->t2[2] = data2[6]);
}
}
else
{
// no feedback is required, let's compute cforce the faster way
if (body[0])
Multiply1_8q1 (cforce, Jinfo.J1l, lambda, m);
if (body[1])
Multiply1_8q1 (cforce + 8, Jinfo.J2l, lambda, m);
}
for (i = 0; i < 2; i++)
{
if (!body[i])
continue;
for (j = 0; j < 3; j++)
{
body[i]->facc[j] += cforce[i * 8 + j];
body[i]->tacc[j] += cforce[i * 8 + 4 + j];
}
}
}
