Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_solvers.cpp	Lines:	91	91	100.0 %
Date:	2024-02-13 11:12:33	Branches:	216	420	51.4 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, New York University,
//                          Max Planck Gesellschaft, University of Edinburgh,
//                          INRIA
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API

#include "crocoddyl/core/utils/callbacks.hpp"
#include "factory/solver.hpp"
#include "unittest_common.hpp"

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

//____________________________________________________________________________//

void test_kkt_dimension(ActionModelTypes::Type action_type, size_t T) {
  // Create the kkt solver
  SolverFactory factory;
  boost::shared_ptr<crocoddyl::SolverKKT> kkt =
      boost::static_pointer_cast<crocoddyl::SolverKKT>(
          factory.create(SolverTypes::SolverKKT, action_type, T));

  // define some aliases
  const std::size_t ndx = kkt->get_ndx();
  const std::size_t nu = kkt->get_nu();

  // Test the different matrix sizes
  BOOST_CHECK_EQUAL(kkt->get_kkt().rows(), 2 * ndx + nu);
  BOOST_CHECK_EQUAL(kkt->get_kkt().cols(), 2 * ndx + nu);
  BOOST_CHECK_EQUAL(kkt->get_kktref().size(), 2 * ndx + nu);
  BOOST_CHECK_EQUAL(kkt->get_primaldual().size(), 2 * ndx + nu);
  BOOST_CHECK_EQUAL(kkt->get_us().size(), T);
  BOOST_CHECK_EQUAL(kkt->get_xs().size(), T + 1);
}

//____________________________________________________________________________//

void test_kkt_search_direction(ActionModelTypes::Type action_type, size_t T) {
  // Create the kkt solver
  SolverFactory factory;
  boost::shared_ptr<crocoddyl::SolverKKT> kkt =
      boost::static_pointer_cast<crocoddyl::SolverKKT>(
          factory.create(SolverTypes::SolverKKT, action_type, T));

  // Generate the different state along the trajectory
  const boost::shared_ptr<crocoddyl::ShootingProblem>& problem =
      kkt->get_problem();
  const boost::shared_ptr<crocoddyl::StateAbstract>& state =
      problem->get_runningModels()[0]->get_state();
  std::vector<Eigen::VectorXd> xs;
  std::vector<Eigen::VectorXd> us;
  for (std::size_t i = 0; i < T; ++i) {
    const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
        problem->get_runningModels()[i];
    xs.push_back(state->rand());
    us.push_back(Eigen::VectorXd::Random(model->get_nu()));
  }
  xs.push_back(state->rand());

  // Compute the search direction
  kkt->setCandidate(xs, us);
  kkt->computeDirection();

  // define some aliases
  const std::size_t ndx = kkt->get_ndx();
  const std::size_t nu = kkt->get_nu();
  Eigen::MatrixXd kkt_mat = kkt->get_kkt();
  Eigen::Block<Eigen::MatrixXd> hess = kkt_mat.block(0, 0, ndx + nu, ndx + nu);

  // Checking the symmetricity of the Hessian
  BOOST_CHECK((hess - hess.transpose()).isZero(1e-9));

  // Check initial state
  BOOST_CHECK((state->diff_dx(state->integrate_x(xs[0], kkt->get_dxs()[0]),
                              kkt->get_problem()->get_x0()))
                  .isZero(1e-9));
}

//____________________________________________________________________________//

