[solver] added support for equality constraints using combined recipi… #1

qlin960618 · 2023-07-25T01:55:25Z

@dqrobotics/developers

Hi @mmmarinho,

This PR adds the equality constraints support to the cpp-interface-qpoases, which implimented the following method

void set_equality_constraints_tolerance(const double& equality_constraints_tolerance)
double get_equality_constraints_tolerance()
The default value of which is initialized with DQ_threshold

Best regards,
Quentin Lin

TODO

I notice on the documentation of the qpoases, there is support for lbA and ubA, maybe can move the implimentation there for more effieicent implimentation. (rather then the current make-shift solution)

…cal inequality

mmmarinho · 2023-10-29T23:00:14Z

@qlin960618 We've talked about this some months ago, but I think we haven't reached a conclusion.

Could you add a minimal example to https://github.com/dqrobotics/cpp-examples?

I doesn't even need to depend on a robot, it could be an example solving an optimization problem with equality constraints that is somewhat easy to verify manually.

qlin960618 · 2023-10-30T03:28:28Z

@mmmarinho. Sorry, I kind of forgot since I have just been using a workaround on my side. I will try to do so asap....

better implimentation using lbA

mmmarinho · 2023-10-31T14:15:32Z

@qlin960618 Thanks!

I’ve looked through the file and there are too many style modifications.

Please keep the original style of the file and alter just the parts strictly needed for your proposed additions.

qlin960618 · 2023-10-31T14:41:25Z

@mmmarinho ,
Sorry for the autoformatting on my side...
I have just clean up the code + check on the example pass too.

mmmarinho · 2023-11-02T00:15:02Z

@qlin960618 Thanks, this looks better now.
We'll wait on dqrobotics/cpp-examples#18.

juanjqo · 2023-11-03T09:02:43Z

Hi @qlin960618, the example qpoases_interface.cpp is not running with the version you are proposing.

Example:

#include <iostream>

#include <dqrobotics/DQ.h>
#include <dqrobotics/robots/KukaLw4Robot.h>
#include <dqrobotics/solvers/DQ_QPOASESSolver.h>
#include <dqrobotics/robot_control/DQ_ClassicQPController.h>
#include <dqrobotics/utils/DQ_Constants.h>

using namespace DQ_robotics;

int main(void)
{
    DQ_QPOASESSolver qpoases_solver;
    DQ_SerialManipulatorDH robot = KukaLw4Robot::kinematics();

    DQ_ClassicQPController classic_qp_controller(&robot,&qpoases_solver);
    classic_qp_controller.set_control_objective(ControlObjective::Pose);
    classic_qp_controller.set_gain(0.01);
    classic_qp_controller.set_damping(0.01);

    VectorXd theta_init(7);
    theta_init << 0.,pi/4.,0.,0.,pi/4.,0.,0.;

    VectorXd theta_xd(7);
    theta_xd << 0.,pi/2.,0.,0.,pi/2.,0.,0.;
    DQ xd = robot.fkm(theta_xd);

    VectorXd qd = classic_qp_controller.compute_setpoint_control_signal(theta_init,vec8(xd));

    std::cout << "q_dot is " << qd.transpose() << std::endl;

    return 0;
}

Expected output (this is the current output with the current version of DQ Robotics cpp-interface-qpoases-master):

q_dot is  -0.00311208   0.00617398   0.00304193   -0.0015048   0.00304193 -0.000791966   0.00304193

Output using the current version of qlin960618/cpp-interface-qpoases-master (your PR):

qpoases_interface: /usr/include/eigen3/Eigen/src/Core/CommaInitializer.h:122: XprType& 

Eigen::CommaInitializer<MatrixType>::finished() [with XprType = Eigen::Matrix<double, -1, -1>]: Assertion 

