GCC Code Coverage Report

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