Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_boxqp.cpp	Lines:	97	97	100.0 %
Date:	2024-02-13 11:12:33	Branches:	253	498	50.8 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2022, University of Edinburgh, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API

#include <boost/random.hpp>

#include "crocoddyl/core/solvers/box-qp.hpp"
#include "unittest_common.hpp"

using namespace boost::unit_test;
using namespace crocoddyl::unittest;

void test_constructor() {
  // Setup the test
  std::size_t nx = random_int_in_range(1, 100);
  crocoddyl::BoxQP boxqp(nx);

  // Test dimension of the decision vector
  BOOST_CHECK(boxqp.get_nx() == nx);
}

void test_unconstrained_qp_with_identity_hessian() {
  std::size_t nx = random_int_in_range(2, 5);
  crocoddyl::BoxQP boxqp(nx);
  boxqp.set_reg(0.);

  Eigen::MatrixXd hessian = Eigen::MatrixXd::Identity(nx, nx);
  Eigen::VectorXd gradient = Eigen::VectorXd::Random(nx);
  Eigen::VectorXd lb =
      -std::numeric_limits<double>::infinity() * Eigen::VectorXd::Ones(nx);
  Eigen::VectorXd ub =
      std::numeric_limits<double>::infinity() * Eigen::VectorXd::Ones(nx);
  Eigen::VectorXd xinit = Eigen::VectorXd::Random(nx);
  crocoddyl::BoxQPSolution sol = boxqp.solve(hessian, gradient, lb, ub, xinit);

  // Checking the solution of the problem. Note that it the negative of the
  // gradient since Hessian is identity matrix
  BOOST_CHECK((sol.x + gradient).isZero(1e-9));

  // Checking the solution against a regularized case
  double reg = random_real_in_range(1e-9, 1e2);
  boxqp.set_reg(reg);
  crocoddyl::BoxQPSolution sol_reg =
      boxqp.solve(hessian, gradient, lb, ub, xinit);
  BOOST_CHECK((sol_reg.x + gradient / (1 + reg)).isZero(1e-9));

  // Checking the all bounds are free and zero clamped
  BOOST_CHECK(sol.free_idx.size() == nx);
  BOOST_CHECK(sol.clamped_idx.size() == 0);
  BOOST_CHECK(sol_reg.free_idx.size() == nx);
  BOOST_CHECK(sol_reg.clamped_idx.size() == 0);
}

void test_unconstrained_qp() {
  std::size_t nx = random_int_in_range(2, 5);
  crocoddyl::BoxQP boxqp(nx);
  boxqp.set_reg(0.);

  Eigen::MatrixXd H = Eigen::MatrixXd::Random(nx, nx);
  Eigen::MatrixXd hessian =
      H.transpose() * H + nx * Eigen::MatrixXd::Identity(nx, nx);
  hessian = 0.5 * (hessian + hessian.transpose()).eval();
  Eigen::VectorXd gradient = Eigen::VectorXd::Random(nx);
  Eigen::VectorXd lb =
      -std::numeric_limits<double>::infinity() * Eigen::VectorXd::Ones(nx);
  Eigen::VectorXd ub =
      std::numeric_limits<double>::infinity() * Eigen::VectorXd::Ones(nx);
  Eigen::VectorXd xinit = Eigen::VectorXd::Random(nx);
  crocoddyl::BoxQPSolution sol = boxqp.solve(hessian, gradient, lb, ub, xinit);

  // Checking the solution against the KKT solution
  Eigen::VectorXd xkkt = -hessian.inverse() * gradient;
  BOOST_CHECK((sol.x - xkkt).isZero(1e-9));

  // Checking the solution against a regularized KKT problem
  double reg = random_real_in_range(1e-9, 1e2);
  boxqp.set_reg(reg);
  crocoddyl::BoxQPSolution sol_reg =
      boxqp.solve(hessian, gradient, lb, ub, xinit);
  Eigen::VectorXd xkkt_reg =
      -(hessian + reg * Eigen::MatrixXd::Identity(nx, nx)).inverse() * gradient;
  BOOST_CHECK((sol_reg.x - xkkt_reg).isZero(1e-9));

  // Checking the all bounds are free and zero clamped
  BOOST_CHECK(sol.free_idx.size() == nx);
  BOOST_CHECK(sol.clamped_idx.size() == 0);
  BOOST_CHECK(sol_reg.free_idx.size() == nx);
  BOOST_CHECK(sol_reg.clamped_idx.size() == 0);
}

