Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_friction_cone.cpp	Lines:	138	138	100.0 %
Date:	2024-02-13 11:12:33	Branches:	386	762	50.7 %


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

#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API

#include <pinocchio/math/quaternion.hpp>

#include "crocoddyl/core/activations/quadratic-barrier.hpp"
#include "crocoddyl/multibody/friction-cone.hpp"
#include "unittest_common.hpp"

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

void test_constructor() {
  // Common parameters
  double mu = random_real_in_range(0.01, 1.);
  std::size_t nf = 2 * random_int_in_range(2, 16);
  bool inner_appr = false;

  // No rotation
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();

  // Create the friction cone with rotation and surface normal
  crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);

  BOOST_CHECK(cone.get_R().isApprox(R));
  BOOST_CHECK(cone.get_mu() == mu);
  BOOST_CHECK(cone.get_nf() == nf);
  BOOST_CHECK(cone.get_inner_appr() == inner_appr);
  BOOST_CHECK(static_cast<std::size_t>(cone.get_A().rows()) == nf + 1);
  BOOST_CHECK(static_cast<std::size_t>(cone.get_lb().size()) == nf + 1);
  BOOST_CHECK(static_cast<std::size_t>(cone.get_ub().size()) == nf + 1);

  // With rotation
  Eigen::Quaterniond q;
  pinocchio::quaternion::uniformRandom(q);
  R = q.toRotationMatrix();

  // Create the friction cone
  cone = crocoddyl::FrictionCone(R, mu, nf, inner_appr);

  BOOST_CHECK(cone.get_R().isApprox(R));
  BOOST_CHECK(cone.get_mu() == mu);
  BOOST_CHECK(cone.get_nf() == nf);
  BOOST_CHECK(cone.get_inner_appr() == inner_appr);
  BOOST_CHECK(static_cast<std::size_t>(cone.get_A().rows()) == nf + 1);
  BOOST_CHECK(static_cast<std::size_t>(cone.get_lb().size()) == nf + 1);
  BOOST_CHECK(static_cast<std::size_t>(cone.get_ub().size()) == nf + 1);

  // Create the friction cone from a reference
  {
    crocoddyl::FrictionCone cone_reference(cone);

    BOOST_CHECK(cone.get_nf() == cone_reference.get_nf());
    BOOST_CHECK(cone.get_A().isApprox(cone_reference.get_A()));
    for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size());
         ++i) {
      BOOST_CHECK(cone.get_ub()[i] == cone_reference.get_ub()[i]);
      BOOST_CHECK(cone.get_lb()[i] == cone_reference.get_lb()[i]);
    }
    BOOST_CHECK(cone.get_R().isApprox(cone_reference.get_R()));
    BOOST_CHECK(std::abs(cone.get_mu() - cone_reference.get_mu()) < 1e-9);
    BOOST_CHECK(cone.get_inner_appr() == cone_reference.get_inner_appr());
    BOOST_CHECK(std::abs(cone.get_min_nforce() -
                         cone_reference.get_min_nforce()) < 1e-9);
    BOOST_CHECK(cone.get_max_nforce() == cone_reference.get_max_nforce());
  }

  // Create the friction cone through the copy operator
  {
    crocoddyl::FrictionCone cone_copy;
    cone_copy = cone;

    BOOST_CHECK(cone.get_nf() == cone_copy.get_nf());
    BOOST_CHECK(cone.get_A().isApprox(cone_copy.get_A()));
    for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size());
         ++i) {
      BOOST_CHECK(cone.get_ub()[i] == cone_copy.get_ub()[i]);
      BOOST_CHECK(cone.get_lb()[i] == cone_copy.get_lb()[i]);
    }
    BOOST_CHECK(cone.get_R().isApprox(cone_copy.get_R()));
    BOOST_CHECK(std::abs(cone.get_mu() - cone_copy.get_mu()) < 1e-9);
    BOOST_CHECK(cone.get_inner_appr() == cone_copy.get_inner_appr());
    BOOST_CHECK(std::abs(cone.get_min_nforce() - cone_copy.get_min_nforce()) <
                1e-9);
    BOOST_CHECK(cone.get_max_nforce() == cone_copy.get_max_nforce());
  }
}

