Directory: | ./ |
---|---|
File: | unittest/test_wrench_cone.cpp |
Date: | 2025-03-26 19:23:43 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 244 | 250 | 97.6% |
Branches: | 1043 | 2050 | 50.9% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | /////////////////////////////////////////////////////////////////////////////// | ||
2 | // BSD 3-Clause License | ||
3 | // | ||
4 | // Copyright (C) 2021-2025, University of Edinburgh, University of Oxford, | ||
5 | // Heriot-Watt University | ||
6 | // Copyright note valid unless otherwise stated in individual files. | ||
7 | // All rights reserved. | ||
8 | /////////////////////////////////////////////////////////////////////////////// | ||
9 | |||
10 | #define BOOST_TEST_NO_MAIN | ||
11 | #define BOOST_TEST_ALTERNATIVE_INIT_API | ||
12 | |||
13 | #include <pinocchio/math/quaternion.hpp> | ||
14 | |||
15 | #include "crocoddyl/core/activations/quadratic-barrier.hpp" | ||
16 | #include "crocoddyl/multibody/cop-support.hpp" | ||
17 | #include "crocoddyl/multibody/friction-cone.hpp" | ||
18 | #include "crocoddyl/multibody/wrench-cone.hpp" | ||
19 | #include "unittest_common.hpp" | ||
20 | |||
21 | using namespace boost::unit_test; | ||
22 | using namespace crocoddyl::unittest; | ||
23 | |||
24 | 1 | void test_constructor() { | |
25 | // Common parameters | ||
26 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double mu = random_real_in_range(0.01, 1.); |
27 | ✗ | Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1), | |
28 |
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)); |
29 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | std::size_t nf = 2 * random_int_in_range(2, 16); |
30 | 1 | bool inner_appr = false; | |
31 | |||
32 | // No rotation | ||
33 |
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(); |
34 | |||
35 | // Create the wrench cone | ||
36 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | crocoddyl::WrenchCone cone(R, mu, box, nf, inner_appr); |
37 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::WrenchCone casted_cone = cone.cast<double>(); |
38 | |||
39 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(cone.get_R().isApprox(R)); |
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 14 taken 1 times.
✗ Branch 15 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(cone.get_mu() == mu); |
41 |
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 14 taken 1 times.
✗ Branch 15 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(cone.get_nf() == nf); |
42 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(cone.get_box().isApprox(box)); |
43 |
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 14 taken 1 times.
✗ Branch 15 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(cone.get_inner_appr() == inner_appr); |
44 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
1 | BOOST_CHECK(static_cast<std::size_t>(cone.get_A().rows()) == nf + 13); |
45 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
1 | BOOST_CHECK(static_cast<std::size_t>(cone.get_lb().size()) == nf + 13); |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
1 | BOOST_CHECK(static_cast<std::size_t>(cone.get_ub().size()) == nf + 13); |
47 | |||
48 | // Checking that casted computation is the same | ||
49 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(casted_cone.get_R().isApprox(R)); |
50 |
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 14 taken 1 times.
✗ Branch 15 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(casted_cone.get_mu() == mu); |
51 |
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 14 taken 1 times.
✗ Branch 15 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(casted_cone.get_nf() == nf); |
52 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(casted_cone.get_box().isApprox(box)); |
53 |
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 14 taken 1 times.
✗ Branch 15 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(casted_cone.get_inner_appr() == inner_appr); |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
1 | BOOST_CHECK(static_cast<std::size_t>(casted_cone.get_A().rows()) == nf + 13); |
55 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
1 | BOOST_CHECK(static_cast<std::size_t>(casted_cone.get_lb().size()) == nf + 13); |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
1 | BOOST_CHECK(static_cast<std::size_t>(casted_cone.get_ub().size()) == nf + 13); |
57 | |||
58 | // With rotation | ||
59 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Quaterniond q; |
60 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::quaternion::uniformRandom(q); |
61 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | R = q.toRotationMatrix(); |
62 | |||
63 | // Create the wrench cone | ||
64 |
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); |
65 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | casted_cone = cone.cast<double>(); |
66 | |||
67 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(cone.get_R().isApprox(R)); |
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 14 taken 1 times.
✗ Branch 15 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(cone.get_mu() == mu); |
69 |
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 14 taken 1 times.
✗ Branch 15 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(cone.get_nf() == nf); |
70 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(cone.get_box().isApprox(box)); |
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 14 taken 1 times.
✗ Branch 15 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(cone.get_inner_appr() == inner_appr); |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
1 | BOOST_CHECK(static_cast<std::size_t>(cone.get_A().rows()) == nf + 13); |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
1 | BOOST_CHECK(static_cast<std::size_t>(cone.get_lb().size()) == nf + 13); |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
1 | BOOST_CHECK(static_cast<std::size_t>(cone.get_ub().size()) == nf + 13); |
75 | |||
76 | // Checking that casted computation is the same | ||
77 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(casted_cone.get_R().isApprox(R)); |
78 |
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 14 taken 1 times.
✗ Branch 15 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(casted_cone.get_mu() == mu); |
79 |
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 14 taken 1 times.
✗ Branch 15 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(casted_cone.get_nf() == nf); |
80 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(casted_cone.get_box().isApprox(box)); |
81 |
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 14 taken 1 times.
✗ Branch 15 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(casted_cone.get_inner_appr() == inner_appr); |
82 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
1 | BOOST_CHECK(static_cast<std::size_t>(casted_cone.get_A().rows()) == nf + 13); |
83 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
1 | BOOST_CHECK(static_cast<std::size_t>(casted_cone.get_lb().size()) == nf + 13); |
84 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
1 | BOOST_CHECK(static_cast<std::size_t>(casted_cone.get_ub().size()) == nf + 13); |
85 | |||
86 | // Create the wrench cone from a reference | ||
87 | { | ||
88 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::WrenchCone cone_reference(cone); |
89 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | casted_cone = cone_reference.cast<double>(); |
90 | |||
91 |
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 14 taken 1 times.
✗ Branch 15 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_nf() == cone_reference.get_nf()); |
92 |
9/18✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
1 | BOOST_CHECK(cone.get_A().isApprox(cone_reference.get_A())); |
93 |
3/4✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 43 times.
✓ Branch 5 taken 1 times.
|
44 | for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size()); |
94 | ++i) { | ||
95 |
10/20✓ 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 14 taken 43 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 43 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 43 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 43 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 43 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 43 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 43 times.
|
43 | BOOST_CHECK(cone.get_ub()[i] == cone_reference.get_ub()[i]); |
96 |
10/20✓ 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 14 taken 43 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 43 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 43 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 43 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 43 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 43 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 43 times.
|
43 | BOOST_CHECK(cone.get_lb()[i] == cone_reference.get_lb()[i]); |
97 | } | ||
98 |
9/18✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
1 | BOOST_CHECK(cone.get_R().isApprox(cone_reference.get_R())); |
99 |
9/18✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
1 | BOOST_CHECK(cone.get_box().isApprox(cone_reference.get_box())); |
100 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(std::abs(cone.get_mu() - cone_reference.get_mu()) < 1e-9); |
101 |
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 14 taken 1 times.
✗ Branch 15 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_inner_appr() == cone_reference.get_inner_appr()); |
102 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(std::abs(cone.get_min_nforce() - |
103 | cone_reference.get_min_nforce()) < 1e-9); | ||
104 |
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 14 taken 1 times.
✗ Branch 15 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_max_nforce() == cone_reference.get_max_nforce()); |
105 | |||
106 | // Checking that casted computation is the same | ||
107 |
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 14 taken 1 times.
✗ Branch 15 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(casted_cone.get_nf() == cone_reference.get_nf()); |
108 |
9/18✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
1 | BOOST_CHECK(casted_cone.get_A().isApprox(cone_reference.get_A())); |
109 |
3/4✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 43 times.
✓ Branch 5 taken 1 times.
|
44 | for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size()); |
110 | ++i) { | ||
111 |
10/20✓ 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 14 taken 43 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 43 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 43 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 43 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 43 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 43 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 43 times.
|
43 | BOOST_CHECK(casted_cone.get_ub()[i] == cone_reference.get_ub()[i]); |
112 |
10/20✓ 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 14 taken 43 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 43 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 43 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 43 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 43 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 43 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 43 times.
|
43 | BOOST_CHECK(casted_cone.get_lb()[i] == cone_reference.get_lb()[i]); |
113 | } | ||
114 |
9/18✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
1 | BOOST_CHECK(casted_cone.get_R().isApprox(cone_reference.get_R())); |
115 |
9/18✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
1 | BOOST_CHECK(casted_cone.get_box().isApprox(cone_reference.get_box())); |
116 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(std::abs(casted_cone.get_mu() - cone_reference.get_mu()) < |
117 | 1e-9); | ||
118 |
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 14 taken 1 times.
✗ Branch 15 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(casted_cone.get_inner_appr() == |
119 | cone_reference.get_inner_appr()); | ||
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(std::abs(casted_cone.get_min_nforce() - |
121 | cone_reference.get_min_nforce()) < 1e-9); | ||
122 |
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 14 taken 1 times.
✗ Branch 15 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(casted_cone.get_max_nforce() == |
123 | cone_reference.get_max_nforce()); | ||
124 | 1 | } | |
125 | |||
126 | // Create the wrench cone through the copy operator | ||
127 | { | ||
128 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::WrenchCone cone_copy; |
129 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | cone_copy = cone; |
130 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | casted_cone = cone_copy.cast<double>(); |
131 | |||
132 |
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 14 taken 1 times.
✗ Branch 15 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_nf() == cone_copy.get_nf()); |
133 |
9/18✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
1 | BOOST_CHECK(cone.get_A().isApprox(cone_copy.get_A())); |
134 |
3/4✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 43 times.
✓ Branch 5 taken 1 times.
|
44 | for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size()); |
135 | ++i) { | ||
136 |
10/20✓ 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 14 taken 43 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 43 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 43 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 43 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 43 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 43 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 43 times.
|
43 | BOOST_CHECK(cone.get_ub()[i] == cone_copy.get_ub()[i]); |
137 |
10/20✓ 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 14 taken 43 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 43 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 43 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 43 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 43 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 43 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 43 times.
|
43 | BOOST_CHECK(cone.get_lb()[i] == cone_copy.get_lb()[i]); |
138 | } | ||
139 |
9/18✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
1 | BOOST_CHECK(cone.get_R().isApprox(cone_copy.get_R())); |
140 |
9/18✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
1 | BOOST_CHECK(cone.get_box().isApprox(cone_copy.get_box())); |
141 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(std::abs(cone.get_mu() - cone_copy.get_mu()) < 1e-9); |
142 |
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 14 taken 1 times.
✗ Branch 15 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_inner_appr() == cone_copy.get_inner_appr()); |
143 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(std::abs(cone.get_min_nforce() - cone_copy.get_min_nforce()) < |
144 | 1e-9); | ||
145 |
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 14 taken 1 times.
✗ Branch 15 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_max_nforce() == cone_copy.get_max_nforce()); |
146 | |||
147 | // Checking that casted computation is the same | ||
148 |
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 14 taken 1 times.
✗ Branch 15 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(casted_cone.get_nf() == cone_copy.get_nf()); |
149 |
9/18✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
1 | BOOST_CHECK(casted_cone.get_A().isApprox(cone_copy.get_A())); |
150 |
3/4✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 43 times.
✓ Branch 5 taken 1 times.
|
44 | for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size()); |
151 | ++i) { | ||
152 |
10/20✓ 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 14 taken 43 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 43 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 43 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 43 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 43 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 43 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 43 times.
|
43 | BOOST_CHECK(casted_cone.get_ub()[i] == cone_copy.get_ub()[i]); |
153 |
10/20✓ 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 14 taken 43 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 43 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 43 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 43 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 43 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 43 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 43 times.
|
43 | BOOST_CHECK(casted_cone.get_lb()[i] == cone_copy.get_lb()[i]); |
154 | } | ||
155 |
9/18✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
1 | BOOST_CHECK(casted_cone.get_R().isApprox(cone_copy.get_R())); |
156 |
9/18✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
1 | BOOST_CHECK(casted_cone.get_box().isApprox(cone_copy.get_box())); |
157 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(std::abs(casted_cone.get_mu() - cone_copy.get_mu()) < 1e-9); |
158 |
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 14 taken 1 times.
✗ Branch 15 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(casted_cone.get_inner_appr() == cone_copy.get_inner_appr()); |
159 |
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
1 | BOOST_CHECK(std::abs(casted_cone.get_min_nforce() - |
160 | cone_copy.get_min_nforce()) < 1e-9); | ||
161 |
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 14 taken 1 times.
✗ Branch 15 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(casted_cone.get_max_nforce() == cone_copy.get_max_nforce()); |
162 | 1 | } | |
163 | 1 | } | |
164 | |||
165 | 1 | void test_against_friction_cone() { | |
166 | // Common parameters | ||
167 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Quaterniond q; |
168 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::quaternion::uniformRandom(q); |
169 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Matrix3d R = q.toRotationMatrix(); |
170 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double mu = random_real_in_range(0.01, 1.); |
171 | ✗ | Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1), | |
172 |
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)); |
173 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | std::size_t nf = 2 * random_int_in_range(2, 16); |
174 | 1 | bool inner_appr = true; | |
175 | |||
176 | // Create wrench and friction cone | ||
177 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::WrenchCone wrench_cone(R, mu, box, nf, inner_appr, 0., 100.); |
178 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::FrictionCone friction_cone(R, mu, nf, inner_appr, 0., 100.); |
179 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::WrenchCone casted_wrench_cone = wrench_cone.cast<double>(); |
180 | |||
181 |
10/20✓ 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 14 taken 1 times.
✗ Branch 15 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 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
|
1 | BOOST_CHECK((wrench_cone.get_R() - friction_cone.get_R()).isZero(1e-9)); |
182 |
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 14 taken 1 times.
✗ Branch 15 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(wrench_cone.get_mu() == friction_cone.get_mu()); |
183 |
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 14 taken 1 times.
✗ Branch 15 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(wrench_cone.get_nf() == friction_cone.get_nf()); |
184 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
|
10 | for (std::size_t i = 0; i < nf + 1; ++i) { |
185 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 9 times.
|
36 | for (std::size_t j = 0; j < 3; ++j) { |
186 |
12/24✓ 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 14 taken 27 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 27 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 27 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 27 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 27 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 27 times.
|
27 | BOOST_CHECK(wrench_cone.get_A().row(i)[j] == |
187 | friction_cone.get_A().row(i)[j]); | ||
188 | } | ||
189 | } | ||
190 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
|
10 | for (std::size_t i = 0; i < nf + 1; ++i) { |
191 |
10/20✓ 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 14 taken 9 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 9 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 9 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 9 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 9 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 9 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 9 times.
|
9 | BOOST_CHECK(wrench_cone.get_ub()[i] == friction_cone.get_ub()[i]); |
192 |
10/20✓ 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 14 taken 9 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 9 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 9 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 9 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 9 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 9 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 9 times.
|
9 | BOOST_CHECK(wrench_cone.get_lb()[i] == friction_cone.get_lb()[i]); |
193 | } | ||
194 | |||
195 | // Checking that casted computation is the same | ||
196 |
10/20✓ 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 14 taken 1 times.
✗ Branch 15 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 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
|
1 | BOOST_CHECK( |
197 | (casted_wrench_cone.get_R() - friction_cone.get_R()).isZero(1e-9)); | ||
198 |
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 14 taken 1 times.
✗ Branch 15 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(casted_wrench_cone.get_mu() == friction_cone.get_mu()); |
199 |
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 14 taken 1 times.
✗ Branch 15 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(casted_wrench_cone.get_nf() == friction_cone.get_nf()); |
200 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
|
10 | for (std::size_t i = 0; i < nf + 1; ++i) { |
201 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 9 times.
|
36 | for (std::size_t j = 0; j < 3; ++j) { |
202 |
12/24✓ 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 14 taken 27 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 27 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 27 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 27 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 27 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 27 times.
|
27 | BOOST_CHECK(casted_wrench_cone.get_A().row(i)[j] == |
203 | friction_cone.get_A().row(i)[j]); | ||
204 | } | ||
205 | } | ||
206 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
|
10 | for (std::size_t i = 0; i < nf + 1; ++i) { |
207 |
10/20✓ 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 14 taken 9 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 9 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 9 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 9 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 9 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 9 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 9 times.
|
9 | BOOST_CHECK(casted_wrench_cone.get_ub()[i] == friction_cone.get_ub()[i]); |
208 |
10/20✓ 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 14 taken 9 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 9 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 9 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 9 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 9 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 9 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 9 times.
|
9 | BOOST_CHECK(casted_wrench_cone.get_lb()[i] == friction_cone.get_lb()[i]); |
209 | } | ||
210 | 1 | } | |
211 | |||
212 | 1 | void test_against_cop_support() { | |
213 | // Common parameters | ||
214 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Quaterniond q; |
215 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::quaternion::uniformRandom(q); |
216 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Matrix3d R = q.toRotationMatrix(); |
217 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double mu = random_real_in_range(0.01, 1.); |
218 | ✗ | Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1), | |
219 |
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)); |
220 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | std::size_t nf = 2 * random_int_in_range(2, 16); |
221 | 1 | bool inner_appr = true; | |
222 | |||
223 | // Create wrench and friction cone | ||
224 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::WrenchCone wrench_cone(R, mu, box, nf, inner_appr, 0., 100.); |
225 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::CoPSupport cop_support(R, box); |
226 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::WrenchCone casted_wrench_cone = wrench_cone.cast<double>(); |
227 | |||
228 |
10/20✓ 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 14 taken 1 times.
✗ Branch 15 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 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
|
1 | BOOST_CHECK((wrench_cone.get_R() - cop_support.get_R()).isZero(1e-9)); |
229 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
|
5 | for (std::size_t i = 0; i < 4; ++i) { |
230 |
2/2✓ Branch 0 taken 24 times.
✓ Branch 1 taken 4 times.
|
28 | for (std::size_t j = 0; j < 6; ++j) { |
231 |
12/24✓ 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 14 taken 24 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 24 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 24 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 24 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 24 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 24 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 24 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 24 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 24 times.
|
24 | BOOST_CHECK(wrench_cone.get_A().row(nf + i + 1)[j] == |
232 | cop_support.get_A().row(i)[j]); | ||
233 | } | ||
234 | } | ||
235 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
|
5 | for (std::size_t i = 0; i < 4; ++i) { |
236 |
10/20✓ 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 23 taken 4 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 4 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 4 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 4 times.
|
4 | BOOST_CHECK(wrench_cone.get_ub()[i + nf + 1] == cop_support.get_ub()[i]); |
237 |
10/20✓ 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 23 taken 4 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 4 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 4 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 4 times.
|
4 | BOOST_CHECK(wrench_cone.get_lb()[i + nf + 1] == cop_support.get_lb()[i]); |
238 | } | ||
239 | |||
240 | // Checking that casted computation is the same | ||
241 |
10/20✓ 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 14 taken 1 times.
✗ Branch 15 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 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
|
1 | BOOST_CHECK((casted_wrench_cone.get_R() - cop_support.get_R()).isZero(1e-9)); |
242 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
|
5 | for (std::size_t i = 0; i < 4; ++i) { |
243 |
2/2✓ Branch 0 taken 24 times.
✓ Branch 1 taken 4 times.
|
28 | for (std::size_t j = 0; j < 6; ++j) { |
244 |
12/24✓ 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 14 taken 24 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 24 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 24 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 24 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 24 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 24 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 24 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 24 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 24 times.
|
24 | BOOST_CHECK(casted_wrench_cone.get_A().row(nf + i + 1)[j] == |
245 | cop_support.get_A().row(i)[j]); | ||
246 | } | ||
247 | } | ||
248 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
|
5 | for (std::size_t i = 0; i < 4; ++i) { |
249 |
10/20✓ 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 23 taken 4 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 4 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 4 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 4 times.
|
4 | BOOST_CHECK(casted_wrench_cone.get_ub()[i + nf + 1] == |
250 | cop_support.get_ub()[i]); | ||
251 |
10/20✓ 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 23 taken 4 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 4 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 4 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 4 times.
|
4 | BOOST_CHECK(casted_wrench_cone.get_lb()[i + nf + 1] == |
252 | cop_support.get_lb()[i]); | ||
253 | } | ||
254 | 1 | } | |
255 | |||
256 | 1 | void test_force_along_wrench_cone_normal() { | |
257 | // Create the wrench cone | ||
258 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Quaterniond q; |
259 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::quaternion::uniformRandom(q); |
260 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Matrix3d R = q.toRotationMatrix(); |
261 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double mu = random_real_in_range(0.01, 1.); |
262 | ✗ | Eigen::Vector2d cone_box = Eigen::Vector2d(random_real_in_range(0.01, 0.1), | |
263 |
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)); |
264 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | crocoddyl::WrenchCone cone(R, mu, cone_box); |
265 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::WrenchCone casted_cone = cone.cast<double>(); |
266 | |||
267 | // Create the activation for quadratic barrier | ||
268 |
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 | crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub()); |
269 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::ActivationModelQuadraticBarrier activation(bounds); |
270 | crocoddyl::ActivationModelQuadraticBarrier casted_activation = | ||
271 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | activation.cast<double>(); |
272 | const std::shared_ptr<crocoddyl::ActivationDataAbstract>& data = | ||
273 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | activation.createData(); |
274 | const std::shared_ptr<crocoddyl::ActivationDataAbstract>& casted_data = | ||
275 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | casted_activation.createData(); |
276 | |||
277 | // Compute the activation value | ||
278 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::VectorXd wrench(6); |
279 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | wrench.setZero(); |
280 |
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); |
281 |
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 | Eigen::VectorXd r = cone.get_A() * wrench; |
282 |
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); |
283 | |||
284 | // The activation value has to be zero since the wrench is inside the wrench | ||
285 | // cone | ||
286 |
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.); |
287 | |||
288 | // Checking that casted computation is the same | ||
289 |
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 | r = casted_cone.get_A() * wrench; |
290 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | casted_activation.calc(casted_data, r); |
291 |
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(casted_data->a_value == 0.); |
292 | 1 | } | |
293 | |||
294 | 1 | void test_negative_force_along_wrench_cone_normal() { | |
295 | // Create the wrench cone | ||
296 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Quaterniond q; |
297 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::quaternion::uniformRandom(q); |
298 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Matrix3d R = q.toRotationMatrix(); |
299 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double mu = random_real_in_range(0.01, 1.); |
300 | ✗ | Eigen::Vector2d cone_box = Eigen::Vector2d(random_real_in_range(0.01, 0.1), | |
301 |
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)); |
302 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | crocoddyl::WrenchCone cone(R, mu, cone_box); |
303 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::WrenchCone casted_cone = cone.cast<double>(); |
304 | |||
305 | // Create the activation for quadratic barrier | ||
306 |
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 | crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub()); |
307 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::ActivationModelQuadraticBarrier activation(bounds); |
308 | crocoddyl::ActivationModelQuadraticBarrier casted_activation = | ||
309 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | activation.cast<double>(); |
310 | const std::shared_ptr<crocoddyl::ActivationDataAbstract>& data = | ||
311 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | activation.createData(); |
312 | const std::shared_ptr<crocoddyl::ActivationDataAbstract>& casted_data = | ||
313 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | casted_activation.createData(); |
314 | |||
315 | // Compute the activation value | ||
316 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::VectorXd wrench(6); |
317 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | wrench.setZero(); |
318 |
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); |
319 |
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 | Eigen::VectorXd r = cone.get_A() * wrench; |
320 |
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); |
321 | |||
322 | // The first nf elements of the residual has to be positive since the force is | ||
323 | // outside the wrench cone. Additionally, the last value has to be equals to | ||
324 | // the force norm but with negative value since the wrench is aligned and in | ||
325 | // opposite direction to the wrench cone orientation | ||
326 |
3/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 1 times.
|
5 | for (std::size_t i = 0; i < cone.get_nf(); ++i) { |
327 |
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.); |
328 | } | ||
329 | |||
330 | // The activation value has to be positive since the wrench is outside the | ||
331 | // wrench cone | ||
332 |
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); |
333 |
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.); |
334 | |||
335 | // Checking that casted computation is the same | ||
336 |
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 | r = casted_cone.get_A() * wrench; |
337 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | casted_activation.calc(casted_data, r); |
338 |
3/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 1 times.
|
5 | for (std::size_t i = 0; i < casted_cone.get_nf(); ++i) { |
339 |
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.); |
340 | } | ||
341 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | casted_activation.calc(casted_data, r); |
342 |
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(casted_data->a_value > 0.); |
343 | 1 | } | |
344 | |||
345 | 1 | void test_force_parallel_to_wrench_cone_normal() { | |
346 | // Create the wrench cone | ||
347 |
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(); |
348 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double mu = random_real_in_range(0.01, 1.); |
349 | ✗ | Eigen::Vector2d cone_box = Eigen::Vector2d(random_real_in_range(0.01, 0.1), | |
350 |
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)); |
351 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | crocoddyl::WrenchCone cone(R, mu, cone_box); |
352 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::WrenchCone casted_cone = cone.cast<double>(); |
353 | |||
354 | // Create the activation for quadratic barrier | ||
355 |
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 | crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub()); |
356 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | crocoddyl::ActivationModelQuadraticBarrier activation(bounds); |
357 | crocoddyl::ActivationModelQuadraticBarrier casted_activation = | ||
358 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | activation.cast<double>(); |
359 | const std::shared_ptr<crocoddyl::ActivationDataAbstract>& data = | ||
360 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | activation.createData(); |
361 | const std::shared_ptr<crocoddyl::ActivationDataAbstract>& casted_data = | ||
362 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | casted_activation.createData(); |
363 | |||
364 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::VectorXd wrench(6); |
365 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | wrench.setZero(); |
366 |
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(); |
367 |
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 | Eigen::VectorXd r = cone.get_A() * wrench; |
368 | |||
369 | // The activation value has to be positive since the force is outside the | ||
370 | // wrench cone | ||
371 |
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); |
372 |
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.); |
373 | |||
374 | // Checking that casted computation is the same | ||
375 |
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 | r = casted_cone.get_A() * wrench; |
376 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | casted_activation.calc(casted_data, r); |
377 |
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(casted_data->a_value > 0.); |
378 | 1 | } | |
379 | |||
380 | 1 | void register_unit_tests() { | |
381 | 1 | framework::master_test_suite().add( | |
382 |
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))); |
383 | 1 | framework::master_test_suite().add( | |
384 |
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))); |
385 | 1 | framework::master_test_suite().add( | |
386 |
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))); |
387 | 1 | framework::master_test_suite().add( | |
388 |
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))); |
389 |
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( |
390 | boost::bind(&test_negative_force_along_wrench_cone_normal))); | ||
391 | 1 | framework::master_test_suite().add( | |
392 |
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))); |
393 | 1 | } | |
394 | |||
395 | 1 | bool init_function() { | |
396 | 1 | register_unit_tests(); | |
397 | 1 | return true; | |
398 | } | ||
399 | |||
400 | 1 | int main(int argc, char* argv[]) { | |
401 | 1 | return ::boost::unit_test::unit_test_main(&init_function, argc, argv); | |
402 | } | ||
403 |