Directory: | ./ |
---|---|
File: | unittest/test_wrench_cone.cpp |
Date: | 2025-01-16 08:47:40 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 163 | 169 | 96.4% |
Branches: | 484 | 950 | 50.9% |
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/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
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 |
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)); |
28 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | std::size_t nf = 2 * random_int_in_range(2, 16); |
29 | 1 | bool inner_appr = false; | |
30 | |||
31 | // No rotation | ||
32 |
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(); |
33 | |||
34 | // Create the wrench cone | ||
35 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | crocoddyl::WrenchCone cone(R, mu, box, nf, inner_appr); |
36 | |||
37 |
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)); |
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 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); |
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 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); |
40 |
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_box().isApprox(box)); |
41 |
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); |
42 |
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 + 13); |
43 |
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 + 13); |
44 |
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 + 13); |
45 | |||
46 | // With rotation | ||
47 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Quaterniond q; |
48 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::quaternion::uniformRandom(q); |
49 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | R = q.toRotationMatrix(); |
50 | |||
51 | // Create the wrench cone | ||
52 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | cone = crocoddyl::WrenchCone(R, mu, box, nf, inner_appr); |
53 | |||
54 |
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)); |
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 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); |
56 |
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); |
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 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_box().isApprox(box)); |
58 |
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); |
59 |
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 + 13); |
60 |
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 + 13); |
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(static_cast<std::size_t>(cone.get_ub().size()) == nf + 13); |
62 | |||
63 | // Create the wrench cone from a reference | ||
64 | { | ||
65 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::WrenchCone cone_reference(cone); |
66 | |||
67 |
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()); |
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_A().isApprox(cone_reference.get_A())); |
69 |
2/2✓ Branch 2 taken 43 times.
✓ Branch 3 taken 1 times.
|
44 | for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size()); |
70 | ++i) { | ||
71 |
8/16✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 43 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 43 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 43 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 43 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 43 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 43 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 43 times.
|
43 | BOOST_CHECK(cone.get_ub()[i] == cone_reference.get_ub()[i]); |
72 |
8/16✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 43 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 43 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 43 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 43 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 43 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 43 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 43 times.
|
43 | BOOST_CHECK(cone.get_lb()[i] == cone_reference.get_lb()[i]); |
73 | } | ||
74 |
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())); |
75 |
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_box().isApprox(cone_reference.get_box())); |
76 |
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); |
77 |
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()); |
78 |
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() - |
79 | cone_reference.get_min_nforce()) < 1e-9); | ||
80 |
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()); |
81 | 1 | } | |
82 | |||
83 | // Create the wrench cone through the copy operator | ||
84 | { | ||
85 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::WrenchCone cone_copy; |
86 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | cone_copy = cone; |
87 | |||
88 |
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()); |
89 |
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())); |
90 |
2/2✓ Branch 2 taken 43 times.
✓ Branch 3 taken 1 times.
|
44 | for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size()); |
91 | ++i) { | ||
92 |
8/16✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 43 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 43 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 43 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 43 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 43 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 43 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 43 times.
|
43 | BOOST_CHECK(cone.get_ub()[i] == cone_copy.get_ub()[i]); |
93 |
8/16✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 43 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 43 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 43 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 43 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 43 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 43 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 43 times.
|
43 | BOOST_CHECK(cone.get_lb()[i] == cone_copy.get_lb()[i]); |
94 | } | ||
95 |
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())); |
96 |
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_box().isApprox(cone_copy.get_box())); |
97 |
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); |
98 |
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()); |
99 |
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()) < |
100 | 1e-9); | ||
101 |
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()); |
102 | 1 | } | |
103 | 1 | } | |
104 | |||
105 | 1 | void test_against_friction_cone() { | |
106 | // Common parameters | ||
107 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Quaterniond q; |
108 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::quaternion::uniformRandom(q); |
109 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Matrix3d R = q.toRotationMatrix(); |
110 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
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 |
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)); |
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 = true; | |
115 | |||
116 | // Create wrench and friction cone | ||
117 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::WrenchCone wrench_cone(R, mu, box, nf, inner_appr, 0., 100.); |
118 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::FrictionCone friction_cone(R, mu, nf, inner_appr, 0., 100.); |
119 | |||
120 |
8/16✓ 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 25 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
|
1 | BOOST_CHECK((wrench_cone.get_R() - friction_cone.get_R()).isZero(1e-9)); |
121 |
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(wrench_cone.get_mu() == friction_cone.get_mu()); |
122 |
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(wrench_cone.get_nf() == friction_cone.get_nf()); |
123 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
|
10 | for (std::size_t i = 0; i < nf + 1; ++i) { |
124 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 9 times.
|
36 | for (std::size_t j = 0; j < 3; ++j) { |
125 |
10/20✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 27 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 27 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 27 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 27 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 27 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 27 times.
✗ Branch 32 not taken.
✗ Branch 36 not taken.
✓ Branch 37 taken 27 times.
|
27 | BOOST_CHECK(wrench_cone.get_A().row(i)[j] == |
126 | friction_cone.get_A().row(i)[j]); | ||
127 | } | ||
128 | } | ||
129 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
|
10 | for (std::size_t i = 0; i < nf + 1; ++i) { |
130 |
8/16✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 9 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 9 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 9 times.
|
9 | BOOST_CHECK(wrench_cone.get_ub()[i] == friction_cone.get_ub()[i]); |
131 |
8/16✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 9 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 9 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 9 times.
|
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/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Quaterniond q; |
138 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::quaternion::uniformRandom(q); |
139 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Matrix3d R = q.toRotationMatrix(); |
140 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
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 |
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)); |
143 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
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 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::WrenchCone wrench_cone(R, mu, box, nf, inner_appr, 0., 100.); |
148 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::CoPSupport cop_support(R, box); |
149 | |||
150 |
8/16✓ 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 25 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
|
1 | BOOST_CHECK((wrench_cone.get_R() - cop_support.get_R()).isZero(1e-9)); |
151 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
|
5 | for (std::size_t i = 0; i < 4; ++i) { |
152 |
2/2✓ Branch 0 taken 24 times.
✓ Branch 1 taken 4 times.
|
28 | for (std::size_t j = 0; j < 6; ++j) { |
153 |
10/20✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 24 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 24 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 24 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 24 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 24 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 24 times.
✗ Branch 32 not taken.
✗ Branch 36 not taken.
✓ Branch 37 taken 24 times.
|
24 | BOOST_CHECK(wrench_cone.get_A().row(nf + i + 1)[j] == |
154 | cop_support.get_A().row(i)[j]); | ||
155 | } | ||
156 | } | ||
157 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
|
5 | for (std::size_t i = 0; i < 4; ++i) { |
158 |
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(wrench_cone.get_ub()[i + nf + 1] == cop_support.get_ub()[i]); |
159 |
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(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/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Quaterniond q; |
166 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::quaternion::uniformRandom(q); |
167 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Matrix3d R = q.toRotationMatrix(); |
168 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
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 |
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)); |
171 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | crocoddyl::WrenchCone cone(R, mu, cone_box); |
172 | |||
173 | // Create the activation for quadratic barrier | ||
174 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub()); |
175 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::ActivationModelQuadraticBarrier activation(bounds); |
176 | boost::shared_ptr<crocoddyl::ActivationDataAbstract> data = | ||
177 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | activation.createData(); |
178 | |||
179 | // Compute the activation value | ||
180 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::VectorXd wrench(6); |
181 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | wrench.setZero(); |
182 |
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); |
183 |
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() * wrench; |
184 |
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); |
185 | |||
186 | // The activation value has to be zero since the wrench is inside the wrench | ||
187 | // cone | ||
188 |
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.); |
189 | 1 | } | |
190 | |||
191 | 1 | void test_negative_force_along_wrench_cone_normal() { | |
192 | // Create the wrench cone | ||
193 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Quaterniond q; |
194 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::quaternion::uniformRandom(q); |
195 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Matrix3d R = q.toRotationMatrix(); |
196 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
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 |
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)); |
199 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | crocoddyl::WrenchCone cone(R, mu, cone_box); |
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 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::VectorXd wrench(6); |
209 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | wrench.setZero(); |
210 |
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); |
211 |
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() * wrench; |
212 |
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); |
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 |
2/2✓ Branch 1 taken 4 times.
✓ Branch 2 taken 1 times.
|
5 | for (std::size_t i = 0; i < cone.get_nf(); ++i) { |
219 |
7/14✓ 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 14 taken 4 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 4 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 4 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 4 times.
|
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 |
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); |
225 |
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.); |
226 | 1 | } | |
227 | |||
228 | 1 | void test_force_parallel_to_wrench_cone_normal() { | |
229 | // Create the wrench cone | ||
230 |
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(); |
231 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
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 |
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)); |
234 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | crocoddyl::WrenchCone cone(R, mu, cone_box); |
235 | |||
236 | // Create the activation for quadratic barrier | ||
237 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub()); |
238 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::ActivationModelQuadraticBarrier activation(bounds); |
239 | boost::shared_ptr<crocoddyl::ActivationDataAbstract> data = | ||
240 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | activation.createData(); |
241 | |||
242 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::VectorXd wrench(6); |
243 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | wrench.setZero(); |
244 |
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.) * Eigen::Vector3d::UnitX(); |
245 |
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() * wrench; |
246 | |||
247 | // The activation value has to be positive since the force is outside the | ||
248 | // wrench cone | ||
249 |
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); |
250 |
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.); |
251 | 1 | } | |
252 | |||
253 | 1 | void register_unit_tests() { | |
254 | 1 | framework::master_test_suite().add( | |
255 |
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))); |
256 | 1 | framework::master_test_suite().add( | |
257 |
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_against_friction_cone))); |
258 | 1 | framework::master_test_suite().add( | |
259 |
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_against_cop_support))); |
260 | 1 | framework::master_test_suite().add( | |
261 |
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_wrench_cone_normal))); |
262 |
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( |
263 | boost::bind(&test_negative_force_along_wrench_cone_normal))); | ||
264 | 1 | framework::master_test_suite().add( | |
265 |
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_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 | } | ||
276 |