Skip to content

Commit

Permalink
fix boxqp indexing hack
Browse files Browse the repository at this point in the history
  • Loading branch information
@kazuotani14
kazuotani14 committed Mar 31, 2018
1 parent 51e2111 commit 398a7ea
Showing 1 changed file with 31 additions and 59 deletions.
90 changes: 31 additions & 59 deletions src/boxqp.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
c - bias vector (m)
x0 - initial state (m)
lower - lower bounds (m)
lower - lower bounds (m)
upper - upper bounds (m)
outputs:
Expand All @@ -20,8 +20,7 @@
*/

boxQPResult boxQP(const MatrixXd &Q, const VectorXd &c, const VectorXd &x0,
const VectorXd& lower, const VectorXd& upper)
{
const VectorXd& lower, const VectorXd& upper) {
int n_dims = x0.size();
assert(Q.cols() == n_dims);
assert(Q.rows() == n_dims);
Expand All @@ -43,13 +42,11 @@ boxQPResult boxQP(const MatrixXd &Q, const VectorXd &c, const VectorXd &x0,
VectorXd clamped_dims(n_dims);
VectorXd old_clamped_dims(n_dims);

for(int iter=0; iter<=qp_maxIter; iter++)
{
for(int iter=0; iter<=qp_maxIter; iter++) {
if(res.result != 0) break;

// Check if we've stopped improving
if(iter>0 && (oldvalue - val) < minRelImprove*std::abs(oldvalue))
{
if(iter>0 && (oldvalue - val) < minRelImprove*std::abs(oldvalue)) {
res.result = 4;
break;
}
Expand All @@ -60,41 +57,34 @@ boxQPResult boxQP(const MatrixXd &Q, const VectorXd &c, const VectorXd &x0,
old_clamped_dims = clamped_dims;
clamped_dims.setZero();
res.v_free.setOnes();
for (int i=0; i<n_dims; i++)
{
if( (approx_eq(x(i), lower(i)) && grad(i)>0) || (approx_eq(x(i), upper(i)) && grad(i)<0) )
{
for (int i=0; i<n_dims; i++) {
if( (approx_eq(x(i), lower(i)) && grad(i)>0) || (approx_eq(x(i), upper(i)) && grad(i)<0) ) {
clamped_dims(i) = 1;
res.v_free(i) = 0;
}
}

// Check if all dimensions are clamped
if(clamped_dims.all())
{
if(clamped_dims.all()) {
res.result = 6;
break;
}

// Factorize if clamped dimensions have changed
if (iter==0 || (old_clamped_dims-clamped_dims).sum() != 0)
{
if (iter==0 || (old_clamped_dims-clamped_dims).sum() != 0) {
MatrixXd Qfree;
Qfree = extract_bool_rowsandcols(Q, res.v_free);

Eigen::LLT<MatrixXd> choleskyOfQfree(Qfree); // Cholesky decomposition
if(choleskyOfQfree.matrixL().size() > 0) // I'm not sure why this happens, but just in case
{
if(choleskyOfQfree.matrixL().size() > 0) { // I'm not sure why this happens, but just in case TODO debug
res.H_free = choleskyOfQfree.matrixL().transpose();
}

nfactors++;
}

// Check if gradient norm is below threshold
double grad_norm = grad.cwiseProduct(res.v_free).norm();
if (grad_norm < minGrad)
{
double grad_norm = subvec_w_ind(grad, res.v_free).norm();
if (grad_norm < minGrad) {
res.result = 5;
break;
}
Expand All @@ -104,22 +94,25 @@ boxQPResult boxQP(const MatrixXd &Q, const VectorXd &c, const VectorXd &x0,

VectorXd search = VectorXd::Zero(x.size());

// TODO remove this hack - assumes size(x)==2
//what we want is Qfree = Q(v_free, v_ free)
if(res.v_free[0]==1 && res.v_free[1]==1)
{
// search(free) = -Hfree \ (Hfree' \ grad_clamped(free)) - x(free);
if (res.v_free.all()) {
search = -res.H_free.inverse() * (res.H_free.transpose().inverse()*subvec_w_ind(grad_clamped, res.v_free)) - subvec_w_ind(x, res.v_free);
}
else if (res.v_free[0]==1){
search(0) = (-res.H_free.inverse() * (res.H_free.transpose().inverse()*subvec_w_ind(grad_clamped, res.v_free)) - subvec_w_ind(x, res.v_free))(0);
}
else if (res.v_free[1]==1){
search(1) = (-res.H_free.inverse() * (res.H_free.transpose().inverse()*subvec_w_ind(grad_clamped, res.v_free)) - subvec_w_ind(x, res.v_free))(0);
else {
VectorXd search_update_vals = (-res.H_free.inverse() * (res.H_free.transpose().inverse()*subvec_w_ind(grad_clamped, res.v_free)) - subvec_w_ind(x, res.v_free));

// TODO find cleaner way
int update_idx = 0;
for (int k=0; k<search.size(); ++k) {
if(res.v_free[k]==1) {
search(k) = search_update_vals(update_idx);
update_idx++;
}
}
}

lineSearchResult linesearch_res = quadclamp_line_search(x, search, Q, c, lower, upper);
if(linesearch_res.failed)
{
if(linesearch_res.failed) {
res.result = 2;
break;
}
Expand All @@ -129,33 +122,15 @@ boxQPResult boxQP(const MatrixXd &Q, const VectorXd &c, const VectorXd &x0,
iter, linesearch_res.v_opt, grad_norm, oldvalue-linesearch_res.v_opt, stepDec, linesearch_res.n_steps, int(clamped_dims.sum()));
#endif

// accept candidate
x = linesearch_res.x_opt;
val = linesearch_res.v_opt;
// accept candidate
x = linesearch_res.x_opt;
val = linesearch_res.v_opt;
}

res.x_opt = x;
return res;

} //boxQP


double quadCost(const MatrixXd& Q, const VectorXd& c, const VectorXd& x)
{
return 0.5*x.transpose()*Q*x + x.dot(c);
}

VectorXd clamp_to_limits(const VectorXd &x, const VectorXd& lower, const VectorXd& upper)
{
VectorXd x_clamped(x.size());
for(int i=0; i<x.size(); i++)
{
x_clamped(i) = std::min(upper(i), std::max(lower(i), x(i)));
}
// VectorXd x_clamped = upper.cwiseMin(x.cwiseMax(lower));
return x_clamped;
}

// Armijo line search: for quadratic cost function with limits on x
// Find a step size in the given search direction that leads to at least the expected decrease in value
lineSearchResult quadclamp_line_search(const VectorXd& x0, const VectorXd& search_dir,
Expand All @@ -167,8 +142,7 @@ lineSearchResult quadclamp_line_search(const VectorXd& x0, const VectorXd& searc

VectorXd grad = Q*x0 + c;
double local_slope = search_dir.dot(grad);
if(local_slope >= 0) // check if search direction isn't descent direction - this shoudn't happen
{
if(local_slope >= 0) { // check if search direction isn't descent direction - this shoudn't happen
res.failed = true;
return res;
}
Expand All @@ -178,17 +152,15 @@ lineSearchResult quadclamp_line_search(const VectorXd& x0, const VectorXd& searc
double v = quadCost(Q, c, x_clamped);
double old_v = quadCost(Q, c, x0);

while ((v - old_v)/(step*local_slope) < Armijo)
{
while ((v - old_v)/(step*local_slope) < Armijo) {
step *= stepDec;
res.n_steps++;

x_reach = x0 + step*search_dir;
x_clamped = clamp_to_limits(x_reach, lower, upper);
v = quadCost(Q, c, x_clamped);

if (step < minStep)
{
if (step < minStep) {
res.failed = true;
break;
}
Expand Down

0 comments on commit 398a7ea

Please sign in to comment.