Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_wrench_cone.cpp	Lines:	161	167	96.4 %
Date:	2024-02-13 11:12:33	Branches:	492	966	50.9 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 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/cop-support.hpp"
#include "crocoddyl/multibody/friction-cone.hpp"
#include "crocoddyl/multibody/wrench-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.);
  Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
                                        random_real_in_range(0.01, 0.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 wrench cone
  crocoddyl::WrenchCone cone(R, mu, box, 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_box().isApprox(box));
  BOOST_CHECK(cone.get_inner_appr() == inner_appr);
  BOOST_CHECK(static_cast<std::size_t>(cone.get_A().rows()) == nf + 13);
  BOOST_CHECK(static_cast<std::size_t>(cone.get_lb().size()) == nf + 13);
  BOOST_CHECK(static_cast<std::size_t>(cone.get_ub().size()) == nf + 13);

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

  // Create the wrench cone
  cone = crocoddyl::WrenchCone(R, mu, box, 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_box().isApprox(box));
  BOOST_CHECK(cone.get_inner_appr() == inner_appr);
  BOOST_CHECK(static_cast<std::size_t>(cone.get_A().rows()) == nf + 13);
  BOOST_CHECK(static_cast<std::size_t>(cone.get_lb().size()) == nf + 13);
  BOOST_CHECK(static_cast<std::size_t>(cone.get_ub().size()) == nf + 13);

  // Create the wrench cone from a reference
  {
    crocoddyl::WrenchCone 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(cone.get_box().isApprox(cone_reference.get_box()));
    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 wrench cone through the copy operator
  {
    crocoddyl::WrenchCone 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(cone.get_box().isApprox(cone_copy.get_box()));
    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_against_friction_cone() {
  // Common parameters
  Eigen::Quaterniond q;
  pinocchio::quaternion::uniformRandom(q);
  Eigen::Matrix3d R = q.toRotationMatrix();
  double mu = random_real_in_range(0.01, 1.);
  Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
                                        random_real_in_range(0.01, 0.1));
  std::size_t nf = 2 * random_int_in_range(2, 16);
  bool inner_appr = true;

  // Create wrench and friction cone
  crocoddyl::WrenchCone wrench_cone(R, mu, box, nf, inner_appr, 0., 100.);
  crocoddyl::FrictionCone friction_cone(R, mu, nf, inner_appr, 0., 100.);

  BOOST_CHECK((wrench_cone.get_R() - friction_cone.get_R()).isZero(1e-9));
  BOOST_CHECK(wrench_cone.get_mu() == friction_cone.get_mu());
  BOOST_CHECK(wrench_cone.get_nf() == friction_cone.get_nf());
  for (std::size_t i = 0; i < nf + 1; ++i) {
    for (std::size_t j = 0; j < 3; ++j) {
      BOOST_CHECK(wrench_cone.get_A().row(i)[j] ==
                  friction_cone.get_A().row(i)[j]);
    }
  }
  for (std::size_t i = 0; i < nf + 1; ++i) {
    BOOST_CHECK(wrench_cone.get_ub()[i] == friction_cone.get_ub()[i]);
    BOOST_CHECK(wrench_cone.get_lb()[i] == friction_cone.get_lb()[i]);
  }
}

void test_against_cop_support() {
  // Common parameters
  Eigen::Quaterniond q;
  pinocchio::quaternion::uniformRandom(q);
  Eigen::Matrix3d R = q.toRotationMatrix();
  double mu = random_real_in_range(0.01, 1.);
  Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
                                        random_real_in_range(0.01, 0.1));
  std::size_t nf = 2 * random_int_in_range(2, 16);
  bool inner_appr = true;

  // Create wrench and friction cone
  crocoddyl::WrenchCone wrench_cone(R, mu, box, nf, inner_appr, 0., 100.);
  crocoddyl::CoPSupport cop_support(R, box);

  BOOST_CHECK((wrench_cone.get_R() - cop_support.get_R()).isZero(1e-9));
  for (std::size_t i = 0; i < 4; ++i) {
    for (std::size_t j = 0; j < 6; ++j) {
      BOOST_CHECK(wrench_cone.get_A().row(nf + i + 1)[j] ==
                  cop_support.get_A().row(i)[j]);
    }
  }
  for (std::size_t i = 0; i < 4; ++i) {
    BOOST_CHECK(wrench_cone.get_ub()[i + nf + 1] == cop_support.get_ub()[i]);
    BOOST_CHECK(wrench_cone.get_lb()[i + nf + 1] == cop_support.get_lb()[i]);
  }
}

void test_force_along_wrench_cone_normal() {
  // Create the wrench cone
  Eigen::Quaterniond q;
  pinocchio::quaternion::uniformRandom(q);
  Eigen::Matrix3d R = q.toRotationMatrix();
  double mu = random_real_in_range(0.01, 1.);
  Eigen::Vector2d cone_box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
                                             random_real_in_range(0.01, 0.1));
  crocoddyl::WrenchCone cone(R, mu, cone_box);

  // 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::VectorXd wrench(6);
  wrench.setZero();
  wrench.head(3) = random_real_in_range(0., 100.) * R.col(2);
  Eigen::VectorXd r = cone.get_A() * wrench;
  activation.calc(data, r);

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

void test_negative_force_along_wrench_cone_normal() {
  // Create the wrench cone
  Eigen::Quaterniond q;
  pinocchio::quaternion::uniformRandom(q);
  Eigen::Matrix3d R = q.toRotationMatrix();
  double mu = random_real_in_range(0.01, 1.);
  Eigen::Vector2d cone_box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
                                             random_real_in_range(0.01, 0.1));
  crocoddyl::WrenchCone cone(R, mu, cone_box);

  // 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::VectorXd wrench(6);
  wrench.setZero();
  wrench.head(3) = -random_real_in_range(0., 100.) * R.col(2);
  Eigen::VectorXd r = cone.get_A() * wrench;
  activation.calc(data, r);

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

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

void test_force_parallel_to_wrench_cone_normal() {
  // Create the wrench cone
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  double mu = random_real_in_range(0.01, 1.);
  Eigen::Vector2d cone_box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
                                             random_real_in_range(0.01, 0.1));
  crocoddyl::WrenchCone cone(R, mu, cone_box);

  // 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();

  Eigen::VectorXd wrench(6);
  wrench.setZero();
  wrench.head(3) = -random_real_in_range(0., 100.) * Eigen::Vector3d::UnitX();
  Eigen::VectorXd r = cone.get_A() * wrench;

  // The activation value has to be positive since the force is outside the
  // wrench 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_against_friction_cone)));
  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_against_cop_support)));
  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_force_along_wrench_cone_normal)));
  framework::master_test_suite().add(BOOST_TEST_CASE(
      boost::bind(&test_negative_force_along_wrench_cone_normal)));
  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_force_parallel_to_wrench_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) 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/cop-support.hpp"
