Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_cop_support.cpp	Lines:	90	93	96.8 %
Date:	2024-02-13 11:12:33	Branches:	266	526	50.6 %


///////////////////////////////////////////////////////////////////////////////
// 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 "unittest_common.hpp"

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

void test_constructor() {
  // Common parameters
  Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
                                        random_real_in_range(0.01, 0.1));

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

  // Create the CoP support support
  crocoddyl::CoPSupport support(R, box);

  BOOST_CHECK(support.get_R().isApprox(R));
  BOOST_CHECK(support.get_box().isApprox(box));
  BOOST_CHECK(static_cast<std::size_t>(support.get_A().rows()) == 4);
  BOOST_CHECK(static_cast<std::size_t>(support.get_lb().size()) == 4);
  BOOST_CHECK(static_cast<std::size_t>(support.get_ub().size()) == 4);

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

  // Create the wrench support
  support = crocoddyl::CoPSupport(R, box);

  BOOST_CHECK(support.get_R().isApprox(R));
  BOOST_CHECK(support.get_box().isApprox(box));
  BOOST_CHECK(static_cast<std::size_t>(support.get_A().rows()) == 4);
  BOOST_CHECK(static_cast<std::size_t>(support.get_lb().size()) == 4);
  BOOST_CHECK(static_cast<std::size_t>(support.get_ub().size()) == 4);

  // Create the wrench support from a reference
  {
    crocoddyl::CoPSupport support_from_reference(support);

    BOOST_CHECK(support.get_R().isApprox(support_from_reference.get_R()));
    BOOST_CHECK(support.get_box().isApprox(support_from_reference.get_box()));
    BOOST_CHECK(support.get_A().isApprox(support_from_reference.get_A()));
    for (std::size_t i = 0;
         i < static_cast<std::size_t>(support.get_ub().size()); ++i) {
      BOOST_CHECK(support.get_ub()[i] == support_from_reference.get_ub()[i]);
      BOOST_CHECK(support.get_lb()[i] == support_from_reference.get_lb()[i]);
    }
  }

  // Create the wrench support through the copy operator
  {
    crocoddyl::CoPSupport support_from_copy;
    support_from_copy = support;

    BOOST_CHECK(support.get_R().isApprox(support_from_copy.get_R()));
    BOOST_CHECK(support.get_box().isApprox(support_from_copy.get_box()));
    BOOST_CHECK(support.get_A().isApprox(support_from_copy.get_A()));
    for (std::size_t i = 0;
         i < static_cast<std::size_t>(support.get_ub().size()); ++i) {
      BOOST_CHECK(support.get_ub()[i] == support_from_copy.get_ub()[i]);
      BOOST_CHECK(support.get_lb()[i] == support_from_copy.get_lb()[i]);
    }
  }
}

void test_A_matrix_with_rotation_change() {
  // Common parameters
  Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
                                        random_real_in_range(0.01, 0.1));

  // No rotation
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  crocoddyl::CoPSupport support_1(R, box);

  // With rotation
  Eigen::Quaterniond q;
  pinocchio::quaternion::uniformRandom(q);
  R = q.toRotationMatrix();
  crocoddyl::CoPSupport support_2(R, box);

  for (std::size_t i = 0; i < 4; ++i) {
    BOOST_CHECK((support_1.get_A().row(i).head(3) -
                 support_2.get_A().row(i).head(3) * R)
                    .isZero(1e-9));
  }
}

