convexity of the quadratic cost

[jhu-gh]

Hello all,

I'm trying to implement the MILP formulation mentioned in: Global Inverse Kinematics via Mixed-integer Convex Optimization by Hongkai Dai, Gregory Izatt and Russ Tedrake, ISRR, 2017. Sometimes when I use different data points, the convexity of the cost function changes, and throws error saying "There is no available solver for the optimization program". The following is code to reproduce the issue:

#include "drake/solvers/mixed_integer_rotation_constraint.h"
#include <random>
#include <gtest/gtest.h>
#include "drake/common/symbolic.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/math/gray_code.h"
#include "drake/math/random_rotation.h"
#include "drake/math/rotation_matrix.h"
#include "drake/solvers/gurobi_solver.h"
#include "drake/solvers/integer_optimization_util.h"
#include "drake/solvers/mathematical_program.h"
#include "drake/solvers/mosek_solver.h"
#include "drake/solvers/rotation_constraint.h"
#include "drake/solvers/solve.h"
#include <stdlib.h>
void genData(std::vector<Eigen::Vector3d> &source_pts, std::vector<Eigen::Vector3d> &data_pts)
{
    std::mt19937 generator(22);
    //auto R_test = drake::math::UniformlyRandomRotationMatrix(&generator);
    //auto trans = Eigen::Vector3d::Random();
    Eigen::Matrix3d R_test = Eigen::MatrixXd::Identity(3,3);
    R_test = drake::math::RotationMatrixd::MakeZRotation(M_PI_4) * R_test;
    Eigen::Vector3d trans(0.1,0.2,0.2);//cost is convex
    //Eigen::Vector3d trans(0.05,0.06,0.07);//cost is nonconvex
    std::cout<<"true R:"<<std::endl;
    std::cout<<R_test.matrix()<<std::endl;
    std::cout<<"true T:"<<std::endl;
    std::cout<<trans<<std::endl;
    for(unsigned int i=0; i<source_pts.size(); i++)
    {
        data_pts[i] = R_test*source_pts[i] + trans;
    }
}
int main(int argc, char** argv) {
    std::cout<<"args: "<<argc<<", "<<argv[0]<<std::endl;
    auto mosek_license = drake::solvers::MosekSolver::AcquireLicense();
    std::vector<Eigen::Vector3d> source_pts;//s_i in the paper
    std::vector<Eigen::Vector3d> data_pts;//m_j in the paper
    Eigen::Vector3d v1(1.0,0.0,0.0);
    Eigen::Vector3d v2(0.0,1.0,0.0);
    Eigen::Vector3d v3(0.0,0.0,1.0);
    Eigen::Vector3d v4(0.1,-0.1,0.15);
    source_pts.push_back(v1);
    source_pts.push_back(v2);
    source_pts.push_back(v3);
    source_pts.push_back(v4);
    data_pts.resize(source_pts.size());
    genData(source_pts, data_pts);
    std::cout<<"source data: "<<std::endl;
    for (unsigned int i = 0; i < source_pts.size(); i++){
        std::cout<<source_pts[i].transpose()<<std::endl;
    }
    std::cout<<"transformed data: "<<std::endl;
    for (unsigned int j = 0; j < data_pts.size(); j++){
        std::cout<<data_pts[j].transpose()<<std::endl;
    }
    //create rotation constraint
    drake::solvers::MixedIntegerRotationConstraintGenerator::Approach approach = drake::solvers::MixedIntegerRotationConstraintGenerator::Approach::kBoxSphereIntersection;//kBoth;
    int num_intervals_per_half_axis = 4;//6;
    drake::solvers::IntervalBinning ib = drake::solvers::IntervalBinning::kLogarithmic;//kLinear;
    drake::solvers::MixedIntegerRotationConstraintGenerator mircg(approach, num_intervals_per_half_axis, ib);
    drake::solvers::MathematicalProgram mp; 
    auto trans_var = mp.NewContinuousVariables(3, "trans");//translation vector
    auto C_mat = mp.NewBinaryVariables(source_pts.size(), data_pts.size(), "C");//selection matrix, C in the paper
    auto Rot = mp.NewContinuousVariables(3, 3, "Rot");//rotation matrix
    mircg.AddToProgram(Rot, &mp);
    drake::symbolic::Expression e;//cost
    std::vector<Eigen::Matrix<drake::symbolic::Expression, 3, 1>> temp_vec_all;//each element (MC_i^T in the paper) is a corresp vector for s_i
    temp_vec_all.resize(source_pts.size());
    //cost function
    for (unsigned int i = 0; i < source_pts.size(); i++){
        for (unsigned int j = 0; j < data_pts.size(); j++){
            temp_vec_all[i] += C_mat(i,j)*data_pts[j];
        }
        e += (Rot*source_pts[i] + trans_var - temp_vec_all[i]).squaredNorm();
    }
    std::cout<<"cost expression uninterpreted function? "<<drake::symbolic::is_uninterpreted_function(e)<<std::endl;
    std::cout<<"cost expression polynomial? "<<e.is_polynomial()<<std::endl;
    mp.AddQuadraticCost(e);
    std::cout<<"cost convex? "<<mp.quadratic_costs().back().evaluator()->is_convex()<<std::endl;
    std::cout<<"cost description: "<<mp.quadratic_costs().back().evaluator()->get_description()<<std::endl;
    //constraint for C matrix
    std::vector<drake::symbolic::Expression> C_row_all;//constraint for each row 
    C_row_all.resize(source_pts.size());
    std::vector<drake::symbolic::Expression> C_col_all;//constraint for each col
    C_col_all.resize(data_pts.size());
    for (unsigned int i = 0; i < source_pts.size(); i++){
        for (unsigned int j = 0; j < data_pts.size(); j++){
            C_row_all[i] += C_mat(i,j);
        }
        mp.AddConstraint(C_row_all[i], 1,1);
    }
    for (unsigned int j = 0; j < data_pts.size(); j++){
        for (unsigned int i = 0; i < source_pts.size(); i++){
            C_col_all[j] += C_mat(i,j);
        }
        mp.AddConstraint(C_col_all[j], 0, 1);
    }
    const drake::solvers::MathematicalProgramResult result = drake::solvers::Solve(mp);
    std::cout<<"result: "<<result.is_success()<<std::endl;
    std::cout<<"solver is: "<<result.get_solver_id().name()<<std::endl;
    //std::cout<<"solver details: "<<result.get_solver_details<drake::solvers::MosekSolver>().rescode<<std::endl;
    std::cout<<"R: "<<result.GetSolution(Rot)<<std::endl;
    std::cout<<"T: "<<result.GetSolution(trans_var)<<std::endl;
    std::cout<<"C: "<<result.GetSolution(C_mat)<<std::endl;
    return 0;
}