`((m_row+m_currentBlockRows) == m_xpr.rows() || m_xpr.cols() == 0) && m_col == m_xpr.cols() && "Too few coefficients

passed to comma initializer (operator<<)"' failed.

Please let me know if you can replicate my results. If yes, please make the corresponding modifications to fix the bug.

Best regards,

Juan

qlin960618 · 2023-11-04T04:34:30Z

Hi @juanjqo ,
Thanks for pointing out the problem to me! I will make sure to correct for the commit message in my next commit.
However, I have tried building the interface and test again as it on a clean VM, which doesnt seem to be able to replicate your result.... The test you attached above is the current master branch test correct?

Something I have to do I am not sure if its related. to get past the compile of the test, i have to build the dqrobotics base library from source instead of using the ppa:dqrobotics-dev. Below is the error that I was getting.
But from looking at your error... its more related to eigen3?? I am still not sure where its comming from?

Best,
Quentin

…limentation

qlin960618 · 2023-11-06T02:08:36Z

Hi @juanjqo,
Thanks for your instruction on how to replicate the error. I think the correction should fix the issue for both old and newly proposed test. I have also revert the equality constraints back to the original proposed. As I was having some issue with what I proposed on my side that is not present in the test case.

Best,
Quentin

juanjqo · 2023-11-06T04:43:08Z

Hi @qlin960618, thanks. After your modifications in #[2b94cf0] (2b94cf0), the example is running as expected.

To keep the consistency between other classes and to follow the DQ Robotics policies, please implement the following modifications:

Update the credit attributions in the header. Similar to what you did in PR-18. For instance, it could be something like:

 /**
Contributors:
- Murilo M. Marinho ([email protected])
    - Responsible for the original implementation.
- Quentin Lin ([email protected])
    - Added support for equality constraints
 */

Furthermore, this is a good opportunity to update the copyright year from

(C) Copyright 2022 DQ Robotics Developers

to:

(C) Copyright 2022-2023 DQ Robotics Developers

Add the respective documentation for the new methods you are proposing (i.e., set_equality_constraints_tolerance and get_equality_constraints_tolerance)

Use the same style used in other methods. For instance, this is the documentation of the solve_quadratic_program method:

  /**
   * @brief
   *   Solves the following quadratic program
   *   min(x)  0.5*x'Hx + f'x
   *   s.t.    Ax <= b
   *           Aeqx = beq.
   * Method signature is compatible with MATLAB's 'quadprog'.
   * @param H the n x n matrix of the quadratic coefficients of the decision variables.
   * @param f the n x 1 vector of the linear coefficients of the decision variables.
   * @param A the m x n matrix of inequality constraints.
   * @param b the m x 1 value for the inequality constraints.
   * @param Aeq the m x n matrix of equality constraints.
   * @param beq the m x 1 value for the inequality constraints.
   * @return the optimal x
   */
   VectorXd solve_quadratic_program(const MatrixXd& H, const VectorXd& f, const MatrixXd& A, const VectorXd& b, const MatrixXd& Aeq, const VectorXd& beq) override

Best regards,

Juan

…ditional methods

qlin960618 · 2023-11-06T05:19:33Z

Hi @juanjqo,
The documentaiton is now updated.

Thanks!
Quentin

juanjqo · 2023-11-06T06:44:56Z

Hi @qlin960618, thank you for your effort. There is still a bug (a template runtime error) with your PR when only equality constraints are used.

Consider the following minimal example:

#include <memory>
#include <dqrobotics/solvers/DQ_QPOASESSolver.h>

using namespace Eigen;
using namespace DQ_robotics;

int main(){
    auto solver = std::make_shared<DQ_QPOASESSolver>();

    MatrixXd H = MatrixXd::Zero(3,3);
    H << 1,-1, 1,
        -1, 2,-2,
        1,-2, 4;

    VectorXd f = VectorXd::Zero(3);
    f << 2,-3, 1;

    MatrixXd Aeq = MatrixXd::Ones(1,3);
    VectorXd beq = VectorXd::Ones(1);
    beq(0) = 0.5;

    auto u = solver->solve_quadratic_program(H, f, MatrixXd(), VectorXd(), Aeq, beq);
    std::cout<<"u: "<<u.transpose()<<std::endl;
}

Output (your PR):

test: /usr/include/eigen3/Eigen/src/Core/CommaInitializer.h:97: Eigen::CommaInitializer<MatrixType>& 

Eigen::CommaInitializer<MatrixType>::operator,(const Eigen::DenseBase<OtherDerived>&) [with OtherDerived = 

Eigen::Matrix<double, -1, -1>; XprType = Eigen::Matrix<double, -1, -1>]: Assertion `m_currentBlockRows==other.rows()' 

failed.

Expected behaviour

Example using Matlab

clear all
close all
clc


H = [1,-1,1
    -1,2,-2
    1,-2,4];