void test_box_qp_with_identity_hessian() {
  std::size_t nx = random_int_in_range(2, 5);
  crocoddyl::BoxQP boxqp(nx);
  boxqp.set_reg(0.);

  Eigen::MatrixXd hessian = Eigen::MatrixXd::Identity(nx, nx);
  Eigen::VectorXd gradient = Eigen::VectorXd::Ones(nx);
  for (std::size_t i = 0; i < nx; ++i) {
    gradient(i) *= random_real_in_range(-1., 1.);
  }
  Eigen::VectorXd lb = Eigen::VectorXd::Zero(nx);
  Eigen::VectorXd ub = Eigen::VectorXd::Ones(nx);
  Eigen::VectorXd xinit = Eigen::VectorXd::Random(nx);
  crocoddyl::BoxQPSolution sol = boxqp.solve(hessian, gradient, lb, ub, xinit);

  // The analytical solution is the a bounded, and negative, gradient
  Eigen::VectorXd negbounded_gradient(nx), negbounded_gradient_reg(nx);
  std::size_t nf = nx, nc = 0, nf_reg = nx, nc_reg = 0;
  double reg = random_real_in_range(1e-9, 1e2);
  for (std::size_t i = 0; i < nx; ++i) {
    negbounded_gradient(i) = std::max(std::min(-gradient(i), ub(i)), lb(i));
    negbounded_gradient_reg(i) =
        std::max(std::min(-gradient(i) / (1 + reg), ub(i)), lb(i));
    if (negbounded_gradient(i) != -gradient(i)) {
      nc += 1;
      nf -= 1;
    }
    if (negbounded_gradient_reg(i) != -gradient(i) / (1 + reg)) {
      nc_reg += 1;
      nf_reg -= 1;
    }
  }

  // Checking the solution of the problem. Note that it the negative of the
  // gradient since Hessian is identity matrix
  BOOST_CHECK((sol.x - negbounded_gradient).isZero(1e-9));

  // Checking the solution against a regularized case
  boxqp.set_reg(reg);
  crocoddyl::BoxQPSolution sol_reg =
      boxqp.solve(hessian, gradient, lb, ub, xinit);
  BOOST_CHECK((sol_reg.x - negbounded_gradient_reg).isZero(1e-9));

  // Checking the all bounds are free and zero clamped
  BOOST_CHECK(sol.free_idx.size() == nf);
  BOOST_CHECK(sol.clamped_idx.size() == nc);
  BOOST_CHECK(sol_reg.free_idx.size() == nf_reg);
  BOOST_CHECK(sol_reg.clamped_idx.size() == nc_reg);
}

void register_unit_tests() {
  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_constructor)));
  framework::master_test_suite().add(BOOST_TEST_CASE(
      boost::bind(&test_unconstrained_qp_with_identity_hessian)));
  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_unconstrained_qp)));
  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_box_qp_with_identity_hessian)));
}

bool init_function() {
  register_unit_tests();
  return true;
}