16			#include "crocoddyl/multibody/friction-cone.hpp"
17			#include "crocoddyl/multibody/wrench-cone.hpp"
18			#include "unittest_common.hpp"
19
20			using namespace boost::unit_test;
21			using namespace crocoddyl::unittest;
22
23		1	void test_constructor() {
24			// Common parameters
25	✓✗	1	double mu = random_real_in_range(0.01, 1.);
26			Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
27	✓✗✓✗ ✓✗	1	random_real_in_range(0.01, 0.1));
28	✓✗	1	std::size_t nf = 2 * random_int_in_range(2, 16);
29		1	bool inner_appr = false;
30
31			// No rotation
32	✓✗✓✗	1	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
33
34			// Create the wrench cone
35	✓✗	2	crocoddyl::WrenchCone cone(R, mu, box, nf, inner_appr);
36
37	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_R().isApprox(R));
38	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_mu() == mu);
39	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_nf() == nf);
40	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_box().isApprox(box));
41	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_inner_appr() == inner_appr);
42	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_A().rows()) == nf + 13);
43	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_lb().size()) == nf + 13);
44	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_ub().size()) == nf + 13);
45
46			// With rotation
47	✓✗	1	Eigen::Quaterniond q;
48	✓✗	1	pinocchio::quaternion::uniformRandom(q);
49	✓✗	1	R = q.toRotationMatrix();
50
51			// Create the wrench cone
52	✓✗✓✗	1	cone = crocoddyl::WrenchCone(R, mu, box, nf, inner_appr);
53
54	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_R().isApprox(R));
55	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_mu() == mu);
56	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_nf() == nf);
57	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_box().isApprox(box));
58	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_inner_appr() == inner_appr);
59	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_A().rows()) == nf + 13);
60	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_lb().size()) == nf + 13);
61	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(cone.get_ub().size()) == nf + 13);
62
63			// Create the wrench cone from a reference
64			{
65	✓✗	2	crocoddyl::WrenchCone cone_reference(cone);
66
67	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_nf() == cone_reference.get_nf());
68	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_A().isApprox(cone_reference.get_A()));
69	✓✗✓✓	44	for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size());
70			++i) {
71	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	43	BOOST_CHECK(cone.get_ub()[i] == cone_reference.get_ub()[i]);
72	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	43	BOOST_CHECK(cone.get_lb()[i] == cone_reference.get_lb()[i]);
73			}
74	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_R().isApprox(cone_reference.get_R()));
75	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_box().isApprox(cone_reference.get_box()));
76	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(std::abs(cone.get_mu() - cone_reference.get_mu()) < 1e-9);
77	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_inner_appr() == cone_reference.get_inner_appr());
78	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(std::abs(cone.get_min_nforce() -
79			cone_reference.get_min_nforce()) < 1e-9);
80	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_max_nforce() == cone_reference.get_max_nforce());
81			}
82
83			// Create the wrench cone through the copy operator
84			{
85	✓✗	2	crocoddyl::WrenchCone cone_copy;
86	✓✗	1	cone_copy = cone;
87
88	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_nf() == cone_copy.get_nf());
89	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_A().isApprox(cone_copy.get_A()));
90	✓✗✓✓	44	for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size());
91			++i) {
92	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	43	BOOST_CHECK(cone.get_ub()[i] == cone_copy.get_ub()[i]);
93	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	43	BOOST_CHECK(cone.get_lb()[i] == cone_copy.get_lb()[i]);
94			}
95	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_R().isApprox(cone_copy.get_R()));
96	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(cone.get_box().isApprox(cone_copy.get_box()));
97	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(std::abs(cone.get_mu() - cone_copy.get_mu()) < 1e-9);
98	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_inner_appr() == cone_copy.get_inner_appr());
99	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(std::abs(cone.get_min_nforce() - cone_copy.get_min_nforce()) <
100			1e-9);
101	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(cone.get_max_nforce() == cone_copy.get_max_nforce());
102			}
103		1	}
104
105		1	void test_against_friction_cone() {
106			// Common parameters
107	✓✗	1	Eigen::Quaterniond q;
108	✓✗	1	pinocchio::quaternion::uniformRandom(q);
109	✓✗	1	Eigen::Matrix3d R = q.toRotationMatrix();
110	✓✗	1	double mu = random_real_in_range(0.01, 1.);
111			Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
112	✓✗✓✗ ✓✗	1	random_real_in_range(0.01, 0.1));
113	✓✗	1	std::size_t nf = 2 * random_int_in_range(2, 16);
114		1	bool inner_appr = true;
115
116			// Create wrench and friction cone
117	✓✗	2	crocoddyl::WrenchCone wrench_cone(R, mu, box, nf, inner_appr, 0., 100.);
118	✓✗	2	crocoddyl::FrictionCone friction_cone(R, mu, nf, inner_appr, 0., 100.);
119
120	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK((wrench_cone.get_R() - friction_cone.get_R()).isZero(1e-9));
121	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(wrench_cone.get_mu() == friction_cone.get_mu());
122	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(wrench_cone.get_nf() == friction_cone.get_nf());
123	✓✓	10	for (std::size_t i = 0; i < nf + 1; ++i) {
124	✓✓	36	for (std::size_t j = 0; j < 3; ++j) {
125	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	27	BOOST_CHECK(wrench_cone.get_A().row(i)[j] ==
126			friction_cone.get_A().row(i)[j]);
127			}
128			}
129	✓✓	10	for (std::size_t i = 0; i < nf + 1; ++i) {
130	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	9	BOOST_CHECK(wrench_cone.get_ub()[i] == friction_cone.get_ub()[i]);
131	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	9	BOOST_CHECK(wrench_cone.get_lb()[i] == friction_cone.get_lb()[i]);
132			}
133		1	}
134
135		1	void test_against_cop_support() {
136			// Common parameters
137	✓✗	1	Eigen::Quaterniond q;
138	✓✗	1	pinocchio::quaternion::uniformRandom(q);
139	✓✗	1	Eigen::Matrix3d R = q.toRotationMatrix();
140	✓✗	1	double mu = random_real_in_range(0.01, 1.);
141			Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
142	✓✗✓✗ ✓✗	1	random_real_in_range(0.01, 0.1));
143	✓✗	1	std::size_t nf = 2 * random_int_in_range(2, 16);
144		1	bool inner_appr = true;
145
146			// Create wrench and friction cone
147	✓✗	2	crocoddyl::WrenchCone wrench_cone(R, mu, box, nf, inner_appr, 0., 100.);
148	✓✗	2	crocoddyl::CoPSupport cop_support(R, box);
149
150	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK((wrench_cone.get_R() - cop_support.get_R()).isZero(1e-9));
151	✓✓	5	for (std::size_t i = 0; i < 4; ++i) {
152	✓✓	28	for (std::size_t j = 0; j < 6; ++j) {
153	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	24	BOOST_CHECK(wrench_cone.get_A().row(nf + i + 1)[j] ==
154			cop_support.get_A().row(i)[j]);
155			}
156			}
157	✓✓	5	for (std::size_t i = 0; i < 4; ++i) {
158	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	4	BOOST_CHECK(wrench_cone.get_ub()[i + nf + 1] == cop_support.get_ub()[i]);
159	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	4	BOOST_CHECK(wrench_cone.get_lb()[i + nf + 1] == cop_support.get_lb()[i]);
160			}
161		1	}
162
163		1	void test_force_along_wrench_cone_normal() {
164			// Create the wrench cone
165	✓✗	1	Eigen::Quaterniond q;
166	✓✗	1	pinocchio::quaternion::uniformRandom(q);
167	✓✗	1	Eigen::Matrix3d R = q.toRotationMatrix();
168	✓✗	1	double mu = random_real_in_range(0.01, 1.);
169			Eigen::Vector2d cone_box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
170	✓✗✓✗ ✓✗	1	random_real_in_range(0.01, 0.1));
171	✓✗	2	crocoddyl::WrenchCone cone(R, mu, cone_box);
172
173			// Create the activation for quadratic barrier
174	✓✗	2	crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub());
175	✓✗	2	crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
176			boost::shared_ptr<crocoddyl::ActivationDataAbstract> data =
177	✓✗	2	activation.createData();
178
179			// Compute the activation value
180	✓✗	2	Eigen::VectorXd wrench(6);
181	✓✗	1	wrench.setZero();
182	✓✗✓✗ ✓✗✓✗ ✓✗	1	wrench.head(3) = random_real_in_range(0., 100.) * R.col(2);
183	✓✗✓✗	2	Eigen::VectorXd r = cone.get_A() * wrench;
184	✓✗✓✗	1	activation.calc(data, r);
185
186			// The activation value has to be zero since the wrench is inside the wrench
187			// cone
188	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(data->a_value == 0.);
189		1	}
190
191		1	void test_negative_force_along_wrench_cone_normal() {
192			// Create the wrench cone
193	✓✗	1	Eigen::Quaterniond q;
194	✓✗	1	pinocchio::quaternion::uniformRandom(q);
195	✓✗	1	Eigen::Matrix3d R = q.toRotationMatrix();
196	✓✗	1	double mu = random_real_in_range(0.01, 1.);
197			Eigen::Vector2d cone_box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
198	✓✗✓✗ ✓✗	1	random_real_in_range(0.01, 0.1));
199	✓✗	2	crocoddyl::WrenchCone cone(R, mu, cone_box);
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	✓✗	2	Eigen::VectorXd wrench(6);
209	✓✗	1	wrench.setZero();
210	✓✗✓✗ ✓✗✓✗ ✓✗	1	wrench.head(3) = -random_real_in_range(0., 100.) * R.col(2);
211	✓✗✓✗	2	Eigen::VectorXd r = cone.get_A() * wrench;
212	✓✗✓✗	1	activation.calc(data, r);
213
214			// The first nf elements of the residual has to be positive since the force is
215			// outside the wrench cone. Additionally, the last value has to be equals to
216			// the force norm but with negative value since the wrench is aligned and in
217			// opposite direction to the wrench cone orientation
218	✓✓	5	for (std::size_t i = 0; i < cone.get_nf(); ++i) {
219	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	4	BOOST_CHECK(r(i) > 0.);
220			}
221
222			// The activation value has to be positive since the wrench is outside the
223			// wrench cone
224	✓✗✓✗	1	activation.calc(data, r);
225	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(data->a_value > 0.);
226		1	}
227
228		1	void test_force_parallel_to_wrench_cone_normal() {
229			// Create the wrench cone
230	✓✗✓✗	1	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
231	✓✗	1	double mu = random_real_in_range(0.01, 1.);
232			Eigen::Vector2d cone_box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
233	✓✗✓✗ ✓✗	1	random_real_in_range(0.01, 0.1));
234	✓✗	2	crocoddyl::WrenchCone cone(R, mu, cone_box);
235
236			// Create the activation for quadratic barrier
237	✓✗	2	crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub());
238	✓✗	2	crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
239			boost::shared_ptr<crocoddyl::ActivationDataAbstract> data =
240	✓✗	2	activation.createData();
241
242	✓✗	2	Eigen::VectorXd wrench(6);
243	✓✗	1	wrench.setZero();
244	✓✗✓✗ ✓✗✓✗ ✓✗	1	wrench.head(3) = -random_real_in_range(0., 100.) * Eigen::Vector3d::UnitX();
245	✓✗✓✗	2	Eigen::VectorXd r = cone.get_A() * wrench;
246
247			// The activation value has to be positive since the force is outside the
248			// wrench cone
249	✓✗✓✗	1	activation.calc(data, r);
250	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(data->a_value > 0.);
251		1	}
252
253		1	void register_unit_tests() {
254		1	framework::master_test_suite().add(
255	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_constructor)));
256		1	framework::master_test_suite().add(
257	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_against_friction_cone)));
258		1	framework::master_test_suite().add(
259	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_against_cop_support)));
260		1	framework::master_test_suite().add(
261	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_force_along_wrench_cone_normal)));
262	✓✗✓✗ ✓✗✓✗	1	framework::master_test_suite().add(BOOST_TEST_CASE(
263			boost::bind(&test_negative_force_along_wrench_cone_normal)));
264		1	framework::master_test_suite().add(
265	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_force_parallel_to_wrench_cone_normal)));
266		1	}
267
268		1	bool init_function() {
269		1	register_unit_tests();
270		1	return true;
271			}
272
273		1	int main(int argc, char* argv[]) {
274		1	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
275			}