f = [2;-3;1];
Aeq = ones(1,3);
beq = 1/2;

ub = quadprog(H,f,[],[],Aeq,beq,[],[])

Matlab output:

Minimum found that satisfies the constraints.

Optimization completed because the objective function is non-decreasing in 
feasible directions, to within the value of the optimality tolerance,
and constraints are satisfied to within the value of the constraint tolerance.

<stopping criteria details>

ub =

   -1.6429
    1.3571
    0.7857

Example using Proxqp with cpp-interface-proxqp

#include <memory>
#include <dqrobotics/solvers/DQ_PROXQPSolver.h>

using namespace Eigen;
using namespace DQ_robotics;

int main(){
    auto solver = std::make_shared<DQ_PROXQPSolver>();

    MatrixXd H = MatrixXd::Zero(3,3);
    H << 1,-1, 1,
        -1, 2,-2,
        1,-2, 4;

    VectorXd f = VectorXd::Zero(3);
    f << 2,-3, 1;

    MatrixXd Aeq = MatrixXd::Ones(1,3);
    VectorXd beq = VectorXd::Ones(1);
    beq(0) = 0.5;


    auto u = solver->solve_quadratic_program(H, f, MatrixXd(), VectorXd(), Aeq, beq);
    std::cout<<"u: "<<u.transpose()<<std::endl;
}

Output:

u: -1.64286  1.35714 0.785714

Please implement the required modifications to fix the bug.

Best regards,

Juan

…lity is none

qlin960618 · 2023-11-06T07:28:29Z

Hi @juanjqo ,

Thanks for pointing out the case, I have added tighter check for where there is no inequality. Test pass for the example code you give as well.

Best,
Quentin

juanjqo · 2023-11-06T08:19:44Z

@qlin960618 thanks. I'm going to review the example you are proposing.

mmmarinho · 2023-12-08T17:39:39Z

@qlin960618 @juanjqo
The example
dqrobotics/cpp-examples#18
has been merged. Please move on with this pull request and let me know when you need my review.

juanjqo · 2023-12-11T03:27:04Z

include/dqrobotics/solvers/DQ_QPOASESSolver.h

@@ -43,6 +46,9 @@ namespace DQ_robotics
        //Page 14 of https://www.coin-or.org/qpOASES/doc/3.0/manual.pdf
        int_t maximum_working_set_recalculations_;

+        //implimentation of the equality constraints is done via appending it and recipical of it to the inequalities


@qlin960618 Please fix the typos and add some brief details about the tolerance variable. For instance:

// Equality constraints are handled by means of appended inequality constraints within an equality tolerance threshold.

@juanjqo , I see. please let me know if this is sufficient!

juanjqo · 2023-12-12T03:45:09Z

include/dqrobotics/solvers/DQ_QPOASESSolver.h

@@ -46,7 +46,8 @@ namespace DQ_robotics
        //Page 14 of https://www.coin-or.org/qpOASES/doc/3.0/manual.pdf
        int_t maximum_working_set_recalculations_;

-        //implimentation of the equality constraints is done via appending it and recipical of it to the inequalities
+        // Equality constraints are handled by means of appended inequality constraints within an equality tolerance threshold.  
+        // initialized to default of DQ_threshold


@qlin960618 thanks, almost there! Please replace "initialized to default of DQ_threshold" by "Initialized to DQ_threshold by default."

juanjqo · 2023-12-12T08:44:47Z

@mmmarinho This PR is ready for your review.

PS: I performed some basic tests and it looks like now is working well.

test

#include <memory>
#include <dqrobotics/solvers/DQ_QPOASESSolver.h>
#include <dqrobotics/solvers/DQ_PROXQPSolver.h>  //https://github.com/juanjqo/cpp-interface-proxqp

using namespace Eigen;
using namespace DQ_robotics;

void show_results(const std::string& msg, const VectorXd& u_qpoases, const VectorXd& u_proxqp);

