Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add relative tolerance for linear solver and newton with line search

Browse Source
This commit is contained in:
Xuchen Han
2019-09-09 16:35:37 -07:00
committed by Xuchen Han
parent 36278edc00
commit 1bc75cc833
6 changed files with 167 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
src/BulletSoftBody/btConjugateGradient.h
View File

@@ -26,16 +26,18 @@ class btConjugateGradient
typedef btAlignedObjectArray<btVector3> TVStack;
TVStack r,p,z,temp;
int max_iterations;
btScalar tolerance;
public:
btConjugateGradient(const int max_it_in)
: max_iterations(max_it_in)
{
tolerance = 1024 * std::numeric_limits<btScalar>::epsilon();
}
virtual ~btConjugateGradient(){}
// return the number of iterations taken
int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance, bool verbose = false)
int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar relative_tolerance, bool verbose = false)
{
BT_PROFILE("CGSolve");
btAssert(x.size() == b.size());
@@ -48,7 +50,8 @@ public:
A.precondition(r, z);
A.project(z);
btScalar r_dot_z = dot(z,r);
if (dot(z,z) < tolerance) {
btScalar local_tolerance = btMin(relative_tolerance * std::sqrt(r_dot_z), tolerance);
if (std::sqrt(r_dot_z) < local_tolerance) {
if (verbose)
{
std::cout << "Iteration = 0" << std::endl;
@@ -58,11 +61,21 @@ public:
}
p = z;
btScalar r_dot_z_new = r_dot_z;
for (int k = 1; k < max_iterations; k++) {
for (int k = 1; k <= max_iterations; k++) {
// temp = A*p
A.multiply(p, temp);
A.project(temp);
// alpha = r^T * z / (p^T * A * p)
if (dot(p,temp) < 0)
{
if (verbose)
std::cout << "Encountered negative direction in CG!"<<std::endl;
if (k == 1)
{
x = b;
}
return k;
}
btScalar alpha = r_dot_z_new / dot(p, temp);
// x += alpha * p;
multAndAddTo(alpha, p, x);
@@ -72,7 +85,7 @@ public:
A.precondition(r, z);
r_dot_z = r_dot_z_new;
r_dot_z_new = dot(r,z);
if (r_dot_z_new < tolerance) {
if (std::sqrt(r_dot_z_new) < local_tolerance) {
if (verbose)
{
std::cout << "ConjugateGradient iterations " << k << std::endl;

14
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@@ -134,9 +134,19 @@ btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual
btScalar mag = 0;
for (int i = 0; i < residual.size(); ++i)
{
mag += residual[i].length();
mag += residual[i].length2();
}
return mag;
return std::sqrt(mag);
}
btScalar btDeformableBackwardEulerObjective::totalEnergy()
{
btScalar e = 0;
for (int i = 0; i < m_lf.size(); ++i)
{
e += m_lf[i]->totalElasticEnergy();
}
return e;
}
void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)

2
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@@ -124,6 +124,8 @@ public:
{
m_implicit = implicit;
}
btScalar totalEnergy();
};
#endif /* btBackwardEulerObjective_h */

118
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -23,6 +23,11 @@ btDeformableBodySolver::btDeformableBodySolver()
, m_cg(20)
, m_maxNewtonIterations(5)
, m_newtonTolerance(1e-4)
//, m_lineSearch(false)
//, m_cg(10)
//, m_maxNewtonIterations(5)
//, m_newtonTolerance(1e-3)
, m_lineSearch(true)
{
m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
}
@@ -63,13 +68,37 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
}
m_objective->computeResidual(solverdt, m_residual);
if (m_objective->computeNorm(m_residual) < m_newtonTolerance)
if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0)
{
break;
}
m_objective->applyDynamicFriction(m_residual);
computeStep(m_ddv, m_residual);
updateDv();
if (m_lineSearch)
{
btScalar inner_product = computeDescentStep(m_ddv,m_residual);
btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8
btScalar scale = 2;
btScalar f0 = m_objective->totalEnergy()+kineticEnergy(), f1, f2;
backupDv();
do {
scale *= beta;
if (scale < 1e-8) {
//std::cout << "Could not find sufficient descent!" << std::endl;
return;
}
updateEnergy(scale);
f1 = m_objective->totalEnergy()+kineticEnergy();
f2 = f0 - alpha * scale * inner_product;
} while (!(f1 < f2)); // if anything here is nan then the search continues
revertDv();
updateDv(scale);
}
else
{
computeStep(m_ddv, m_residual);
updateDv();
}
for (int j = 0; j < m_numNodes; ++j)
{
m_ddv[j].setZero();
@@ -79,26 +108,99 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
}
}
btScalar btDeformableBodySolver::kineticEnergy()
{
btScalar ke = 0;
for (int i = 0; i < m_softBodySet.size();++i)
{
btSoftBody* psb = m_softBodySet[i];
for (int j = 0; j < psb->m_nodes.size();++j)
{
btSoftBody::Node& node = psb->m_nodes[j];
if (node.m_im > 0)
{
ke += m_dv[node.index].length2() * 0.5 / node.m_im;
}
}
}
return ke;
}
void btDeformableBodySolver::backupDv()
{
m_backup_dv.resize(m_dv.size());
for (int i = 0; i<m_backup_dv.size(); ++i)
{
m_backup_dv[i] = m_dv[i];
}
}
void btDeformableBodySolver::revertDv()
{
for (int i = 0; i<m_backup_dv.size(); ++i)
{
m_dv[i] = m_backup_dv[i];
}
}
void btDeformableBodySolver::updateEnergy(btScalar scale)
{
for (int i = 0; i<m_dv.size(); ++i)
{
m_dv[i] = m_backup_dv[i] + scale * m_ddv[i];
}
updateState();
}
btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual)
{
btScalar relative_tolerance = btMin(0.5, std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance)));
m_cg.solve(*m_objective, ddv, residual, relative_tolerance, false);
btScalar inner_product = m_cg.dot(residual, m_ddv);
btScalar tol = 1e-5 * m_objective->computeNorm(residual) * m_objective->computeNorm(m_ddv);
if (inner_product < -tol)
{
std::cout << "Looking backwards!" << std::endl;
for (int i = 0; i < m_ddv.size();++i)
{
m_ddv[i] = -m_ddv[i];
}
inner_product = -inner_product;
}
else if (std::abs(inner_product) < tol)
{
std::cout << "Gradient Descent!" << std::endl;
btScalar res_norm = m_objective->computeNorm(residual);
btScalar scale = m_objective->computeNorm(m_ddv) / res_norm;
for (int i = 0; i < m_ddv.size();++i)
{
m_ddv[i] = scale * residual[i];
}
inner_product = scale * res_norm * res_norm;
}
return inner_product;
}
void btDeformableBodySolver::updateState()
{
updateVelocity();
updateTempPosition();
}
void btDeformableBodySolver::updateDv()
void btDeformableBodySolver::updateDv(btScalar scale)
{
for (int i = 0; i < m_numNodes; ++i)
{
m_dv[i] += m_ddv[i];
m_dv[i] += scale * m_ddv[i];
}
}
void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
{
//btScalar tolerance = std::numeric_limits<btScalar>::epsilon() * m_objective->computeNorm(residual);
btScalar tolerance = std::numeric_limits<btScalar>::epsilon();
m_cg.solve(*m_objective, ddv, residual, tolerance);
btScalar relative_tolerance = btMin(0.5, std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance)));
m_cg.solve(*m_objective, ddv, residual, relative_tolerance, false);
}
void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)