void test_inner_approximation_of_friction_cone() {
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  double mu = random_real_in_range(0.01, 1.);
  std::size_t nf = 2 * random_int_in_range(2, 16);
  bool inner_appr = true;
  crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
  const Eigen::VectorXd A_mu = -cone.get_A().col(2);
  for (std::size_t i = 0; i < nf; ++i) {
    BOOST_CHECK_CLOSE(
        A_mu(i), mu * cos((2 * M_PI / static_cast<double>(nf)) / 2.), 1e-9);
  }
}

void test_A_matrix_with_rotation_change() {
  // Common parameters
  double mu = random_real_in_range(0.01, 1.);
  std::size_t nf = 2 * random_int_in_range(2, 16);
  bool inner_appr = false;

  // No rotation
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  crocoddyl::FrictionCone cone_1(R, mu, nf, inner_appr);

  // With rotation
  Eigen::Quaterniond q;
  pinocchio::quaternion::uniformRandom(q);
  R = q.toRotationMatrix();
  crocoddyl::FrictionCone cone_2(R, mu, nf, inner_appr);

  for (std::size_t i = 0; i < 5; ++i) {
    BOOST_CHECK(
        (cone_1.get_A().row(i) - cone_2.get_A().row(i) * R).isZero(1e-9));
  }
}

void test_force_along_friction_cone_normal() {
  // Create the friction cone
  Eigen::Quaterniond q;
  pinocchio::quaternion::uniformRandom(q);
  Eigen::Matrix3d R = q.toRotationMatrix();
  double mu = random_real_in_range(0.01, 1.);
  std::size_t nf = 2 * random_int_in_range(2, 16);
  bool inner_appr = false;
  crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);

  // Create the activation for quadratic barrier
  crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub());
  crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
  boost::shared_ptr<crocoddyl::ActivationDataAbstract> data =
      activation.createData();

  // Compute the activation value
  Eigen::Vector3d force = random_real_in_range(0., 100.) * R.col(2);
  Eigen::VectorXd r = cone.get_A() * force;
  activation.calc(data, r);

  // The activation value has to be zero since the force is inside the friction
  // cone
  BOOST_CHECK(data->a_value == 0.);
}

void test_negative_force_along_friction_cone_normal() {
  // Create the friction cone
  Eigen::Quaterniond q;
  pinocchio::quaternion::uniformRandom(q);
  Eigen::Matrix3d R = q.toRotationMatrix();
  double mu = random_real_in_range(0.01, 1.);
  std::size_t nf = 2 * random_int_in_range(2, 16);
  bool inner_appr = false;
  crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);

  // Create the activation for quadratic barrier
  crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub());
  crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
  boost::shared_ptr<crocoddyl::ActivationDataAbstract> data =
      activation.createData();

  // Compute the activation value
  Eigen::Vector3d force = -random_real_in_range(0., 100.) * R.col(2);
  Eigen::VectorXd r = cone.get_A() * force;

  // The first nf elements of the residual has to be positive since the force is
  // outside the friction cone. Additionally, the last value has to be equals to
  // the force norm but with negative value since the forces is aligned and in
  // opposite direction to the friction cone orientation
  for (std::size_t i = 0; i < nf; ++i) {
    BOOST_CHECK(r(i) > 0.);
  }
  BOOST_CHECK_CLOSE(r(nf), -force.norm(), 1e-9);

  // The activation value has to be positive since the force is outside the
  // friction cone
  activation.calc(data, r);
  BOOST_CHECK(data->a_value > 0.);
}