int main(){

    // This constructor did not work  :(
    //std::vector<std::shared_ptr<DQ_QPOASESSolver>>  qpoases_solvers(4, std::make_shared<DQ_QPOASESSolver>());
    // Therefore, I used the following:
    std::vector<std::shared_ptr<DQ_QPOASESSolver>>  qpoases_solvers{std::make_shared<DQ_QPOASESSolver>(),std::make_shared<DQ_QPOASESSolver>(),
                                                                    std::make_shared<DQ_QPOASESSolver>(),std::make_shared<DQ_QPOASESSolver>()};

    std::vector<std::shared_ptr<DQ_PROXQPSolver>>  proxqp_solvers{std::make_shared<DQ_PROXQPSolver>(), std::make_shared<DQ_PROXQPSolver>(),
                                                                  std::make_shared<DQ_PROXQPSolver>(), std::make_shared<DQ_PROXQPSolver>()};

    for (auto solver : proxqp_solvers)
    {
        solver->set_absolute_stopping_criterion(DQ_threshold);
    }

    // This example was taken from https://www.mathworks.com/help/optim/ug/quadprog.html
    const MatrixXd H   = (MatrixXd(3,3) << 1,-1, 1,-1, 2,-2, 1,-2, 4).finished();
    const VectorXd f   = (VectorXd(3) <<2,-3, 1).finished();
    const MatrixXd Aeq = MatrixXd::Ones(1,3);
    const VectorXd beq = (VectorXd(1)<< 0.5).finished();
    const VectorXd b   = (VectorXd(6) << VectorXd::Ones(3), VectorXd::Zero(3)).finished();
    const MatrixXd A   = (MatrixXd(6,3) << MatrixXd::Identity(3,3), -MatrixXd::Identity(3,3)).finished();
    VectorXd u_qpoases;
    VectorXd u_proxqp;


    // Test with both equalities and inequalities
    u_qpoases = qpoases_solvers.at(0)->solve_quadratic_program(H, f, A, b, Aeq, beq);
    u_proxqp  =  proxqp_solvers.at(0)->solve_quadratic_program(H, f, A, b, Aeq, beq);
    show_results("Test with both equalities and inequalities. ", u_qpoases, u_proxqp);

    // Test with equality constraints only
    u_qpoases = qpoases_solvers.at(1)->solve_quadratic_program(H, f, MatrixXd(), VectorXd(), Aeq, beq);
    u_proxqp  =  proxqp_solvers.at(1)->solve_quadratic_program(H, f, MatrixXd(), VectorXd(), Aeq, beq);
    show_results("Test with equality constraints only. ", u_qpoases, u_proxqp);


    // Unconstrained case
    u_qpoases = qpoases_solvers.at(2)->solve_quadratic_program(H, f, MatrixXd(), VectorXd(), MatrixXd(), VectorXd());
    u_proxqp  =  proxqp_solvers.at(2)->solve_quadratic_program(H, f, MatrixXd(), VectorXd(), MatrixXd(), VectorXd());
    show_results("Unconstrained case. ", u_qpoases, u_proxqp);

    // Test with inequality constraints only
    u_qpoases = qpoases_solvers.at(3)->solve_quadratic_program(H, f, A, b, MatrixXd(), VectorXd());
    u_proxqp  =  proxqp_solvers.at(3)->solve_quadratic_program(H, f, A, b, MatrixXd(), VectorXd());
    show_results("Test with inequality constraints only. ", u_qpoases, u_proxqp);

}

void show_results(const std::string& msg, const VectorXd& u_qpoases, const VectorXd& u_proxqp)
{
    std::cout<<msg<<std::endl;
    std::cout<<"u_qpoases: "<<u_qpoases.transpose()<<std::endl;
    std::cout<<"u_proxqp:  "<< u_proxqp.transpose()<<std::endl;
    std::cout<<"Error norm: "<<(u_qpoases-u_proxqp).norm()<<std::endl;
    std::cout<<" "<<std::endl;
}

Output:

Test with both equalities and inequalities. 
u_qpoases: 4.16334e-17         0.5           0
u_proxqp:  7.57766e-17         0.5 2.59191e-16
Error norm: 1.00014e-12
 
Test with equality constraints only. 
u_qpoases: -1.64286  1.35714 0.785714
u_proxqp:  -1.64286  1.35714 0.785714
Error norm: 6.17365e-13
 
Unconstrained case. 
u_qpoases: -1  2  1
u_proxqp:  -1  2  1
Error norm: 5.47807e-14
 
Test with inequality constraints only. 
u_qpoases:    0    1 0.25
u_proxqp:  2.38185e-16           1        0.25
Error norm: 1.58806e-15

