GCC Code Coverage Report

Directory:	./
File:	unittest/test_friction_cone.cpp
Date:	2025-01-16 08:47:40

	Exec	Total	Coverage
Lines:	140	140	100.0%
Branches:	378	746	50.7%

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2021, University of Edinburgh, University of Oxford
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 <pinocchio/math/quaternion.hpp>
13
14			#include "crocoddyl/core/activations/quadratic-barrier.hpp"
15			#include "crocoddyl/multibody/friction-cone.hpp"
16			#include "unittest_common.hpp"
17
18			using namespace boost::unit_test;
19			using namespace crocoddyl::unittest;
20
21		1	void test_constructor() {
22			// Common parameters
23	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	double mu = random_real_in_range(0.01, 1.);
24	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	std::size_t nf = 2 * random_int_in_range(2, 16);
25		1	bool inner_appr = false;
26
27			// No rotation
28	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	1	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
29
30			// Create the friction cone with rotation and surface normal
31	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	1	crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
32
33	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 1 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 1 times.	1	BOOST_CHECK(cone.get_R().isApprox(R));
34	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 1 times.	1	BOOST_CHECK(cone.get_mu() == mu);
35	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 1 times.	1	BOOST_CHECK(cone.get_nf() == nf);
36	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 1 times.	1	BOOST_CHECK(cone.get_inner_appr() == inner_appr);
37	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✗ Branch 24 not taken. ✓ Branch 25 taken 1 times.	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_A().rows()) == nf + 1);
38	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✗ Branch 24 not taken. ✓ Branch 25 taken 1 times.	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_lb().size()) == nf + 1);
39	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✗ Branch 24 not taken. ✓ Branch 25 taken 1 times.	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_ub().size()) == nf + 1);
40
41			// With rotation
42	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	Eigen::Quaterniond q;
43	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	pinocchio::quaternion::uniformRandom(q);
44	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	R = q.toRotationMatrix();
45
46			// Create the friction cone
47	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	1	cone = crocoddyl::FrictionCone(R, mu, nf, inner_appr);
48
49	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 1 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 1 times.	1	BOOST_CHECK(cone.get_R().isApprox(R));
50	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 1 times.	1	BOOST_CHECK(cone.get_mu() == mu);
51	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 1 times.	1	BOOST_CHECK(cone.get_nf() == nf);
52	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 1 times.	1	BOOST_CHECK(cone.get_inner_appr() == inner_appr);
53	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✗ Branch 24 not taken. ✓ Branch 25 taken 1 times.	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_A().rows()) == nf + 1);
54	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✗ Branch 24 not taken. ✓ Branch 25 taken 1 times.	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_lb().size()) == nf + 1);
55	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✗ Branch 24 not taken. ✓ Branch 25 taken 1 times.	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_ub().size()) == nf + 1);
56
57			// Create the friction cone from a reference
58			{
59	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	crocoddyl::FrictionCone cone_reference(cone);
60
61	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✗ Branch 24 not taken. ✓ Branch 25 taken 1 times.	1	BOOST_CHECK(cone.get_nf() == cone_reference.get_nf());
62	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 1 times.	1	BOOST_CHECK(cone.get_A().isApprox(cone_reference.get_A()));
63	2/2 ✓ Branch 2 taken 31 times. ✓ Branch 3 taken 1 times.	32	for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size());
64			++i) {
65	8/16 ✓ Branch 1 taken 31 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 31 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 31 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 31 times. ✗ Branch 16 not taken. ✓ Branch 19 taken 31 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 31 times. ✗ Branch 23 not taken. ✓ Branch 25 taken 31 times. ✗ Branch 26 not taken. ✗ Branch 30 not taken. ✓ Branch 31 taken 31 times.	31	BOOST_CHECK(cone.get_ub()[i] == cone_reference.get_ub()[i]);
66	8/16 ✓ Branch 1 taken 31 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 31 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 31 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 31 times. ✗ Branch 16 not taken. ✓ Branch 19 taken 31 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 31 times. ✗ Branch 23 not taken. ✓ Branch 25 taken 31 times. ✗ Branch 26 not taken. ✗ Branch 30 not taken. ✓ Branch 31 taken 31 times.	31	BOOST_CHECK(cone.get_lb()[i] == cone_reference.get_lb()[i]);
67			}
68	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 1 times.	1	BOOST_CHECK(cone.get_R().isApprox(cone_reference.get_R()));
69	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✗ Branch 25 not taken. ✓ Branch 26 taken 1 times.	1	BOOST_CHECK(std::abs(cone.get_mu() - cone_reference.get_mu()) < 1e-9);
70	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✗ Branch 24 not taken. ✓ Branch 25 taken 1 times.	1	BOOST_CHECK(cone.get_inner_appr() == cone_reference.get_inner_appr());
71	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✗ Branch 25 not taken. ✓ Branch 26 taken 1 times.	1	BOOST_CHECK(std::abs(cone.get_min_nforce() -
72			cone_reference.get_min_nforce()) < 1e-9);
73	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✗ Branch 24 not taken. ✓ Branch 25 taken 1 times.	1	BOOST_CHECK(cone.get_max_nforce() == cone_reference.get_max_nforce());
74		1	}
75
76			// Create the friction cone through the copy operator
77			{
78	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	crocoddyl::FrictionCone cone_copy;
79	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	cone_copy = cone;
80
81	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✗ Branch 24 not taken. ✓ Branch 25 taken 1 times.	1	BOOST_CHECK(cone.get_nf() == cone_copy.get_nf());
82	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 1 times.	1	BOOST_CHECK(cone.get_A().isApprox(cone_copy.get_A()));
83	2/2 ✓ Branch 2 taken 31 times. ✓ Branch 3 taken 1 times.	32	for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size());
84			++i) {
85	8/16 ✓ Branch 1 taken 31 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 31 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 31 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 31 times. ✗ Branch 16 not taken. ✓ Branch 19 taken 31 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 31 times. ✗ Branch 23 not taken. ✓ Branch 25 taken 31 times. ✗ Branch 26 not taken. ✗ Branch 30 not taken. ✓ Branch 31 taken 31 times.	31	BOOST_CHECK(cone.get_ub()[i] == cone_copy.get_ub()[i]);
86	8/16 ✓ Branch 1 taken 31 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 31 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 31 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 31 times. ✗ Branch 16 not taken. ✓ Branch 19 taken 31 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 31 times. ✗ Branch 23 not taken. ✓ Branch 25 taken 31 times. ✗ Branch 26 not taken. ✗ Branch 30 not taken. ✓ Branch 31 taken 31 times.	31	BOOST_CHECK(cone.get_lb()[i] == cone_copy.get_lb()[i]);
87			}
88	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 1 times.	1	BOOST_CHECK(cone.get_R().isApprox(cone_copy.get_R()));
89	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✗ Branch 25 not taken. ✓ Branch 26 taken 1 times.	1	BOOST_CHECK(std::abs(cone.get_mu() - cone_copy.get_mu()) < 1e-9);
90	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✗ Branch 24 not taken. ✓ Branch 25 taken 1 times.	1	BOOST_CHECK(cone.get_inner_appr() == cone_copy.get_inner_appr());
91	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✗ Branch 25 not taken. ✓ Branch 26 taken 1 times.	1	BOOST_CHECK(std::abs(cone.get_min_nforce() - cone_copy.get_min_nforce()) <
92			1e-9);
93	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✗ Branch 24 not taken. ✓ Branch 25 taken 1 times.	1	BOOST_CHECK(cone.get_max_nforce() == cone_copy.get_max_nforce());
94		1	}
95		1	}
96
97		1	void test_inner_approximation_of_friction_cone() {
98	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	1	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
99	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	double mu = random_real_in_range(0.01, 1.);
100	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	std::size_t nf = 2 * random_int_in_range(2, 16);
101		1	bool inner_appr = true;
102	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	1	crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
103	3/6 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	1	const Eigen::VectorXd A_mu = -cone.get_A().col(2);
104	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	33	for (std::size_t i = 0; i < nf; ++i) {
105	7/14 ✓ Branch 1 taken 32 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 32 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 32 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 32 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 32 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 32 times. ✗ Branch 20 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 32 times.	32	BOOST_CHECK_CLOSE(
106			A_mu(i), mu * cos((2 * M_PI / static_cast<double>(nf)) / 2.), 1e-9);
107			}
108		1	}
109
110		1	void test_A_matrix_with_rotation_change() {
111			// Common parameters
112	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	double mu = random_real_in_range(0.01, 1.);
113	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	std::size_t nf = 2 * random_int_in_range(2, 16);
114		1	bool inner_appr = false;
115
116			// No rotation
117	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	1	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
118	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	1	crocoddyl::FrictionCone cone_1(R, mu, nf, inner_appr);
119
120			// With rotation
121	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	Eigen::Quaterniond q;
122	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	pinocchio::quaternion::uniformRandom(q);
123	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	R = q.toRotationMatrix();
124	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	1	crocoddyl::FrictionCone cone_2(R, mu, nf, inner_appr);
125
126	2/2 ✓ Branch 0 taken 5 times. ✓ Branch 1 taken 1 times.	6	for (std::size_t i = 0; i < 5; ++i) {
127	11/22 ✓ Branch 1 taken 5 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 5 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 5 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 5 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 5 times. ✗ Branch 19 not taken. ✓ Branch 22 taken 5 times. ✗ Branch 23 not taken. ✓ Branch 25 taken 5 times. ✗ Branch 26 not taken. ✓ Branch 28 taken 5 times. ✗ Branch 29 not taken. ✓ Branch 31 taken 5 times. ✗ Branch 32 not taken. ✓ Branch 34 taken 5 times. ✗ Branch 35 not taken. ✗ Branch 39 not taken. ✓ Branch 40 taken 5 times.	5	BOOST_CHECK(
128			(cone_1.get_A().row(i) - cone_2.get_A().row(i) * R).isZero(1e-9));
129			}
130		1	}
131
132		1	void test_force_along_friction_cone_normal() {
133			// Create the friction cone
134	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	Eigen::Quaterniond q;
135	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	pinocchio::quaternion::uniformRandom(q);
136	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	Eigen::Matrix3d R = q.toRotationMatrix();
137	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	double mu = random_real_in_range(0.01, 1.);
138	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	std::size_t nf = 2 * random_int_in_range(2, 16);
139		1	bool inner_appr = false;
140	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	1	crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
141
142			// Create the activation for quadratic barrier
143	1/2 ✓ Branch 3 taken 1 times. ✗ Branch 4 not taken.	1	crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub());
144	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
145			boost::shared_ptr<crocoddyl::ActivationDataAbstract> data =
146	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	activation.createData();
147
148			// Compute the activation value
149	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken.	1	Eigen::Vector3d force = random_real_in_range(0., 100.) * R.col(2);
150	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	1	Eigen::VectorXd r = cone.get_A() * force;
151	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	1	activation.calc(data, r);
152
153			// The activation value has to be zero since the force is inside the friction
154			// cone
155	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 1 times.	1	BOOST_CHECK(data->a_value == 0.);
156		1	}
157
158		1	void test_negative_force_along_friction_cone_normal() {
159			// Create the friction cone
160	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	Eigen::Quaterniond q;
161	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	pinocchio::quaternion::uniformRandom(q);
162	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	Eigen::Matrix3d R = q.toRotationMatrix();
163	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	double mu = random_real_in_range(0.01, 1.);
164	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	std::size_t nf = 2 * random_int_in_range(2, 16);
165		1	bool inner_appr = false;
166	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	1	crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
167
168			// Create the activation for quadratic barrier
169	1/2 ✓ Branch 3 taken 1 times. ✗ Branch 4 not taken.	1	crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub());
170	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
171			boost::shared_ptr<crocoddyl::ActivationDataAbstract> data =
172	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	activation.createData();
173
174			// Compute the activation value
175	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken.	1	Eigen::Vector3d force = -random_real_in_range(0., 100.) * R.col(2);
176	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	1	Eigen::VectorXd r = cone.get_A() * force;
177
178			// The first nf elements of the residual has to be positive since the force is
179			// outside the friction cone. Additionally, the last value has to be equals to
180			// the force norm but with negative value since the forces is aligned and in
181			// opposite direction to the friction cone orientation
182	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	33	for (std::size_t i = 0; i < nf; ++i) {
183	7/14 ✓ Branch 1 taken 32 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 32 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 32 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 32 times. ✗ Branch 15 not taken. ✓ Branch 17 taken 32 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 32 times. ✗ Branch 21 not taken. ✗ Branch 25 not taken. ✓ Branch 26 taken 32 times.	32	BOOST_CHECK(r(i) > 0.);
184			}
185	8/16 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 1 times. ✗ Branch 23 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	1	BOOST_CHECK_CLOSE(r(nf), -force.norm(), 1e-9);
186
187			// The activation value has to be positive since the force is outside the
188			// friction cone
189	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	1	activation.calc(data, r);
190	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 1 times.	1	BOOST_CHECK(data->a_value > 0.);
191		1	}
192
193		1	void test_force_parallel_to_friction_cone_normal() {
194			// Create the friction cone
195	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	1	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
196	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	double mu = random_real_in_range(0.01, 1.);
197	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	std::size_t nf = 2 * random_int_in_range(2, 16);
198		1	bool inner_appr = false;
199	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	1	crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
200
201			// Create the activation for quadratic barrier
202	1/2 ✓ Branch 3 taken 1 times. ✗ Branch 4 not taken.	1	crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub());
203	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
204			boost::shared_ptr<crocoddyl::ActivationDataAbstract> data =
205	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	activation.createData();
206
207			// Compute the activation value
208			Eigen::Vector3d force =
209	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken.	1	-random_real_in_range(0., 100.) * Eigen::Vector3d::UnitX();
210	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	1	Eigen::VectorXd r = cone.get_A() * force;
211
212			// The last value of the residual is equals to zero since the force is
213			// parallel to the friction cone orientation
214	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 1 times.	1	BOOST_CHECK_CLOSE(r(nf), 0., 1e-9);
215
216			// The activation value has to be positive since the force is outside the
217			// friction cone
218	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	1	activation.calc(data, r);
219	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 1 times.	1	BOOST_CHECK(data->a_value > 0.);
220		1	}
221
222		1	void register_unit_tests() {
223		1	framework::master_test_suite().add(
224	4/8 ✓ Branch 3 taken 1 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 1 times. ✗ Branch 7 not taken. ✓ Branch 9 taken 1 times. ✗ Branch 10 not taken. ✓ Branch 12 taken 1 times. ✗ Branch 13 not taken.	1	BOOST_TEST_CASE(boost::bind(&test_constructor)));
225		1	framework::master_test_suite().add(
226	4/8 ✓ Branch 3 taken 1 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 1 times. ✗ Branch 7 not taken. ✓ Branch 9 taken 1 times. ✗ Branch 10 not taken. ✓ Branch 12 taken 1 times. ✗ Branch 13 not taken.	1	BOOST_TEST_CASE(boost::bind(&test_inner_approximation_of_friction_cone)));
227		1	framework::master_test_suite().add(
228	4/8 ✓ Branch 3 taken 1 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 1 times. ✗ Branch 7 not taken. ✓ Branch 9 taken 1 times. ✗ Branch 10 not taken. ✓ Branch 12 taken 1 times. ✗ Branch 13 not taken.	1	BOOST_TEST_CASE(boost::bind(&test_A_matrix_with_rotation_change)));
229		1	framework::master_test_suite().add(
230	4/8 ✓ Branch 3 taken 1 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 1 times. ✗ Branch 7 not taken. ✓ Branch 9 taken 1 times. ✗ Branch 10 not taken. ✓ Branch 12 taken 1 times. ✗ Branch 13 not taken.	1	BOOST_TEST_CASE(boost::bind(&test_force_along_friction_cone_normal)));
231	4/8 ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken.	1	framework::master_test_suite().add(BOOST_TEST_CASE(
232			boost::bind(&test_negative_force_along_friction_cone_normal)));
233	4/8 ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken.	1	framework::master_test_suite().add(BOOST_TEST_CASE(
234			boost::bind(&test_force_parallel_to_friction_cone_normal)));
235		1	}
236
237		1	bool init_function() {
238		1	register_unit_tests();
239		1	return true;
240			}
241
242		1	int main(int argc, char* argv[]) {
243		1	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
244			}
245