void test_cop_within_the_support_region() {
  // Create the CoP support with a random orientation
  Eigen::Quaterniond q;
  pinocchio::quaternion::uniformRandom(q);
  Eigen::Matrix3d R = q.toRotationMatrix();
  Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
                                        random_real_in_range(0.01, 0.1));
  crocoddyl::CoPSupport support(R, box);

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

  // Compute the activation value with a force along the contact normal
  Eigen::VectorXd wrench(6);
  wrench.setZero();
  wrench.head(3) = random_real_in_range(0., 100.) * R.col(2);
  Eigen::VectorXd r = support.get_A() * wrench;
  activation.calc(data, r);
  BOOST_CHECK(data->a_value == 0.);

  // Create the CoP support with no rotation
  R = Eigen::Matrix3d::Identity();
  support = crocoddyl::CoPSupport(R, box);

  // Compute the activation value with a small enough torque in X
  wrench.setZero();
  wrench(5) = random_real_in_range(0., 100.);
  wrench(0) = (box(0) - 1e-9) * wrench(5) / 2.;
  r = support.get_A() * wrench;
  activation.calc(data, r);
  BOOST_CHECK(data->a_value == 0.);

  // Compute the activation value with a small enough torque in Y
  wrench.setZero();
  wrench(5) = random_real_in_range(0., 100.);
  wrench(1) = (box(1) - 1e-9) * wrench(5) / 2.;
  r = support.get_A() * wrench;
  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_A_matrix_with_rotation_change)));
  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_cop_within_the_support_region)));
}

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 "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			Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
24	✓✗✓✗ ✓✗	1	random_real_in_range(0.01, 0.1));
25
26			// No rotation
27	✓✗✓✗	1	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
28
29			// Create the CoP support support
30	✓✗	2	crocoddyl::CoPSupport support(R, box);
31
32	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(support.get_R().isApprox(R));
33	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(support.get_box().isApprox(box));
34	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(support.get_A().rows()) == 4);
35	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(support.get_lb().size()) == 4);
36	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(support.get_ub().size()) == 4);
37
38			// With rotation
39	✓✗	1	Eigen::Quaterniond q;
40	✓✗	1	pinocchio::quaternion::uniformRandom(q);
41	✓✗	1	R = q.toRotationMatrix();
42
43			// Create the wrench support
44	✓✗✓✗	1	support = crocoddyl::CoPSupport(R, box);
45
46	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(support.get_R().isApprox(R));
47	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(support.get_box().isApprox(box));
48	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(support.get_A().rows()) == 4);
49	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(support.get_lb().size()) == 4);
50	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(static_cast<std::size_t>(support.get_ub().size()) == 4);
51
52			// Create the wrench support from a reference
53			{
54	✓✗	2	crocoddyl::CoPSupport support_from_reference(support);
55
56	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(support.get_R().isApprox(support_from_reference.get_R()));
57	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(support.get_box().isApprox(support_from_reference.get_box()));
58	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(support.get_A().isApprox(support_from_reference.get_A()));
59		5	for (std::size_t i = 0;
60	✓✗✓✓	5	i < static_cast<std::size_t>(support.get_ub().size()); ++i) {
61	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	4	BOOST_CHECK(support.get_ub()[i] == support_from_reference.get_ub()[i]);
62	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	4	BOOST_CHECK(support.get_lb()[i] == support_from_reference.get_lb()[i]);
63			}
64			}
65
66			// Create the wrench support through the copy operator
67			{
68	✓✗	2	crocoddyl::CoPSupport support_from_copy;
69	✓✗	1	support_from_copy = support;
70
71	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(support.get_R().isApprox(support_from_copy.get_R()));
72	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(support.get_box().isApprox(support_from_copy.get_box()));
73	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	1	BOOST_CHECK(support.get_A().isApprox(support_from_copy.get_A()));
74		5	for (std::size_t i = 0;
75	✓✗✓✓	5	i < static_cast<std::size_t>(support.get_ub().size()); ++i) {
76	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	4	BOOST_CHECK(support.get_ub()[i] == support_from_copy.get_ub()[i]);
77	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	4	BOOST_CHECK(support.get_lb()[i] == support_from_copy.get_lb()[i]);
78			}
79			}
80		1	}
81
82		1	void test_A_matrix_with_rotation_change() {
83			// Common parameters
84			Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
85	✓✗✓✗ ✓✗	1	random_real_in_range(0.01, 0.1));
86
87			// No rotation
88	✓✗✓✗	1	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
89	✓✗	2	crocoddyl::CoPSupport support_1(R, box);
90
91			// With rotation
92	✓✗	1	Eigen::Quaterniond q;
93	✓✗	1	pinocchio::quaternion::uniformRandom(q);
94	✓✗	1	R = q.toRotationMatrix();
95	✓✗	2	crocoddyl::CoPSupport support_2(R, box);
96
97	✓✓	5	for (std::size_t i = 0; i < 4; ++i) {
98	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	4	BOOST_CHECK((support_1.get_A().row(i).head(3) -
99			support_2.get_A().row(i).head(3) * R)
100			.isZero(1e-9));
101			}
102		1	}
103
104		1	void test_cop_within_the_support_region() {
105			// Create the CoP support with a random orientation
106	✓✗	1	Eigen::Quaterniond q;
107	✓✗	1	pinocchio::quaternion::uniformRandom(q);
108	✓✗	1	Eigen::Matrix3d R = q.toRotationMatrix();
109			Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
110	✓✗✓✗ ✓✗	1	random_real_in_range(0.01, 0.1));
111	✓✗	2	crocoddyl::CoPSupport support(R, box);
112
113			// Create the activation for quadratic barrier
114	✓✗✓✗ ✓✗	3	crocoddyl::ActivationBounds bounds(support.get_lb(), support.get_ub());
115	✓✗	2	crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
116			boost::shared_ptr<crocoddyl::ActivationDataAbstract> data =
117	✓✗	2	activation.createData();
118
119			// Compute the activation value with a force along the contact normal
120	✓✗	2	Eigen::VectorXd wrench(6);
121	✓✗	1	wrench.setZero();
122	✓✗✓✗ ✓✗✓✗ ✓✗	1	wrench.head(3) = random_real_in_range(0., 100.) * R.col(2);
123	✓✗✓✗	2	Eigen::VectorXd r = support.get_A() * wrench;
124	✓✗✓✗	1	activation.calc(data, r);
125	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(data->a_value == 0.);
126
127			// Create the CoP support with no rotation
128	✓✗✓✗	1	R = Eigen::Matrix3d::Identity();
129	✓✗✓✗	1	support = crocoddyl::CoPSupport(R, box);
130
131			// Compute the activation value with a small enough torque in X
132	✓✗	1	wrench.setZero();
133	✓✗✓✗	1	wrench(5) = random_real_in_range(0., 100.);
134	✓✗✓✗ ✓✗	1	wrench(0) = (box(0) - 1e-9) * wrench(5) / 2.;
135	✓✗✓✗	1	r = support.get_A() * wrench;
136	✓✗✓✗	1	activation.calc(data, r);
137	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(data->a_value == 0.);
138
139			// Compute the activation value with a small enough torque in Y
140	✓✗	1	wrench.setZero();
141	✓✗✓✗	1	wrench(5) = random_real_in_range(0., 100.);
142	✓✗✓✗ ✓✗	1	wrench(1) = (box(1) - 1e-9) * wrench(5) / 2.;
143	✓✗✓✗	1	r = support.get_A() * wrench;
144	✓✗✓✗	1	activation.calc(data, r);
145	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK(data->a_value == 0.);
146		1	}
147
148		1	void register_unit_tests() {
149		1	framework::master_test_suite().add(
150	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_constructor)));
151		1	framework::master_test_suite().add(
152	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_A_matrix_with_rotation_change)));
153		1	framework::master_test_suite().add(
154	✓✗✓✗ ✓✗✓✗	1	BOOST_TEST_CASE(boost::bind(&test_cop_within_the_support_region)));
155		1	}
156
157		1	bool init_function() {
158		1	register_unit_tests();
159		1	return true;
160			}
161
162		1	int main(int argc, char* argv[]) {
163		1	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
164			}