one translation vector in genData() gives convex cost while the other doesn't. I'm not sure if it's because I'm not defining the cost correctly.

On a side note, is_convex() returns true for the quadratic function mentioned here, not sure if they're related.

Please let me know if this is not the right place to ask this question. Thanks.

[hongkai-dai]

When the solver complains that "There is no available solver for the optimization program", the line

std::cout<<"cost convex? "<<mp.quadratic_costs().back().evaluator()->is_convex()<<std::endl;

returns False.

But your cost is actually convex (as it is the sum of squared norm). So when Drake checks the convexity of the cost, it returns the wrong result. The easy fix is to change the line

mp.AddQuadraticCost(e);

to

mp.AddQuadraticCost(e, true);

where the flag true explicitly tells Drake that the cost is convex, and then Drake doesn't check its convexity.

The deeper reason why Drake thinks the cost is non-convex, while the cost is actually convex, is that Drake uses Eigen's LDLT solver to do a Cholesky decomposition. Apparently this Cholesky decomposition by Eigen is inaccurate for your problem.

[SeanCurtis-TRI]

@jhu-gh This was a great question. We'd strongly encourage you to post this kind of thing in stackoverflow with the drake tag. In fact, it's probably worth taking your original post and adding it there specifically. More users will directly benefit from your question and @hongkai-dai's answer.

I'm going to close this issue as "solved" (based on @hongkai-dai's response). If you want to take the conversation further, I recommend you open the stackoverflow issue (and link to it here).