Commit e58d4082 authored by David Bommes's avatar David Bommes

finished AQP example + minor fixes

parent e1bb48a1
Pipeline #2360 passed with stage
in 25 minutes and 47 seconds
......@@ -28,7 +28,7 @@
#include <CoMISo/NSolver/FiniteElementProblem.hh>
#include <CoMISo/NSolver/NPDerivativeChecker.hh>
#include <CoMISo/NSolver/IPOPTSolver.hh>
//#include <CoMISo/NSolver/AcceleratedQuadraticProxy.hh>
#include <CoMISo/NSolver/AcceleratedQuadraticProxy.hh>
// create log barrier term for triangle orientation
......@@ -115,6 +115,55 @@ public:
_triplets.push_back(Triplet(0,5, d));
_triplets.push_back(Triplet(2,5,-d));
}
inline double max_feasible_step(const VecV& _x, const VecV& _v, const VecC& _c)
{
double a = ((-_v[0]+_v[2])*(-_v[1]+_v[5])-(-_v[0]+_v[4])*(-_v[1]+_v[3]));
double b = ((-_x[0]+_x[2])*(-_v[1]+_v[5])+(-_v[0]+_v[2])*(-_x[1]+_x[5])-(-_x[0]+_x[4])*(-_v[1]+_v[3])-(-_v[0]+_v[4])*(-_x[1]+_x[3]));
double c = (-_x[0]+_x[2])*(-_x[1]+_x[5])-(-_x[0]+_x[4])*(-_x[1]+_x[3]);
double a2 = a*a;
double b2 = b*b;
double c2 = c*c;
double eps = 1e-9*(a2+b2+c2);
// special case a=0?
if(a2 <= eps)
{
if(b2 <= eps) // special case a=b=0
{
if(c2 <= eps) return 0.0; // special case a=b=c=0
else return DBL_MAX; // special case a=b=0 & c!=0
}
else // special case a=0 & b!=0
{
double t=-c/b;
if( t>=0 ) return t;
else return DBL_MAX;
}
}
else
{
// discriminant
double d = b*b-4*a*c;
if(d < 0 ) return DBL_MAX;
else
{
d = std::sqrt(d);
double t1 = (-b+d)/(2.0*a);
double t2 = (-b-d)/(2.0*a);
if(t1 < 0.0) t1 = DBL_MAX;
if(t2 < 0.0) t2 = DBL_MAX;
return std::min(t1,t2);
}
}
}
};
// create a simple finite element (x-c)^2
......@@ -146,6 +195,11 @@ public:
_triplets.clear();
_triplets.push_back(Triplet(0,0,2));
}
inline double max_feasible_step(const VecV& _x, const VecV& _v, const VecC& _c)
{
return DBL_MAX;
}
};
......@@ -154,6 +208,16 @@ public:
// Example main
int main(void)
{
{
TriangleOrientationLogBarrierElement tolbe;
TriangleOrientationLogBarrierElement::VecV x; x << 0.0, 0.0, 2.0, 0.0, 1.0, 1.0;
TriangleOrientationLogBarrierElement::VecV v; v << 0.0, 0.0, 0.0, 0.0,-1.0,-1.0;
TriangleOrientationLogBarrierElement::VecC c; c << 1.0;
std::cerr << "max feasible step: " << tolbe.max_feasible_step(x,v,c) << std::endl;
}
std::cout << "---------- 1) Get an instance of a FiniteElementProblem..." << std::endl;
// first create sets of different finite elements
......@@ -161,30 +225,32 @@ int main(void)
TriangleOrientationLogBarrierElement::VecI tidx;
tidx << 0,1,2,3,4,5;
const TriangleOrientationLogBarrierElement::VecC c(1.0);
TriangleOrientationLogBarrierElement::VecC c;
c[0] = 1.0;
fe_set.instances().add_element(tidx, c);
std::cerr << "T1" << std::endl;
// second set for boundary conditions
COMISO::FiniteElementSet<SimpleElement2> fe_set2;
SimpleElement2::VecI idx;
SimpleElement2::VecV c2;
SimpleElement2::VecC c2;
idx[0] = 0; c2[0] = 0.0; fe_set2.instances().add_element(idx, c2);
idx[0] = 1; c2[0] = 0.0; fe_set2.instances().add_element(idx, c2);
idx[0] = 2; c2[0] = 2.0; fe_set2.instances().add_element(idx, c2);
idx[0] = 3; c2[0] = 0.0; fe_set2.instances().add_element(idx, c2);
idx[0] = 4; c2[0] = 1.0; fe_set2.instances().add_element(idx, c2);
idx[0] = 5; c2[0] = -2.0; fe_set2.instances().add_element(idx, c2);
// fe_set2.instances().add_element(SimpleElement2::VecI(1), SimpleElement2::VecV(0));
// fe_set2.instances().add_element(SimpleElement2::VecI(2), SimpleElement2::VecV(2));
// fe_set2.instances().add_element(SimpleElement2::VecI(3), SimpleElement2::VecV(0));
// fe_set2.instances().add_element(SimpleElement2::VecI(4), SimpleElement2::VecV(1));
// fe_set2.instances().add_element(SimpleElement2::VecI(5), SimpleElement2::VecV(1));
std::cerr << "T2" << std::endl;
// then create finite element problem and add sets
COMISO::FiniteElementProblem fe_problem(3);
COMISO::FiniteElementProblem fe_problem(6);
fe_problem.add_set(&fe_set );
fe_problem.add_set(&fe_set2);
std::cerr << "T3" << std::endl;
// set initial values
fe_problem.x()[0] = 0.0;
fe_problem.x()[1] = 0.0;
......@@ -193,6 +259,8 @@ int main(void)
fe_problem.x()[4] = 1.0;
fe_problem.x()[5] = 1.0;
std::cerr << "T4" << std::endl;
std::cout << "---------- 2) (optional for debugging) Check derivatives of problem..." << std::endl;
COMISO::NPDerivativeChecker npd;
npd.check_all(&fe_problem);
......@@ -212,6 +280,47 @@ int main(void)
for(unsigned int i=0; i<fe_problem.x().size(); ++i)
std::cerr << "x[" << i << "] = " << fe_problem.x()[i] << std::endl;
std::cout << "---------- 5) Get AQP Solver... " << std::endl;
COMISO::AcceleratedQuadraticProxy aqp;
std::cout << "---------- 6) Solve..." << std::endl;
// there are no constraints -> provide an empty vector
// then create finite element problems
// first the nonlinear part
COMISO::FiniteElementProblem fe_problem2(6);
fe_problem2.add_set(&fe_set );
// second the quadratic part
COMISO::FiniteElementProblem fe_problem3(6);
fe_problem3.add_set(&fe_set2);
// set initial values
fe_problem3.x()[0] = 0.0;
fe_problem3.x()[1] = 0.0;
fe_problem3.x()[2] = 2.0;
fe_problem3.x()[3] = 0.0;
fe_problem3.x()[4] = 1.0;
fe_problem3.x()[5] = 1.0;
COMISO::AcceleratedQuadraticProxy::SMatrixD A(0,6);
A.setZero();
COMISO::AcceleratedQuadraticProxy::VectorD b; b.resize(0);
// alternatively with constraints
// COMISO::AcceleratedQuadraticProxy::SMatrixD A(2,6);
// A.setZero(); A.coeffRef(0,0) = 1.0; A.coeffRef(1,1) = 1.0;
// COMISO::AcceleratedQuadraticProxy::VectorD b(2);
// b[0] = b[1] = 0.0;
// solve
aqp.solve(&fe_problem3, &fe_problem2, A, b);
// print result
for(unsigned int i=0; i<fe_problem3.x().size(); ++i)
std::cerr << "x[" << i << "] = " << fe_problem3.x()[i] << std::endl;
return 0;
}
......@@ -63,6 +63,12 @@ public:
_triplets.push_back(Triplet(0,1,-2));
_triplets.push_back(Triplet(1,0,-2));
}
inline double max_feasible_step(const VecV& _x, const VecV& _v, const VecC& _c)
{
return DBL_MAX;
}
};
// create a simple finite element (x-c)^2
......@@ -94,6 +100,12 @@ public:
_triplets.clear();
_triplets.push_back(Triplet(0,0,2));
}
inline double max_feasible_step(const VecV& _x, const VecV& _v, const VecC& _c)
{
return DBL_MAX;
}
};
......
......@@ -96,7 +96,7 @@ public:
}
// advanced properties
virtual bool constant_hessian() { return false; }
virtual bool constant_hessian() const { return false; }
};
......
......@@ -62,6 +62,7 @@ public:
// storage of acceleration vector and update vector dx and rhs of KKT system
VectorD dx(n+m), rhs(n+m), g(n);
VectorD dx2(n);
rhs.setZero();
// resize temp vector for line search (and set to x1 to approx Hessian correctly if _quadratic problem is non-quadratic!)
......@@ -81,7 +82,7 @@ public:
double t_max = std::min(theta, 0.5*_nonlinear_problem->max_feasible_step(x1.data(), dx.data()));
// accelerate and update x1 and x2 (x1 will be accelerated point and x2 the previous x1)
x2 = x1 + dx.head(n)*t_max);
x2 = x1 + dx.head(n)*t_max;
x2.swap(x1);
// solve KKT
......@@ -94,14 +95,25 @@ public:
t_max = std::min(1.0, 0.5*_nonlinear_problem->max_feasible_step(x1.data(), dx.data()));
double rel_df(0.0);
double t = backtracking_line_search(_quadratic_problem, _nonlinear_problem, x1, g, dx, rel_df, t_max);
dx2 = dx.head(n);
double t = backtracking_line_search(_quadratic_problem, _nonlinear_problem, x1, g, dx2, rel_df, t_max);
x1 += dx.head(n)*t;
std::cerr << "iter: " << iter << " eps = [f(x_old)-f(x_new)]/f(x_old) = " << rel_df << std::endl;
// converged?
if(rel_df < _eps)
if(rel_df < eps_)
break;
++iter;
}
// store result
_quadratic_problem->store_result(x1.data());
// return success
return 1;
}
protected:
......@@ -150,7 +162,7 @@ protected:
// DEB_line(2, "-> re-try with regularized constraints...");
std::cerr << "Eigen::SparseLU reported problem while factoring KKT system: " << lu_solver_.lastErrorMessage() << std::endl;
for(unsigned int i=0; i<m; ++i)
for( int i=0; i<m; ++i)
trips.push_back(Triplet(n+i,n+i,1e-9));
// create KKT matrix
......
......@@ -77,6 +77,17 @@ void FiniteElementProblem::eval_hessian ( const double* _x, SMatrixNP& _H)
}
double FiniteElementProblem::max_feasible_step( const double* _x, const double* _v)
{
double t = DBL_MAX;
for(unsigned int i=0; i<fe_sets_.size(); ++i)
t = std::min(t, fe_sets_[i]->max_feasible_step(_x, _v));
return t;
}
void FiniteElementProblem::store_result ( const double* _x )
{
if(n_ > 0)
......
......@@ -45,6 +45,8 @@ public:
virtual void accumulate_gradient( const double* _x , double* _g) = 0;
virtual void accumulate_hessian ( const double* _x , std::vector<Triplet>& _triplets) = 0;
virtual double max_feasible_step( const double* _x, const double* _v) { return DBL_MAX;}
};
......@@ -192,6 +194,25 @@ public:
}
}
virtual double max_feasible_step( const double* _x, const double* _v)
{
double t = DBL_MAX;
for(unsigned int i=0; i<instances_.size(); ++i)
{
// get local x vector and v vector
for(unsigned int j=0; j<NV; ++j)
{
x_[j] = _x[instances_.index(i,j)];
g_[j] = _v[instances_.index(i,j)];
}
t = std::min(t, element_.max_feasible_step(x_, g_, instances_.c(i)));
}
return t;
}
private:
FiniteElementType element_;
......@@ -236,6 +257,9 @@ public:
virtual void eval_hessian ( const double* _x, SMatrixNP& _H);
// return largest value t such that _x+t*_v is feasible
virtual double max_feasible_step( const double* _x, const double* _v);
virtual void store_result ( const double* _x );
......
......@@ -70,7 +70,7 @@ public:
// advanced properties
virtual bool constant_gradient () const { return false; }
virtual bool constant_hessian () const { return false; }
virtual double max_feasible_step ( const double* _x, const double* _v) const { return DBL_MAX; }
virtual double max_feasible_step ( const double* _x, const double* _v) { return DBL_MAX; }
};
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment