From 7d974c591dfd73258b22512e15e1571ae3fceaf8 Mon Sep 17 00:00:00 2001 From: Quentin Lin Date: Mon, 30 Oct 2023 01:29:47 -0700 Subject: [PATCH 1/5] [qpoases interface test] added test for equality constraints --- cmake/qpoases_interface/qpoases_interface.cpp | 73 +++++++++++++++++-- 1 file changed, 67 insertions(+), 6 deletions(-) diff --git a/cmake/qpoases_interface/qpoases_interface.cpp b/cmake/qpoases_interface/qpoases_interface.cpp index 38016ad..ca7895d 100644 --- a/cmake/qpoases_interface/qpoases_interface.cpp +++ b/cmake/qpoases_interface/qpoases_interface.cpp @@ -30,26 +30,87 @@ This file is part of DQ Robotics. using namespace DQ_robotics; -int main(void) -{ +void test_solver(void) { DQ_QPOASESSolver qpoases_solver; DQ_SerialManipulatorDH robot = KukaLw4Robot::kinematics(); - DQ_ClassicQPController classic_qp_controller(&robot,&qpoases_solver); + 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.; + 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.; + 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)); + VectorXd qd = classic_qp_controller.compute_setpoint_control_signal(theta_init, vec8(xd)); std::cout << "q_dot is " << qd.transpose() << std::endl; +} + + +bool are_vectors_equal(const Eigen::VectorXd &A, const Eigen::VectorXd &B, double tolerance = 1e-3) { + if (A.size() != B.size()) { + return false; // Vectors have different dimensions + } + + for (int i = 0; i < A.size(); ++i) { + if (std::abs(A(i) - B(i)) > tolerance) { + std::cout << "CHECK::index: " << i << " exceed error: " << std::abs(A(i) - B(i)) << std::endl; + return false; // Elements at (i) differ by more than the tolerance + } + } + + return true; +} + +void test_equality(void) { + DQ_QPOASESSolver qpoases_solver; + auto tolerance = qpoases_solver.get_equality_constraints_tolerance(); +// tolerance = 0.001; + std::cout << "current equality tolerance: " << tolerance << std::endl; + + for (int problem_size = 5; problem_size <= 10; problem_size += 1) { + qpoases_solver = DQ_QPOASESSolver(); + auto tolerance = qpoases_solver.get_equality_constraints_tolerance(); + qpoases_solver.set_equality_constraints_tolerance(tolerance); + + MatrixXd H = MatrixXd::Identity(problem_size, problem_size); + VectorXd f = VectorXd::Ones(problem_size); + + MatrixXd A = MatrixXd::Zero(1, problem_size); + VectorXd b = VectorXd::Zero(1); + std::cout << "Checking Problem Size: " << problem_size << std::endl; + + for (int iter = 0; iter < 10; iter += 1) { + VectorXd mlp = VectorXd::Random(problem_size); + MatrixXd Aeq = mlp.asDiagonal(); + VectorXd beq = VectorXd::Random(problem_size); + auto out = qpoases_solver.solve_quadratic_program(H, f, A, b, Aeq, beq); + if (!are_vectors_equal(beq, Aeq * out, tolerance+DQ_threshold)) { + std::cout << "Solver Failed equality check on mlp: " << mlp.transpose() << + std::endl << "beq: " << beq.transpose() << + std::endl << "Solution: " << out.transpose() << + std::endl << "check " << (Aeq * out).transpose() << std::endl; + throw std::runtime_error("solver equality check failed"); + } + + } + + } + + +} + + +int main(void) { + test_solver(); + test_equality(); return 0; } + + From d3ef31bee455b519bfbcd65649d528e5110ae9df Mon Sep 17 00:00:00 2001 From: Quentin Lin Date: Mon, 30 Oct 2023 03:28:45 -0700 Subject: [PATCH 2/5] [qpoases interface test] added comments and minor bugfix --- cmake/qpoases_interface/qpoases_interface.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cmake/qpoases_interface/qpoases_interface.cpp b/cmake/qpoases_interface/qpoases_interface.cpp index ca7895d..966b22a 100644 --- a/cmake/qpoases_interface/qpoases_interface.cpp +++ b/cmake/qpoases_interface/qpoases_interface.cpp @@ -75,7 +75,7 @@ void test_equality(void) { for (int problem_size = 5; problem_size <= 10; problem_size += 1) { qpoases_solver = DQ_QPOASESSolver(); - auto tolerance = qpoases_solver.get_equality_constraints_tolerance(); + tolerance = qpoases_solver.get_equality_constraints_tolerance(); qpoases_solver.set_equality_constraints_tolerance(tolerance); MatrixXd H = MatrixXd::Identity(problem_size, problem_size); @@ -90,6 +90,7 @@ void test_equality(void) { MatrixXd Aeq = mlp.asDiagonal(); VectorXd beq = VectorXd::Random(problem_size); auto out = qpoases_solver.solve_quadratic_program(H, f, A, b, Aeq, beq); + // just plain tolerance is too tight for the check if (!are_vectors_equal(beq, Aeq * out, tolerance+DQ_threshold)) { std::cout << "Solver Failed equality check on mlp: " << mlp.transpose() << std::endl << "beq: " << beq.transpose() << From 4055d363aed26d21907227a90885c60cfa28eeeb Mon Sep 17 00:00:00 2001 From: Quentin Lin Date: Mon, 30 Oct 2023 03:37:46 -0700 Subject: [PATCH 3/5] [qpoases interface test] added comment --- cmake/qpoases_interface/qpoases_interface.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/cmake/qpoases_interface/qpoases_interface.cpp b/cmake/qpoases_interface/qpoases_interface.cpp index 966b22a..6a770da 100644 --- a/cmake/qpoases_interface/qpoases_interface.cpp +++ b/cmake/qpoases_interface/qpoases_interface.cpp @@ -18,6 +18,9 @@ This file is part of DQ Robotics. Contributors: - Murilo M. Marinho (murilo@g.ecc.u-tokyo.ac.jp) + - initial implementation +- Quentin Lin (qlin1806@g.ecc.u-tokyo.ac.jp) + - added check for equality constraints */ #include From de816df44accc890123dd7231ebd38c2a9b14031 Mon Sep 17 00:00:00 2001 From: qlin960618 Date: Wed, 8 Nov 2023 22:17:58 +0900 Subject: [PATCH 4/5] [qpoases_interface.cpp] style optimization for equality check --- cmake/qpoases_interface/qpoases_interface.cpp | 60 ++++++++++--------- 1 file changed, 31 insertions(+), 29 deletions(-) diff --git a/cmake/qpoases_interface/qpoases_interface.cpp b/cmake/qpoases_interface/qpoases_interface.cpp index 6a770da..3b319f3 100644 --- a/cmake/qpoases_interface/qpoases_interface.cpp +++ b/cmake/qpoases_interface/qpoases_interface.cpp @@ -1,5 +1,5 @@ /** -(C) Copyright 2022 DQ Robotics Developers +(C) Copyright 2022-2023 DQ Robotics Developers This file is part of DQ Robotics. @@ -33,80 +33,82 @@ This file is part of DQ Robotics. using namespace DQ_robotics; +/** + * @brief Test solver + */ void test_solver(void) { DQ_QPOASESSolver qpoases_solver; DQ_SerialManipulatorDH robot = KukaLw4Robot::kinematics(); - DQ_ClassicQPController classic_qp_controller(&robot, &qpoases_solver); + 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.; + 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.; + 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)); + VectorXd qd = classic_qp_controller.compute_setpoint_control_signal(theta_init,vec8(xd)); std::cout << "q_dot is " << qd.transpose() << std::endl; } - -bool are_vectors_equal(const Eigen::VectorXd &A, const Eigen::VectorXd &B, double tolerance = 1e-3) { - if (A.size() != B.size()) { +/** + * @brief Check if two vectors are equal within a tolerance + * @param a Vector 1 + * @param b Vector 2 + * @param tolerance default to 1e-3 + * @return true if pass + */ +bool check_if_vectors_equal(const Eigen::VectorXd &a, const Eigen::VectorXd &b, double tolerance = 1e-3) { + if (a.size() != b.size()) { return false; // Vectors have different dimensions } - - for (int i = 0; i < A.size(); ++i) { - if (std::abs(A(i) - B(i)) > tolerance) { - std::cout << "CHECK::index: " << i << " exceed error: " << std::abs(A(i) - B(i)) << std::endl; + for (int i = 0; i < a.size(); ++i) { + if (std::abs(a(i) - b(i)) > tolerance) { + std::cout << "CHECK::index: " << i << " exceed error: " << std::abs(a(i) - b(i)) << std::endl; return false; // Elements at (i) differ by more than the tolerance } } - return true; } +/** + * @brief Test equality constraints + */ void test_equality(void) { DQ_QPOASESSolver qpoases_solver; auto tolerance = qpoases_solver.get_equality_constraints_tolerance(); -// tolerance = 0.001; + tolerance = qpoases_solver.get_equality_constraints_tolerance(); std::cout << "current equality tolerance: " << tolerance << std::endl; + qpoases_solver.set_equality_constraints_tolerance(tolerance); for (int problem_size = 5; problem_size <= 10; problem_size += 1) { qpoases_solver = DQ_QPOASESSolver(); - tolerance = qpoases_solver.get_equality_constraints_tolerance(); - qpoases_solver.set_equality_constraints_tolerance(tolerance); MatrixXd H = MatrixXd::Identity(problem_size, problem_size); VectorXd f = VectorXd::Ones(problem_size); - - MatrixXd A = MatrixXd::Zero(1, problem_size); - VectorXd b = VectorXd::Zero(1); std::cout << "Checking Problem Size: " << problem_size << std::endl; - for (int iter = 0; iter < 10; iter += 1) { - VectorXd mlp = VectorXd::Random(problem_size); - MatrixXd Aeq = mlp.asDiagonal(); + srand(iter); + VectorXd a_diag = VectorXd::Random(problem_size); + MatrixXd Aeq = a_diag.asDiagonal(); VectorXd beq = VectorXd::Random(problem_size); - auto out = qpoases_solver.solve_quadratic_program(H, f, A, b, Aeq, beq); + auto out = qpoases_solver.solve_quadratic_program(H, f, MatrixXd(), VectorXd(), Aeq, beq); // just plain tolerance is too tight for the check - if (!are_vectors_equal(beq, Aeq * out, tolerance+DQ_threshold)) { - std::cout << "Solver Failed equality check on mlp: " << mlp.transpose() << + if (!check_if_vectors_equal(beq, Aeq * out, tolerance+DQ_threshold)) { + std::cout << "Solver Failed equality check on a_diag: " << a_diag.transpose() << std::endl << "beq: " << beq.transpose() << std::endl << "Solution: " << out.transpose() << std::endl << "check " << (Aeq * out).transpose() << std::endl; throw std::runtime_error("solver equality check failed"); } - } - } - - } From 068289b29feab9508fb00803b8ebc05430126407 Mon Sep 17 00:00:00 2001 From: qlin960618 <31228437+qlin960618@users.noreply.github.com> Date: Fri, 8 Dec 2023 20:32:34 +0900 Subject: [PATCH 5/5] [qpoases_interface.cpp] remove redundant code --- cmake/qpoases_interface/qpoases_interface.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/cmake/qpoases_interface/qpoases_interface.cpp b/cmake/qpoases_interface/qpoases_interface.cpp index 3b319f3..c906a9f 100644 --- a/cmake/qpoases_interface/qpoases_interface.cpp +++ b/cmake/qpoases_interface/qpoases_interface.cpp @@ -83,7 +83,6 @@ bool check_if_vectors_equal(const Eigen::VectorXd &a, const Eigen::VectorXd &b, void test_equality(void) { DQ_QPOASESSolver qpoases_solver; auto tolerance = qpoases_solver.get_equality_constraints_tolerance(); - tolerance = qpoases_solver.get_equality_constraints_tolerance(); std::cout << "current equality tolerance: " << tolerance << std::endl; qpoases_solver.set_equality_constraints_tolerance(tolerance);