void test_force_parallel_to_friction_cone_normal() {
  // Create the friction cone
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  double mu = random_real_in_range(0.01, 1.);
  std::size_t nf = 2 * random_int_in_range(2, 16);
  bool inner_appr = false;
  crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);

  // Create the activation for quadratic barrier
  crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub());
  crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
  boost::shared_ptr<crocoddyl::ActivationDataAbstract> data =
      activation.createData();

  // Compute the activation value
  Eigen::Vector3d force =
      -random_real_in_range(0., 100.) * Eigen::Vector3d::UnitX();
  Eigen::VectorXd r = cone.get_A() * force;

  // The last value of the residual is equals to zero since the force is
  // parallel to the friction cone orientation
  BOOST_CHECK_CLOSE(r(nf), 0., 1e-9);

  // The activation value has to be positive since the force is outside the
  // friction cone
  activation.calc(data, r);
  BOOST_CHECK(data->a_value > 0.);
}

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_inner_approximation_of_friction_cone)));
  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_A_matrix_with_rotation_change)));
  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_force_along_friction_cone_normal)));
  framework::master_test_suite().add(BOOST_TEST_CASE(
      boost::bind(&test_negative_force_along_friction_cone_normal)));
  framework::master_test_suite().add(BOOST_TEST_CASE(
      boost::bind(&test_force_parallel_to_friction_cone_normal)));
}

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-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	double mu = random_real_in_range(0.01, 1.);
24	✓✗	1	std::size_t nf = 2 * random_int_in_range(2, 16);
25		1	bool inner_appr = false;
26
27			// No rotation
28	✓✗✓✗	1	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
29
30			// Create the friction cone with rotation and surface normal
31	✓✗	2	crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
32
33	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_R().isApprox(R));
34	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_mu() == mu);
35	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_nf() == nf);
36	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_inner_appr() == inner_appr);
37	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_A().rows()) == nf + 1);
38	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_lb().size()) == nf + 1);
39	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_ub().size()) == nf + 1);
40
41			// With rotation
42	✓✗	1	Eigen::Quaterniond q;
43	✓✗	1	pinocchio::quaternion::uniformRandom(q);
44	✓✗	1	R = q.toRotationMatrix();
45
46			// Create the friction cone
47	✓✗✓✗	1	cone = crocoddyl::FrictionCone(R, mu, nf, inner_appr);
48
49	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_R().isApprox(R));
50	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_mu() == mu);
51	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_nf() == nf);
52	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_inner_appr() == inner_appr);
53	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_A().rows()) == nf + 1);
54	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_lb().size()) == nf + 1);
55	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	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	✓✗	2	crocoddyl::FrictionCone cone_reference(cone);
60
61	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_nf() == cone_reference.get_nf());
62	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_A().isApprox(cone_reference.get_A()));
63	✓✗✓✓	32	for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size());
64			++i) {
65	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	31	BOOST_CHECK(cone.get_ub()[i] == cone_reference.get_ub()[i]);
66	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	31	BOOST_CHECK(cone.get_lb()[i] == cone_reference.get_lb()[i]);
67			}
68	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_R().isApprox(cone_reference.get_R()));
69	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(std::abs(cone.get_mu() - cone_reference.get_mu()) < 1e-9);
70	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_inner_appr() == cone_reference.get_inner_appr());
71	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(std::abs(cone.get_min_nforce() -
72			cone_reference.get_min_nforce()) < 1e-9);
73	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_max_nforce() == cone_reference.get_max_nforce());
74			}
75
76			// Create the friction cone through the copy operator
77			{
78	✓✗	2	crocoddyl::FrictionCone cone_copy;
79	✓✗	1	cone_copy = cone;
80
81	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_nf() == cone_copy.get_nf());
82	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_A().isApprox(cone_copy.get_A()));
83	✓✗✓✓	32	for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size());
84			++i) {
85	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	31	BOOST_CHECK(cone.get_ub()[i] == cone_copy.get_ub()[i]);
86	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	31	BOOST_CHECK(cone.get_lb()[i] == cone_copy.get_lb()[i]);
87			}
88	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_R().isApprox(cone_copy.get_R()));
89	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(std::abs(cone.get_mu() - cone_copy.get_mu()) < 1e-9);
90	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_inner_appr() == cone_copy.get_inner_appr());
91	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(std::abs(cone.get_min_nforce() - cone_copy.get_min_nforce()) <
92			1e-9);
93	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_max_nforce() == cone_copy.get_max_nforce());
94			}
95		1	}
96
97		1	void test_inner_approximation_of_friction_cone() {
98	✓✗✓✗	1	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
99	✓✗	1	double mu = random_real_in_range(0.01, 1.);
100	✓✗	1	std::size_t nf = 2 * random_int_in_range(2, 16);
101		1	bool inner_appr = true;
102	✓✗	2	crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
103	✓✗✓✗ ✓✗	2	const Eigen::VectorXd A_mu = -cone.get_A().col(2);
104	✓✓	33	for (std::size_t i = 0; i < nf; ++i) {
105	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	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	double mu = random_real_in_range(0.01, 1.);
113	✓✗	1	std::size_t nf = 2 * random_int_in_range(2, 16);
114		1	bool inner_appr = false;
115
116			// No rotation
117	✓✗✓✗	1	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
118	✓✗	2	crocoddyl::FrictionCone cone_1(R, mu, nf, inner_appr);
119
120			// With rotation
121	✓✗	1	Eigen::Quaterniond q;
122	✓✗	1	pinocchio::quaternion::uniformRandom(q);
123	✓✗	1	R = q.toRotationMatrix();
124	✓✗	2	crocoddyl::FrictionCone cone_2(R, mu, nf, inner_appr);
125
126	✓✓	6	for (std::size_t i = 0; i < 5; ++i) {
127	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	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	Eigen::Quaterniond q;
135	✓✗	1	pinocchio::quaternion::uniformRandom(q);
136	✓✗	1	Eigen::Matrix3d R = q.toRotationMatrix();
137	✓✗	1	double mu = random_real_in_range(0.01, 1.);
138	✓✗	1	std::size_t nf = 2 * random_int_in_range(2, 16);
139		1	bool inner_appr = false;
140	✓✗	2	crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
141
142			// Create the activation for quadratic barrier
143	✓✗	2	crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub());
144	✓✗	2	crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
145			boost::shared_ptr<crocoddyl::ActivationDataAbstract> data =
146	✓✗	2	activation.createData();
147
148			// Compute the activation value
149	✓✗✓✗ ✓✗✓✗	1	Eigen::Vector3d force = random_real_in_range(0., 100.) * R.col(2);
150	✓✗✓✗	2	Eigen::VectorXd r = cone.get_A() * force;
151	✓✗✓✗	1	activation.calc(data, r);
152
153			// The activation value has to be zero since the force is inside the friction
154			// cone
155	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	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	Eigen::Quaterniond q;
161	✓✗	1	pinocchio::quaternion::uniformRandom(q);
162	✓✗	1	Eigen::Matrix3d R = q.toRotationMatrix();
163	✓✗	1	double mu = random_real_in_range(0.01, 1.);
164	✓✗	1	std::size_t nf = 2 * random_int_in_range(2, 16);
165		1	bool inner_appr = false;
166	✓✗	2	crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
167
168			// Create the activation for quadratic barrier
169	✓✗	2	crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub());
170	✓✗	2	crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
171			boost::shared_ptr<crocoddyl::ActivationDataAbstract> data =
172	✓✗	2	activation.createData();
173
174			// Compute the activation value
175	✓✗✓✗ ✓✗✓✗	1	Eigen::Vector3d force = -random_real_in_range(0., 100.) * R.col(2);
176	✓✗✓✗	2	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	✓✓	33	for (std::size_t i = 0; i < nf; ++i) {
183	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	32	BOOST_CHECK(r(i) > 0.);
184			}
185	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	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	✓✗✓✗	1	activation.calc(data, r);
190	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	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	✓✗✓✗	1	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
196	✓✗	1	double mu = random_real_in_range(0.01, 1.);
197	✓✗	1	std::size_t nf = 2 * random_int_in_range(2, 16);
198		1	bool inner_appr = false;
199	✓✗	2	crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
200
201			// Create the activation for quadratic barrier
202	✓✗	2	crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub());
203	✓✗	2	crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
204			boost::shared_ptr<crocoddyl::ActivationDataAbstract> data =
205	✓✗	2	activation.createData();
206
207			// Compute the activation value
208			Eigen::Vector3d force =
209	✓✗✓✗ ✓✗✓✗	1	-random_real_in_range(0., 100.) * Eigen::Vector3d::UnitX();
210	✓✗✓✗	2	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	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	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	✓✗✓✗	1	activation.calc(data, r);
219	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(data->a_value > 0.);
220		1	}
221
222		1	void register_unit_tests() {
223		1	framework::master_test_suite().add(
224	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_constructor)));
225		1	framework::master_test_suite().add(
226	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_inner_approximation_of_friction_cone)));
227		1	framework::master_test_suite().add(
228	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_A_matrix_with_rotation_change)));
229		1	framework::master_test_suite().add(
230	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_force_along_friction_cone_normal)));
231	✓✗✓✗ ✓✗✓✗	1	framework::master_test_suite().add(BOOST_TEST_CASE(
232			boost::bind(&test_negative_force_along_friction_cone_normal)));
233	✓✗✓✗ ✓✗✓✗	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			}