void test_solver_against_kkt_solver(SolverTypes::Type solver_type,
                                    ActionModelTypes::Type action_type,
                                    size_t T) {
  // Create the testing and KKT solvers
  SolverFactory solver_factory;
  boost::shared_ptr<crocoddyl::SolverAbstract> solver =
      solver_factory.create(solver_type, action_type, T);
  boost::shared_ptr<crocoddyl::SolverAbstract> kkt =
      solver_factory.create(SolverTypes::SolverKKT, action_type, T);

  // Get the pointer to the problem so we can create the equivalent kkt solver.
  const boost::shared_ptr<crocoddyl::ShootingProblem>& problem =
      solver->get_problem();

  // Generate the different state along the trajectory
  const boost::shared_ptr<crocoddyl::StateAbstract>& state =
      problem->get_runningModels()[0]->get_state();
  std::vector<Eigen::VectorXd> xs;
  std::vector<Eigen::VectorXd> us;
  for (std::size_t i = 0; i < T; ++i) {
    const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
        problem->get_runningModels()[i];
    xs.push_back(state->rand());
    us.push_back(Eigen::VectorXd::Random(model->get_nu()));
  }
  xs.push_back(state->rand());

  // Define the callback function
  std::vector<boost::shared_ptr<crocoddyl::CallbackAbstract> > cbs;
  cbs.push_back(boost::make_shared<crocoddyl::CallbackVerbose>());
  kkt->setCallbacks(cbs);
  solver->setCallbacks(cbs);

  // Print the name of the action model for introspection
  std::cout << ActionModelTypes::all[action_type] << std::endl;

  // Solve the problem using the KKT solver
  kkt->solve(xs, us, 100);

  // Solve the problem using the solver in testing
  solver->solve(xs, us, 100);

  // check trajectory dimensions
  BOOST_CHECK_EQUAL(solver->get_us().size(), T);
  BOOST_CHECK_EQUAL(solver->get_xs().size(), T + 1);

  // initial state
  BOOST_CHECK((solver->get_xs()[0] - problem->get_x0()).isZero(1e-9));

  // check solutions against each other
  for (unsigned int t = 0; t < T; ++t) {
    const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
        solver->get_problem()->get_runningModels()[t];
    std::size_t nu = model->get_nu();
    BOOST_CHECK(
        (state->diff_dx(solver->get_xs()[t], kkt->get_xs()[t])).isZero(1e-9));
    BOOST_CHECK((solver->get_us()[t].head(nu) - kkt->get_us()[t]).isZero(1e-9));
  }
  BOOST_CHECK(
      (state->diff_dx(solver->get_xs()[T], kkt->get_xs()[T])).isZero(1e-9));
}

//____________________________________________________________________________//

void register_kkt_solver_unit_tests(ActionModelTypes::Type action_type,
                                    const std::size_t T) {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_" << action_type;
  test_suite* ts = BOOST_TEST_SUITE(test_name.str());
  ts->add(BOOST_TEST_CASE(boost::bind(&test_kkt_dimension, action_type, T)));
  ts->add(
      BOOST_TEST_CASE(boost::bind(&test_kkt_search_direction, action_type, T)));
  framework::master_test_suite().add(ts);
}

void register_solvers_againt_kkt_unit_tests(SolverTypes::Type solver_type,
                                            ActionModelTypes::Type action_type,
                                            const std::size_t T) {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_" << solver_type << "_" << action_type;
  test_suite* ts = BOOST_TEST_SUITE(test_name.str());
  std::cout << "Running " << test_name.str() << std::endl;
  ts->add(BOOST_TEST_CASE(boost::bind(&test_solver_against_kkt_solver,
                                      solver_type, action_type, T)));
  framework::master_test_suite().add(ts);
}

//____________________________________________________________________________//

