Directory: | ./ |
---|---|
File: | unittest/test_cop_support.cpp |
Date: | 2025-01-30 11:01:55 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 92 | 95 | 96.8% |
Branches: | 258 | 510 | 50.6% |
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 |
3/6✓ 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.
|
1 | random_real_in_range(0.01, 0.1)); |
25 | |||
26 | // No rotation | ||
27 |
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(); |
28 | |||
29 | // Create the CoP support support | ||
30 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::CoPSupport support(R, box); |
31 | |||
32 |
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(support.get_R().isApprox(R)); |
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(support.get_box().isApprox(box)); |
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 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>(support.get_A().rows()) == 4); |
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 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>(support.get_lb().size()) == 4); |
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 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>(support.get_ub().size()) == 4); |
37 | |||
38 | // With rotation | ||
39 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Quaterniond q; |
40 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::quaternion::uniformRandom(q); |
41 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | R = q.toRotationMatrix(); |
42 | |||
43 | // Create the wrench support | ||
44 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | support = crocoddyl::CoPSupport(R, box); |
45 | |||
46 |
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(support.get_R().isApprox(R)); |
47 |
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(support.get_box().isApprox(box)); |
48 |
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>(support.get_A().rows()) == 4); |
49 |
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>(support.get_lb().size()) == 4); |
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 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>(support.get_ub().size()) == 4); |
51 | |||
52 | // Create the wrench support from a reference | ||
53 | { | ||
54 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::CoPSupport support_from_reference(support); |
55 | |||
56 |
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(support.get_R().isApprox(support_from_reference.get_R())); |
57 |
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(support.get_box().isApprox(support_from_reference.get_box())); |
58 |
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(support.get_A().isApprox(support_from_reference.get_A())); |
59 | 5 | for (std::size_t i = 0; | |
60 |
2/2✓ Branch 2 taken 4 times.
✓ Branch 3 taken 1 times.
|
5 | i < static_cast<std::size_t>(support.get_ub().size()); ++i) { |
61 |
8/16✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 4 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 4 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 4 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 4 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 4 times.
|
4 | BOOST_CHECK(support.get_ub()[i] == support_from_reference.get_ub()[i]); |
62 |
8/16✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 4 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 4 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 4 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 4 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 4 times.
|
4 | BOOST_CHECK(support.get_lb()[i] == support_from_reference.get_lb()[i]); |
63 | } | ||
64 | 1 | } | |
65 | |||
66 | // Create the wrench support through the copy operator | ||
67 | { | ||
68 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::CoPSupport support_from_copy; |
69 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | support_from_copy = support; |
70 | |||
71 |
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(support.get_R().isApprox(support_from_copy.get_R())); |
72 |
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(support.get_box().isApprox(support_from_copy.get_box())); |
73 |
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(support.get_A().isApprox(support_from_copy.get_A())); |
74 | 5 | for (std::size_t i = 0; | |
75 |
2/2✓ Branch 2 taken 4 times.
✓ Branch 3 taken 1 times.
|
5 | i < static_cast<std::size_t>(support.get_ub().size()); ++i) { |
76 |
8/16✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 4 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 4 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 4 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 4 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 4 times.
|
4 | BOOST_CHECK(support.get_ub()[i] == support_from_copy.get_ub()[i]); |
77 |
8/16✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 4 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 4 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 4 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 4 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 4 times.
|
4 | BOOST_CHECK(support.get_lb()[i] == support_from_copy.get_lb()[i]); |
78 | } | ||
79 | 1 | } | |
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 |
3/6✓ 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.
|
1 | random_real_in_range(0.01, 0.1)); |
86 | |||
87 | // No rotation | ||
88 |
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(); |
89 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::CoPSupport support_1(R, box); |
90 | |||
91 | // With rotation | ||
92 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Quaterniond q; |
93 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::quaternion::uniformRandom(q); |
94 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | R = q.toRotationMatrix(); |
95 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::CoPSupport support_2(R, box); |
96 | |||
97 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
|
5 | for (std::size_t i = 0; i < 4; ++i) { |
98 |
13/26✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 4 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 4 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 4 times.
✗ Branch 22 not taken.
✓ Branch 25 taken 4 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 4 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 4 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 4 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 4 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 4 times.
✗ Branch 41 not taken.
✗ Branch 45 not taken.
✓ Branch 46 taken 4 times.
|
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/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Quaterniond q; |
107 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::quaternion::uniformRandom(q); |
108 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Matrix3d R = q.toRotationMatrix(); |
109 | ✗ | Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1), | |
110 |
3/6✓ 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.
|
1 | random_real_in_range(0.01, 0.1)); |
111 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::CoPSupport support(R, box); |
112 | |||
113 | // Create the activation for quadratic barrier | ||
114 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
2 | crocoddyl::ActivationBounds bounds(support.get_lb(), support.get_ub()); |
115 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::ActivationModelQuadraticBarrier activation(bounds); |
116 | std::shared_ptr<crocoddyl::ActivationDataAbstract> data = | ||
117 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | activation.createData(); |
118 | |||
119 | // Compute the activation value with a force along the contact normal | ||
120 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::VectorXd wrench(6); |
121 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | wrench.setZero(); |
122 |
5/10✓ 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.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
1 | wrench.head(3) = random_real_in_range(0., 100.) * R.col(2); |
123 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | Eigen::VectorXd r = support.get_A() * wrench; |
124 |
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); |
125 |
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.); |
126 | |||
127 | // Create the CoP support with no rotation | ||
128 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | R = Eigen::Matrix3d::Identity(); |
129 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | support = crocoddyl::CoPSupport(R, box); |
130 | |||
131 | // Compute the activation value with a small enough torque in X | ||
132 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | wrench.setZero(); |
133 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | wrench(5) = random_real_in_range(0., 100.); |
134 |
3/6✓ 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.
|
1 | wrench(0) = (box(0) - 1e-9) * wrench(5) / 2.; |
135 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | r = support.get_A() * wrench; |
136 |
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); |
137 |
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.); |
138 | |||
139 | // Compute the activation value with a small enough torque in Y | ||
140 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | wrench.setZero(); |
141 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | wrench(5) = random_real_in_range(0., 100.); |
142 |
3/6✓ 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.
|
1 | wrench(1) = (box(1) - 1e-9) * wrench(5) / 2.; |
143 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | r = support.get_A() * wrench; |
144 |
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); |
145 |
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.); |
146 | 1 | } | |
147 | |||
148 | 1 | void register_unit_tests() { | |
149 | 1 | framework::master_test_suite().add( | |
150 |
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))); |
151 | 1 | framework::master_test_suite().add( | |
152 |
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))); |
153 | 1 | framework::master_test_suite().add( | |
154 |
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_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 | } | ||
165 |