int main(int argc, char* argv[]) {
  return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2022, University of Edinburgh, Heriot-Watt University
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#define BOOST_TEST_NO_MAIN
10			#define BOOST_TEST_ALTERNATIVE_INIT_API
11
12			#include <boost/random.hpp>
13
14			#include "crocoddyl/core/solvers/box-qp.hpp"
15			#include "unittest_common.hpp"
16
17			using namespace boost::unit_test;
18			using namespace crocoddyl::unittest;
19
20		1	void test_constructor() {
21			// Setup the test
22	✓✗	1	std::size_t nx = random_int_in_range(1, 100);
23	✓✗	2	crocoddyl::BoxQP boxqp(nx);
24
25			// Test dimension of the decision vector
26	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(boxqp.get_nx() == nx);
27		1	}
28
29		1	void test_unconstrained_qp_with_identity_hessian() {
30	✓✗	1	std::size_t nx = random_int_in_range(2, 5);
31	✓✗	2	crocoddyl::BoxQP boxqp(nx);
32	✓✗	1	boxqp.set_reg(0.);
33
34	✓✗✓✗	2	Eigen::MatrixXd hessian = Eigen::MatrixXd::Identity(nx, nx);
35	✓✗✓✗	2	Eigen::VectorXd gradient = Eigen::VectorXd::Random(nx);
36			Eigen::VectorXd lb =
37	✓✗✓✗ ✓✗	2	-std::numeric_limits<double>::infinity() * Eigen::VectorXd::Ones(nx);
38			Eigen::VectorXd ub =
39	✓✗✓✗ ✓✗	2	std::numeric_limits<double>::infinity() * Eigen::VectorXd::Ones(nx);
40	✓✗✓✗	2	Eigen::VectorXd xinit = Eigen::VectorXd::Random(nx);
41	✓✗✓✗	2	crocoddyl::BoxQPSolution sol = boxqp.solve(hessian, gradient, lb, ub, xinit);
42
43			// Checking the solution of the problem. Note that it the negative of the
44			// gradient since Hessian is identity matrix
45	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK((sol.x + gradient).isZero(1e-9));
46
47			// Checking the solution against a regularized case
48	✓✗	1	double reg = random_real_in_range(1e-9, 1e2);
49	✓✗	1	boxqp.set_reg(reg);
50			crocoddyl::BoxQPSolution sol_reg =
51	✓✗✓✗	2	boxqp.solve(hessian, gradient, lb, ub, xinit);
52	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK((sol_reg.x + gradient / (1 + reg)).isZero(1e-9));
53
54			// Checking the all bounds are free and zero clamped
55	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(sol.free_idx.size() == nx);
56	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(sol.clamped_idx.size() == 0);
57	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(sol_reg.free_idx.size() == nx);
58	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(sol_reg.clamped_idx.size() == 0);
59		1	}
60
61		1	void test_unconstrained_qp() {
62	✓✗	1	std::size_t nx = random_int_in_range(2, 5);
63	✓✗	2	crocoddyl::BoxQP boxqp(nx);
64	✓✗	1	boxqp.set_reg(0.);
65
66	✓✗✓✗	2	Eigen::MatrixXd H = Eigen::MatrixXd::Random(nx, nx);
67			Eigen::MatrixXd hessian =
68	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	H.transpose() * H + nx * Eigen::MatrixXd::Identity(nx, nx);
69	✓✗✓✗ ✓✗✓✗ ✓✗	1	hessian = 0.5 * (hessian + hessian.transpose()).eval();
70	✓✗✓✗	2	Eigen::VectorXd gradient = Eigen::VectorXd::Random(nx);
71			Eigen::VectorXd lb =
72	✓✗✓✗ ✓✗	2	-std::numeric_limits<double>::infinity() * Eigen::VectorXd::Ones(nx);
73			Eigen::VectorXd ub =
74	✓✗✓✗ ✓✗	2	std::numeric_limits<double>::infinity() * Eigen::VectorXd::Ones(nx);
75	✓✗✓✗	2	Eigen::VectorXd xinit = Eigen::VectorXd::Random(nx);
76	✓✗✓✗	2	crocoddyl::BoxQPSolution sol = boxqp.solve(hessian, gradient, lb, ub, xinit);
77
78			// Checking the solution against the KKT solution
79	✓✗✓✗ ✓✗✓✗	2	Eigen::VectorXd xkkt = -hessian.inverse() * gradient;
80	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK((sol.x - xkkt).isZero(1e-9));
81
82			// Checking the solution against a regularized KKT problem
83	✓✗	1	double reg = random_real_in_range(1e-9, 1e2);
84	✓✗	1	boxqp.set_reg(reg);
85			crocoddyl::BoxQPSolution sol_reg =
86	✓✗✓✗	2	boxqp.solve(hessian, gradient, lb, ub, xinit);
87			Eigen::VectorXd xkkt_reg =
88	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	2	-(hessian + reg * Eigen::MatrixXd::Identity(nx, nx)).inverse() * gradient;
89	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK((sol_reg.x - xkkt_reg).isZero(1e-9));
90
91			// Checking the all bounds are free and zero clamped
92	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(sol.free_idx.size() == nx);
93	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(sol.clamped_idx.size() == 0);
94	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(sol_reg.free_idx.size() == nx);
95	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(sol_reg.clamped_idx.size() == 0);
96		1	}
97
98		1	void test_box_qp_with_identity_hessian() {
99	✓✗	1	std::size_t nx = random_int_in_range(2, 5);
100	✓✗	2	crocoddyl::BoxQP boxqp(nx);
101	✓✗	1	boxqp.set_reg(0.);
102
103	✓✗✓✗	2	Eigen::MatrixXd hessian = Eigen::MatrixXd::Identity(nx, nx);
104	✓✗✓✗	2	Eigen::VectorXd gradient = Eigen::VectorXd::Ones(nx);
105	✓✓	3	for (std::size_t i = 0; i < nx; ++i) {
106	✓✗✓✗	2	gradient(i) *= random_real_in_range(-1., 1.);
107			}
108	✓✗✓✗	2	Eigen::VectorXd lb = Eigen::VectorXd::Zero(nx);
109	✓✗✓✗	2	Eigen::VectorXd ub = Eigen::VectorXd::Ones(nx);
110	✓✗✓✗	2	Eigen::VectorXd xinit = Eigen::VectorXd::Random(nx);
111	✓✗✓✗	2	crocoddyl::BoxQPSolution sol = boxqp.solve(hessian, gradient, lb, ub, xinit);
112
113			// The analytical solution is the a bounded, and negative, gradient
114	✓✗✓✗	2	Eigen::VectorXd negbounded_gradient(nx), negbounded_gradient_reg(nx);
115		1	std::size_t nf = nx, nc = 0, nf_reg = nx, nc_reg = 0;
116	✓✗	1	double reg = random_real_in_range(1e-9, 1e2);
117	✓✓	3	for (std::size_t i = 0; i < nx; ++i) {
118	✓✗✓✗ ✓✗✓✗	2	negbounded_gradient(i) = std::max(std::min(-gradient(i), ub(i)), lb(i));
119	✓✗	2	negbounded_gradient_reg(i) =
120	✓✗✓✗ ✓✗	2	std::max(std::min(-gradient(i) / (1 + reg), ub(i)), lb(i));
121	✓✗✓✗ ✓✓	2	if (negbounded_gradient(i) != -gradient(i)) {
122		1	nc += 1;
123		1	nf -= 1;
124			}
125	✓✗✓✗ ✓✓	2	if (negbounded_gradient_reg(i) != -gradient(i) / (1 + reg)) {
126		1	nc_reg += 1;
127		1	nf_reg -= 1;
128			}
129			}
130
131			// Checking the solution of the problem. Note that it the negative of the
132			// gradient since Hessian is identity matrix
133	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK((sol.x - negbounded_gradient).isZero(1e-9));
134
135			// Checking the solution against a regularized case
136	✓✗	1	boxqp.set_reg(reg);
137			crocoddyl::BoxQPSolution sol_reg =
138	✓✗✓✗	2	boxqp.solve(hessian, gradient, lb, ub, xinit);
139	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK((sol_reg.x - negbounded_gradient_reg).isZero(1e-9));
140
141			// Checking the all bounds are free and zero clamped
142	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(sol.free_idx.size() == nf);
143	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(sol.clamped_idx.size() == nc);
144	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(sol_reg.free_idx.size() == nf_reg);
145	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(sol_reg.clamped_idx.size() == nc_reg);
146		1	}
147
148		1	void register_unit_tests() {
149		1	framework::master_test_suite().add(
150	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_constructor)));
151	✓✗✓✗ ✓✗✓✗	1	framework::master_test_suite().add(BOOST_TEST_CASE(
152			boost::bind(&test_unconstrained_qp_with_identity_hessian)));
153		1	framework::master_test_suite().add(
154	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_unconstrained_qp)));
155		1	framework::master_test_suite().add(
156	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_box_qp_with_identity_hessian)));
157		1	}
158
159		1	bool init_function() {
160		1	register_unit_tests();
161		1	return true;
162			}
163
164		1	int main(int argc, char* argv[]) {
165		1	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
166			}