bool init_function() {
  std::size_t T = 10;

  for (size_t i = 0; i < ActionModelTypes::all.size(); ++i) {
    register_kkt_solver_unit_tests(ActionModelTypes::all[i], T);
  }

  // We start from 1 as 0 is the kkt solver
  for (size_t s = 1; s < SolverTypes::all.size(); ++s) {
    for (size_t i = 0; i < ActionModelTypes::ActionModelImpulseFwdDynamics_HyQ;
         ++i) {
      register_solvers_againt_kkt_unit_tests(SolverTypes::all[s],
                                             ActionModelTypes::all[i], T);
    }
  }
  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-2021, LAAS-CNRS, New York University,
5			// Max Planck Gesellschaft, University of Edinburgh,
6			// INRIA
7			// Copyright note valid unless otherwise stated in individual files.
8			// All rights reserved.
9			///////////////////////////////////////////////////////////////////////////////
10
11			#define BOOST_TEST_NO_MAIN
12			#define BOOST_TEST_ALTERNATIVE_INIT_API
13
14			#include "crocoddyl/core/utils/callbacks.hpp"
15			#include "factory/solver.hpp"
16			#include "unittest_common.hpp"
17
18			using namespace boost::unit_test;
19			using namespace crocoddyl::unittest;
20
21			//____________________________________________________________________________//
22
23		5	void test_kkt_dimension(ActionModelTypes::Type action_type, size_t T) {
24			// Create the kkt solver
25	✓✗	10	SolverFactory factory;
26			boost::shared_ptr<crocoddyl::SolverKKT> kkt =
27			boost::static_pointer_cast<crocoddyl::SolverKKT>(
28	✓✗	10	factory.create(SolverTypes::SolverKKT, action_type, T));
29
30			// define some aliases
31	✓✗	5	const std::size_t ndx = kkt->get_ndx();
32	✓✗	5	const std::size_t nu = kkt->get_nu();
33
34			// Test the different matrix sizes
35	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	5	BOOST_CHECK_EQUAL(kkt->get_kkt().rows(), 2 * ndx + nu);
36	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	5	BOOST_CHECK_EQUAL(kkt->get_kkt().cols(), 2 * ndx + nu);
37	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	5	BOOST_CHECK_EQUAL(kkt->get_kktref().size(), 2 * ndx + nu);
38	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	5	BOOST_CHECK_EQUAL(kkt->get_primaldual().size(), 2 * ndx + nu);
39	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	5	BOOST_CHECK_EQUAL(kkt->get_us().size(), T);
40	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	5	BOOST_CHECK_EQUAL(kkt->get_xs().size(), T + 1);
41		5	}
42
43			//____________________________________________________________________________//
44
45		5	void test_kkt_search_direction(ActionModelTypes::Type action_type, size_t T) {
46			// Create the kkt solver
47	✓✗	10	SolverFactory factory;
48			boost::shared_ptr<crocoddyl::SolverKKT> kkt =
49			boost::static_pointer_cast<crocoddyl::SolverKKT>(
50	✓✗	10	factory.create(SolverTypes::SolverKKT, action_type, T));
51
52			// Generate the different state along the trajectory
53			const boost::shared_ptr<crocoddyl::ShootingProblem>& problem =
54	✓✗	5	kkt->get_problem();
55			const boost::shared_ptr<crocoddyl::StateAbstract>& state =
56		5	problem->get_runningModels()[0]->get_state();
57		10	std::vector<Eigen::VectorXd> xs;
58		10	std::vector<Eigen::VectorXd> us;
59	✓✓	55	for (std::size_t i = 0; i < T; ++i) {
60			const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
61		50	problem->get_runningModels()[i];
62	✓✗✓✗	50	xs.push_back(state->rand());
63	✓✗✓✗ ✓✗	50	us.push_back(Eigen::VectorXd::Random(model->get_nu()));
64			}
65	✓✗✓✗	5	xs.push_back(state->rand());
66
67			// Compute the search direction
68	✓✗	5	kkt->setCandidate(xs, us);
69	✓✗	5	kkt->computeDirection();
70
71			// define some aliases
72	✓✗	5	const std::size_t ndx = kkt->get_ndx();
73	✓✗	5	const std::size_t nu = kkt->get_nu();
74	✓✗✓✗	10	Eigen::MatrixXd kkt_mat = kkt->get_kkt();
75	✓✗	5	Eigen::Block<Eigen::MatrixXd> hess = kkt_mat.block(0, 0, ndx + nu, ndx + nu);
76
77			// Checking the symmetricity of the Hessian
78	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	5	BOOST_CHECK((hess - hess.transpose()).isZero(1e-9));
79
80			// Check initial state
81	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	5	BOOST_CHECK((state->diff_dx(state->integrate_x(xs[0], kkt->get_dxs()[0]),
82			kkt->get_problem()->get_x0()))
83			.isZero(1e-9));
84		5	}
85
86			//____________________________________________________________________________//
87
88		15	void test_solver_against_kkt_solver(SolverTypes::Type solver_type,
89			ActionModelTypes::Type action_type,
90			size_t T) {
91			// Create the testing and KKT solvers
92	✓✗	30	SolverFactory solver_factory;
93			boost::shared_ptr<crocoddyl::SolverAbstract> solver =
94	✓✗	30	solver_factory.create(solver_type, action_type, T);
95			boost::shared_ptr<crocoddyl::SolverAbstract> kkt =
96	✓✗	30	solver_factory.create(SolverTypes::SolverKKT, action_type, T);
97
98			// Get the pointer to the problem so we can create the equivalent kkt solver.
99			const boost::shared_ptr<crocoddyl::ShootingProblem>& problem =
100	✓✗	15	solver->get_problem();
101
102			// Generate the different state along the trajectory
103			const boost::shared_ptr<crocoddyl::StateAbstract>& state =
104		15	problem->get_runningModels()[0]->get_state();
105		30	std::vector<Eigen::VectorXd> xs;
106		30	std::vector<Eigen::VectorXd> us;
107	✓✓	165	for (std::size_t i = 0; i < T; ++i) {
108			const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
109		150	problem->get_runningModels()[i];
110	✓✗✓✗	150	xs.push_back(state->rand());
111	✓✗✓✗ ✓✗	150	us.push_back(Eigen::VectorXd::Random(model->get_nu()));
112			}
113	✓✗✓✗	15	xs.push_back(state->rand());
114
115			// Define the callback function
116		30	std::vector<boost::shared_ptr<crocoddyl::CallbackAbstract> > cbs;
117	✓✗✓✗	15	cbs.push_back(boost::make_shared<crocoddyl::CallbackVerbose>());
118	✓✗	15	kkt->setCallbacks(cbs);
119	✓✗	15	solver->setCallbacks(cbs);
120
121			// Print the name of the action model for introspection
122	✓✗✓✗	15	std::cout << ActionModelTypes::all[action_type] << std::endl;
123
124			// Solve the problem using the KKT solver
125	✓✗	15	kkt->solve(xs, us, 100);
126
127			// Solve the problem using the solver in testing
128	✓✗	15	solver->solve(xs, us, 100);
129
130			// check trajectory dimensions
131	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	15	BOOST_CHECK_EQUAL(solver->get_us().size(), T);
132	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	15	BOOST_CHECK_EQUAL(solver->get_xs().size(), T + 1);
133
134			// initial state
135	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	15	BOOST_CHECK((solver->get_xs()[0] - problem->get_x0()).isZero(1e-9));
136
137			// check solutions against each other
138	✓✓	165	for (unsigned int t = 0; t < T; ++t) {
139			const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
140	✓✗	150	solver->get_problem()->get_runningModels()[t];
141		150	std::size_t nu = model->get_nu();
142	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	150	BOOST_CHECK(
143			(state->diff_dx(solver->get_xs()[t], kkt->get_xs()[t])).isZero(1e-9));
144	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	150	BOOST_CHECK((solver->get_us()[t].head(nu) - kkt->get_us()[t]).isZero(1e-9));
145			}
146	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	15	BOOST_CHECK(
147			(state->diff_dx(solver->get_xs()[T], kkt->get_xs()[T])).isZero(1e-9));
148		15	}
149
150			//____________________________________________________________________________//
151
152		5	void register_kkt_solver_unit_tests(ActionModelTypes::Type action_type,
153			const std::size_t T) {
154	✓✗✓✗	10	boost::test_tools::output_test_stream test_name;
155	✓✗✓✗	5	test_name << "test_" << action_type;
156	✓✗✓✗ ✓✗✓✗	5	test_suite* ts = BOOST_TEST_SUITE(test_name.str());
157	✓✗✓✗ ✓✗✓✗ ✓✗	5	ts->add(BOOST_TEST_CASE(boost::bind(&test_kkt_dimension, action_type, T)));
158		5	ts->add(
159	✓✗✓✗ ✓✗✓✗ ✓✗	5	BOOST_TEST_CASE(boost::bind(&test_kkt_search_direction, action_type, T)));
160	✓✗✓✗ ✓✗	5	framework::master_test_suite().add(ts);
161		5	}
162
163		15	void register_solvers_againt_kkt_unit_tests(SolverTypes::Type solver_type,
164			ActionModelTypes::Type action_type,
165			const std::size_t T) {
166	✓✗✓✗	30	boost::test_tools::output_test_stream test_name;
167	✓✗✓✗ ✓✗✓✗	15	test_name << "test_" << solver_type << "_" << action_type;
168	✓✗✓✗ ✓✗✓✗	15	test_suite* ts = BOOST_TEST_SUITE(test_name.str());
169	✓✗✓✗ ✓✗✓✗	15	std::cout << "Running " << test_name.str() << std::endl;
170	✓✗✓✗ ✓✗✓✗ ✓✗	15	ts->add(BOOST_TEST_CASE(boost::bind(&test_solver_against_kkt_solver,
171			solver_type, action_type, T)));
172	✓✗✓✗ ✓✗	15	framework::master_test_suite().add(ts);
173		15	}
174
175			//____________________________________________________________________________//
176
177		1	bool init_function() {
178		1	std::size_t T = 10;
179
180	✓✓	6	for (size_t i = 0; i < ActionModelTypes::all.size(); ++i) {
181		5	register_kkt_solver_unit_tests(ActionModelTypes::all[i], T);
182			}
183
184			// We start from 1 as 0 is the kkt solver
185	✓✓	6	for (size_t s = 1; s < SolverTypes::all.size(); ++s) {
186	✓✓	20	for (size_t i = 0; i < ActionModelTypes::ActionModelImpulseFwdDynamics_HyQ;
187			++i) {
188		15	register_solvers_againt_kkt_unit_tests(SolverTypes::all[s],
189		15	ActionModelTypes::all[i], T);
190			}
191			}
192		1	return true;
193			}
194
195			//____________________________________________________________________________//
196
197		1	int main(int argc, char* argv[]) {
198		1	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
199			}