qlin960618 added 2 commits July 25, 2023 10:47

[solver] added support for equality constraints using combined recipi…

6d42fda

…cal inequality

[solver] fix function name typo

039f030

bvadorno requested a review from mmmarinho October 29, 2023 17:42

bvadorno assigned mmmarinho Oct 29, 2023

qlin960618 added 2 commits October 30, 2023 01:26

[qpoases interface] minor bug fix on equality_constraints implimentation

8b2039c

[DQ_QPOASESSolver.h] clean up comment

5009391

qlin960618 mentioned this pull request Oct 30, 2023

[qpoasse_interface test] PR added check for equality constraints dqrobotics/cpp-examples#18

Merged

qlin960618 and others added 2 commits October 31, 2023 09:05

new implimentation need testing

dec4c76

Merge pull request #1 from qlin960618/native_implimentation

db58cf7

better implimentation using lbA

qlin960618 added 2 commits October 31, 2023 23:34

[QPOASES Interface] code style cleanup

d9d3571

[QPOASES Interface] code style cleanup

eeb4e54

[DQ_QPOASESSolver.h] fix issue with eigen3 and revert back to old imp…

2b94cf0

…limentation

qlin960618 added 2 commits November 6, 2023 13:46

[DQ_QPOASESSolver.h] update contributors and documentation for the ad…

ef26ad0

…ditional methods

[DQ_QPOASESSolver.h] update contributors and documentation for the ad…

c46d51a

…ditional methods

[DQ_QPOASESSolver.h] additional check to fix bug in case where inequa…

2032f22

…lity is none

mmmarinho assigned juanjqo and qlin960618 Dec 8, 2023

juanjqo reviewed Dec 11, 2023

View reviewed changes

[DQ_QPOASESSolver.h] update comment + brife for equality constraints

4ce4c89

juanjqo suggested changes Dec 12, 2023

View reviewed changes

[DQ_QPOASESSolver.h] fix comment

7a4eb9b

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[solver] added support for equality constraints using combined recipi… #1

[solver] added support for equality constraints using combined recipi… #1

qlin960618 commented Jul 25, 2023

mmmarinho commented Oct 29, 2023

qlin960618 commented Oct 30, 2023

mmmarinho commented Oct 31, 2023

qlin960618 commented Oct 31, 2023

mmmarinho commented Nov 2, 2023

juanjqo commented Nov 3, 2023 •

edited

Loading

qlin960618 commented Nov 4, 2023 •

edited

Loading

qlin960618 commented Nov 6, 2023

juanjqo commented Nov 6, 2023

qlin960618 commented Nov 6, 2023

juanjqo commented Nov 6, 2023 •

edited

Loading

qlin960618 commented Nov 6, 2023

juanjqo commented Nov 6, 2023

mmmarinho commented Dec 8, 2023

juanjqo Dec 11, 2023

qlin960618 Dec 12, 2023

juanjqo Dec 12, 2023

juanjqo commented Dec 12, 2023

[solver] added support for equality constraints using combined recipi… #1

Are you sure you want to change the base?

[solver] added support for equality constraints using combined recipi… #1

Conversation

qlin960618 commented Jul 25, 2023

mmmarinho commented Oct 29, 2023

qlin960618 commented Oct 30, 2023

mmmarinho commented Oct 31, 2023

qlin960618 commented Oct 31, 2023

mmmarinho commented Nov 2, 2023

juanjqo commented Nov 3, 2023 • edited Loading

qlin960618 commented Nov 4, 2023 • edited Loading

qlin960618 commented Nov 6, 2023

juanjqo commented Nov 6, 2023

qlin960618 commented Nov 6, 2023

juanjqo commented Nov 6, 2023 • edited Loading

Expected behaviour

qlin960618 commented Nov 6, 2023

juanjqo commented Nov 6, 2023

mmmarinho commented Dec 8, 2023

juanjqo Dec 11, 2023

Choose a reason for hiding this comment

qlin960618 Dec 12, 2023

Choose a reason for hiding this comment

juanjqo Dec 12, 2023

Choose a reason for hiding this comment

juanjqo commented Dec 12, 2023

test

Output:

juanjqo commented Nov 3, 2023 •

edited

Loading

qlin960618 commented Nov 4, 2023 •

edited

Loading

juanjqo commented Nov 6, 2023 •

edited

Loading