9
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -34,6 +34,7 @@ class btDeformableBodySolver : public btSoftBodySolver
protected:
int m_numNodes;
TVStack m_dv;
TVStack m_backup_dv;
TVStack m_ddv;
TVStack m_residual;
btAlignedObjectArray<btSoftBody *> m_softBodySet;
@@ -45,6 +46,7 @@ protected:
bool m_implicit;
int m_maxNewtonIterations;
btScalar m_newtonTolerance;
bool m_lineSearch;
public:
btDeformableBackwardEulerObjective* m_objective;
@@ -82,6 +84,7 @@ public:
bool updateNodes();
void computeStep(TVStack& dv, const TVStack& residual);
btScalar computeDescentStep(TVStack& ddv, const TVStack& residual);
virtual void predictMotion(btScalar solverdt);
@@ -103,9 +106,13 @@ public:
void updateState();
void updateDv();
void updateDv(btScalar scale = 1);
void updateTempPosition();
void backupDv();
void revertDv();
void updateEnergy(btScalar scale);
btScalar kineticEnergy();
};
#endif /* btDeformableBodySolver_h */

18
src/BulletSoftBody/btDeformableGravityForce.h
View File

@@ -72,6 +72,24 @@ public:
{
return BT_GRAVITY_FORCE;
}
virtual double totalElasticEnergy()
{
double e = 0;
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
const btSoftBody::Node& node = psb->m_nodes[j];
if (node.m_im > 0)
{
e -= m_gravity.dot(node.m_q)/node.m_im;
}
}
}
return e;
}
};