GCC Code Coverage Report

Directory: ./
File: tests/generic-transformation.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 336 363 92.6%
Branches: 1005 2170 46.3%
Line Branch Exec Source
1 // Copyright (c) 2016, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #define EIGEN_RUNTIME_NO_MALLOC
30
31 #include <hpp/constraints/explicit/relative-pose.hh>
32 #include <hpp/constraints/generic-transformation.hh>
33 #include <hpp/constraints/solver/by-substitution.hh>
34 #include <hpp/pinocchio/configuration.hh>
35 #include <hpp/pinocchio/device.hh>
36 #include <hpp/pinocchio/joint-collection.hh>
37 #include <hpp/pinocchio/joint.hh>
38 #include <hpp/pinocchio/serialization.hh>
39 #include <hpp/pinocchio/simple-device.hh>
40 #include <hpp/pinocchio/urdf/util.hh>
41 #include <pinocchio/algorithm/joint-configuration.hpp>
42 #include <sstream>
43
44 #include "hpp/constraints/tools.hh"
45
46 #define BOOST_TEST_MODULE hpp_constraints
47 #include <stdlib.h>
48
49 #include <boost/test/included/unit_test.hpp>
50
51 using hpp::constraints::EqualToZero;
52 using hpp::pinocchio::Configuration_t;
53 using hpp::pinocchio::ConfigurationPtr_t;
54 using hpp::pinocchio::Device;
55 using hpp::pinocchio::DevicePtr_t;
56 using hpp::pinocchio::JOINT_POSITION;
57 using hpp::pinocchio::JointIndex;
58 using hpp::pinocchio::JointPtr_t;
59 using hpp::pinocchio::LiegroupSpace;
60 using hpp::pinocchio::Transform3s;
61
62 using hpp::pinocchio::urdf::loadModelFromString;
63
64 using namespace hpp::constraints;
65
66 class BasicConfigurationShooter {
67 public:
68 4 BasicConfigurationShooter(const DevicePtr_t& robot) : robot_(robot) {}
69 1006 virtual Configuration_t shoot() const {
70 1006 size_type extraDim = robot_->extraConfigSpace().dimension();
71 1006 size_type offset = robot_->configSize() - extraDim;
72
73
1/2
✓ Branch 3 taken 1006 times.
✗ Branch 4 not taken.
 1006 Configuration_t config(robot_->configSize());
74
3/6
✓ Branch 3 taken 1006 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1006 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1006 times.
✗ Branch 10 not taken.
 1006 config.head(offset) = ::pinocchio::randomConfiguration(robot_->model());
75
76 // Shoot extra configuration variables
77
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1006 times.
 1006 for (size_type i = 0; i < extraDim; ++i) {
78 ✗ value_type lower = robot_->extraConfigSpace().lower(i);
79 ✗ value_type upper = robot_->extraConfigSpace().upper(i);
80 ✗ value_type range = upper - lower;
81 ✗ if ((range < 0) || (range == std::numeric_limits<double>::infinity())) {
82 ✗ std::ostringstream oss("Cannot uniformy sample extra config variable ");
83 ✗ oss << i << ". min = " << lower << ", max = " << upper << std::endl;
84 ✗ throw std::runtime_error(oss.str());
85 }
86 ✗ config[offset + i] = lower + (upper - lower) * rand() / RAND_MAX;
87 }
88 1006 return config;
89 }
90
91 private:
92 DevicePtr_t robot_;
93 }; // class BasicConfigurationShooter
94
95
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(print) {
96 DevicePtr_t device = hpp::pinocchio::unittest::makeDevice(
97
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 hpp::pinocchio::unittest::HumanoidSimple);
98
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 JointPtr_t ee1 = device->getJointByName("lleg5_joint"),
99
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 ee2 = device->getJointByName("rleg5_joint");
100
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
 2 BOOST_REQUIRE(device);
101 2 BasicConfigurationShooter cs(device);
102
103
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 device->currentConfiguration(cs.shoot());
104
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 device->computeForwardKinematics(JOINT_POSITION);
105
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Transform3s tf1(ee1->currentTransformation());
106
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Transform3s tf2(ee2->currentTransformation());
107
108 2 std::vector<DifferentiableFunctionPtr_t> functions;
109
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 functions.push_back(Orientation::create("Orientation", device, ee2, tf2));
110
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 functions.push_back(Position::create("Position", device, ee2, tf2, tf1));
111
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(
112
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 Transformation::create("Transformation", device, ee1, tf1));
113
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 functions.push_back(RelativeOrientation::create("RelativeOrientation", device,
114 ee1, ee2, tf1));
115
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(
116
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 4 RelativePosition::create("RelativePosition", device, ee1, ee2, tf1, tf2));
117
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 functions.push_back(RelativeTransformation::create(
118 "RelativeTransformation", device, ee1, ee2, tf1, tf2));
119
120
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Configuration_t q1 = cs.shoot(), q2 = cs.shoot();
121
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 1 times.
 14 for (std::size_t i = 0; i < functions.size(); ++i) {
122 12 DifferentiableFunctionPtr_t f = functions[i];
123
124
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 std::cout << *f << std::endl;
125
126
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 12 LiegroupElement v(f->outputSpace());
127
1/2
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 matrix_t J(f->outputDerivativeSize(), f->inputDerivativeSize());
128
129
3/6
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
 12 f->value(v, q1);
130
3/6
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
 12 f->jacobian(J, q1);
131 // TODO this is broken at the moment because of the introduction
132 // of a multithreaded device.
133 // Eigen::internal::set_is_malloc_allowed(false);
134
3/6
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
 12 f->value(v, q2);
135
3/6
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
 12 f->jacobian(J, q2);
136 // Eigen::internal::set_is_malloc_allowed(true);
137 12 }
138
139 // Check active parameters
140 ArrayXb ap1 =
141
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 4 Orientation::create("Orientation", device, ee1, tf1)->activeParameters();
142 ArrayXb ap2 =
143
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 4 Orientation::create("Orientation", device, ee2, tf2)->activeParameters();
144 ArrayXb ap12 =
145
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 4 RelativeOrientation::create("RelativeOrientation", device, ee1, ee2, tf1)
146
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 ->activeParameters();
147
148
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 ArrayXb not_ap1 = (ap1 == false);
149
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 ArrayXb not_ap2 = (ap2 == false);
150
11/22
✓ 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 32 taken 1 times.
✗ Branch 33 not taken.
✗ Branch 37 not taken.
✓ Branch 38 taken 1 times.
 2 BOOST_CHECK((ap12 == ((not_ap1 && ap2) || (ap1 && not_ap2))).all());
151
152 // Check active derivative parameters
153
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 ap1 = Orientation::create("Orientation", device, ee1, tf1)
154
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 ->activeDerivativeParameters();
155
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 ap2 = Orientation::create("Orientation", device, ee2, tf2)
156
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 ->activeDerivativeParameters();
157 ap12 =
158
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 4 RelativeOrientation::create("RelativeOrientation", device, ee1, ee2, tf1)
159
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 ->activeDerivativeParameters();
160
161
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 not_ap1 = (ap1 == false);
162
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 not_ap2 = (ap2 == false);
163
11/22
✓ 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 32 taken 1 times.
✗ Branch 33 not taken.
✗ Branch 37 not taken.
✓ Branch 38 taken 1 times.
 2 BOOST_CHECK((ap12 == ((not_ap1 && ap2) || (ap1 && not_ap2))).all());
164 2 }
165
166
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(multithread) {
167 DevicePtr_t device = hpp::pinocchio::unittest::makeDevice(
168
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 hpp::pinocchio::unittest::HumanoidSimple);
169
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 device->numberDeviceData(4);
170
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 JointPtr_t ee1 = device->getJointByName("lleg5_joint"),
171
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 ee2 = device->getJointByName("rleg5_joint");
172
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
 2 BOOST_REQUIRE(device);
173 2 BasicConfigurationShooter cs(device);
174
175
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 device->currentConfiguration(cs.shoot());
176
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 device->computeForwardKinematics(JOINT_POSITION);
177
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Transform3s tf1(ee1->currentTransformation());
178
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Transform3s tf2(ee2->currentTransformation());
179
180 2 std::vector<DifferentiableFunctionPtr_t> functions;
181
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 functions.push_back(Orientation::create("Orientation", device, ee2, tf2));
182
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 functions.push_back(Position::create("Position", device, ee2, tf2, tf1));
183
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(
184
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 Transformation::create("Transformation", device, ee1, tf1));
185
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 functions.push_back(RelativeOrientation::create("RelativeOrientation", device,
186 ee1, ee2, tf1));
187
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(
188
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 4 RelativePosition::create("RelativePosition", device, ee1, ee2, tf1, tf2));
189
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 functions.push_back(RelativeTransformation::create(
190 "RelativeTransformation", device, ee1, ee2, tf1, tf2));
191
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 6 functions.push_back(RelativeOrientation::create("RelativeOrientation", device,
192 4 ee1, JointPtr_t(), tf1));
193
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 6 functions.push_back(RelativePosition::create("RelativePosition", device, ee1,
194 4 JointPtr_t(), tf1, tf2));
195
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 6 functions.push_back(RelativeTransformation::create(
196 4 "RelativeTransformation", device, ee1, JointPtr_t(), tf1, tf2));
197
198 2 const int N = 10;
199
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Configuration_t q = cs.shoot();
200
2/2
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 1 times.
 20 for (std::size_t i = 0; i < functions.size(); ++i) {
201 18 DifferentiableFunctionPtr_t f = functions[i];
202
203
2/4
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 36 std::vector<LiegroupElement> vs(N, LiegroupElement(f->outputSpace()));
204 std::vector<matrix_t> Js(
205
2/4
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
 36 N, matrix_t(f->outputDerivativeSize(), f->inputDerivativeSize()));
206 18 #pragma omp parallel for
207 for (int j = 0; j < 10; ++j) {
208 f->value(vs[j], q);
209 f->jacobian(Js[j], q);
210 }
211
212
2/2
✓ Branch 0 taken 81 times.
✓ Branch 1 taken 9 times.
 180 for (int j = 1; j < N; ++j) {
213
5/10
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 81 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 81 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 81 times.
 162 BOOST_CHECK_EQUAL(vs[0].vector(), vs[j].vector());
214
5/10
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 81 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 81 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 81 times.
 162 BOOST_CHECK_EQUAL(Js[0], Js[j]);
215 }
216 18 }
217 2 }
218
219
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(serialization) {
220 DevicePtr_t device = hpp::pinocchio::unittest::makeDevice(
221
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 hpp::pinocchio::unittest::HumanoidSimple);
222
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 device->numberDeviceData(4);
223
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 JointPtr_t ee1 = device->getJointByName("lleg5_joint"),
224
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 ee2 = device->getJointByName("rleg5_joint");
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
 2 BOOST_REQUIRE(device);
226
227
3/6
✓ 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.
 2 device->currentConfiguration(device->neutralConfiguration());
228
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 device->computeForwardKinematics(JOINT_POSITION);
229
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Transform3s tf1(ee1->currentTransformation());
230
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Transform3s tf2(ee2->currentTransformation());
231
232 2 std::vector<DifferentiableFunctionPtr_t> functions;
233
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 functions.push_back(Orientation::create("Orientation", device, ee2, tf2));
234
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 functions.push_back(Position::create("Position", device, ee2, tf2, tf1));
235
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(
236
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 Transformation::create("Transformation", device, ee1, tf1));
237
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 functions.push_back(RelativeOrientation::create("RelativeOrientation", device,
238 ee1, ee2, tf1));
239
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(
240
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 4 RelativePosition::create("RelativePosition", device, ee1, ee2, tf1, tf2));
241
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 functions.push_back(RelativeTransformation::create(
242 "RelativeTransformation", device, ee1, ee2, tf1, tf2));
243
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 6 functions.push_back(RelativeOrientation::create("RelativeOrientation", device,
244 4 ee1, JointPtr_t(), tf1));
245
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 6 functions.push_back(RelativePosition::create("RelativePosition", device, ee1,
246 4 JointPtr_t(), tf1, tf2));
247
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 6 functions.push_back(RelativeTransformation::create(
248 4 "RelativeTransformation", device, ee1, JointPtr_t(), tf1, tf2));
249
250
2/2
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 1 times.
 20 for (std::size_t i = 0; i < functions.size(); ++i) {
251 18 DifferentiableFunctionPtr_t f = functions[i];
252
253 18 DifferentiableFunctionPtr_t f_restored;
254
255
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 std::stringstream ss;
256 {
257
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 hpp::serialization::xml_oarchive oa(ss);
258
1/2
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 18 oa.insert(device->name(), device.get());
259
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 18 oa << boost::serialization::make_nvp("function", f);
260 18 }
261
262 {
263
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 hpp::serialization::xml_iarchive ia(ss);
264
1/2
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 18 ia.insert(device->name(), device.get());
265
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 18 ia >> boost::serialization::make_nvp("function", f_restored);
266 18 }
267
268
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 18 std::ostringstream oss1, oss2;
269
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 18 oss1 << *f;
270
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 18 oss2 << *f_restored;
271
272
7/14
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 9 times.
 18 BOOST_CHECK_EQUAL(oss1.str(), oss2.str());
273 18 }
274 2 }
275
276
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(RelativeTransformation_R3xSO3) {
277 const std::string model(
278 "<robot name=\"box\">"
279 " <link name=\"baselink\">"
280 " </link>"
281
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 "</robot>");
282
283
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 DevicePtr_t robot(Device::create("two-freeflyers"));
284 // Create two freeflying boxes
285
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 2 loadModelFromString(robot, 0, "1/", "freeflyer", model, "");
286
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 2 loadModelFromString(robot, 0, "2/", "freeflyer", model, "");
287
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 15 taken 1 times.
✗ Branch 16 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.
 2 BOOST_CHECK(robot->configSize() == 14);
288
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 15 taken 1 times.
✗ Branch 16 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.
 2 BOOST_CHECK(robot->numberDof() == 12);
289
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 15 taken 1 times.
✗ Branch 16 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.
 2 BOOST_CHECK(robot->nbJoints() == 2);
290
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 JointPtr_t j1(robot->jointAt(0));
291
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 JointPtr_t j2(robot->jointAt(1));
292
293 // Set joint bounds
294
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 2; ++i) {
295
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 vector_t l(7);
296
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
 4 l << -2, -2, -2, -1, -1, -1, -1;
297
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 vector_t u(7);
298
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
 4 u << 2, 2, 2, 1, 1, 1, 1;
299
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 4 robot->jointAt(i)->lowerBounds(l);
300
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 4 robot->jointAt(i)->upperBounds(u);
301 4 }
302 // Create constraint
303 //
304 // Excerpt from romeo-placard benchmark
305 // Joint1: romeo/LWristPitch
306 // Frame in joint 1
307 // R = 0.7071067739978436073, 0.70710678837525142715, 0
308 // -2.2663502965461253728e-09, 2.2663502504650490188e-09, -1
309 // -0.70710678837525142715,
310 // 0.70710677399784382935, 3.2051032938795742666e-09
311 // p = 0.099999999776482578762, -3.2051032222399330291e-11,
312 // -0.029999999776482582509
313 // Joint2: placard/root_joint
314 // Frame in joint 2
315 // R = 1, 0, 0
316 // 0, 1, 0
317 // 0, 0, 1
318 // p = 0, 0, -0.34999999403953552246
319 // mask: 1, 1, 1, 1, 1, 1,
320 // Rhs: 0, 0, 0, 0, 0, 0, 1
321 // active rows: [ 0, 5],
322
323
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 matrix3_t R1, R2;
324
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 vector3_t p1, p2;
325
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.
 2 R1 << 0.7071067739978436073, 0.70710678837525142715, 0,
326
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.
 2 -2.2663502965461253728e-09, 2.2663502504650490188e-09, -1,
327
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 -0.70710678837525142715, 0.70710677399784382935,
328
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 3.2051032938795742666e-09;
329
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 p1 << 0.099999999776482578762, -3.2051032222399330291e-11,
330
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 -0.029999999776482582509;
331
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 R2.setIdentity();
332
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.
 2 p2 << 0, 0, -0.34999999403953552246;
333
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Transform3s tf1(R1, p1), tf2(R2, p2);
334
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::vector<bool> mask = {false, false, false, false, false, true};
335 ImplicitPtr_t constraint(Implicit::create(
336
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 4 RelativeTransformationR3xSO3::create("RelativeTransformationR3xSO3",
337 robot, j1, j2, tf1, tf2),
338
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 8 6 * Equality, mask));
339 2 BasicConfigurationShooter cs(robot);
340
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 solver::BySubstitution solver(robot->configSpace());
341
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 solver.errorThreshold(1e-10);
342
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 solver.add(constraint);
343 // Check that after setting right hand side with a configuration
344 // the configuration satisfies the constraint since comparison type is
345 // Equality.
346
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 1 times.
 2002 for (size_type i = 0; i < 1000; ++i) {
347
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 2000 Configuration_t q(cs.shoot());
348
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 2000 vector6_t error;
349
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 2000 solver.rightHandSideFromConfig(q);
350
9/18
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1000 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1000 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1000 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1000 times.
✗ Branch 27 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1000 times.
 2000 BOOST_CHECK(solver.isSatisfied(q, error));
351 2000 }
352
353 // Create grasp constraint with one degree of freedom in rotation along z
354
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 mask = {true, true, true, true, true, false};
355 ImplicitPtr_t c1(Implicit::create(
356
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 4 RelativeTransformationR3xSO3::create("RelativeTransformationR3xSO3",
357 robot, j1, j2, tf1, tf2),
358
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 8 6 * EqualToZero, mask));
359
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 solver::BySubstitution s1(robot->configSpace());
360
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 s1.errorThreshold(1e-10);
361
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 s1.add(c1);
362 // Create grasp + complement as an explicit constraint
363 ExplicitPtr_t c2(
364
2/4
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 6 explicit_::RelativePose::create("ExplicitRelativePose", robot, j1, j2,
365
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 6 tf1, tf2, 5 * EqualToZero << Equality));
366
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 solver::BySubstitution s2(robot->configSpace());
367 2 s2.errorThreshold(1e-4);
368
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 s2.add(c2);
369
370
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 2 for (size_type i = 0; i < 0; ++i) {
371 ✗ Configuration_t q_near(cs.shoot());
372 ✗ Configuration_t q_new(cs.shoot());
373 ✗ if (i == 0) {
374 // These configuration reproduce a numerical issue encountered with
375 // benhmark romeo-placard.
376 // If computation was exact, any configuration satisfying c2 should
377 // satisfy c1.
378 // Configuration q_new below satisfies c2 but not c1.
379 ✗ q_near << 0.18006349590534418, 0.3627623741970175, 0.9567759630330663,
380 ✗ 0.044416054309488175, 0.31532356328825556, 0.4604329042168087,
381 ✗ 0.8286131819306576, 0.45813483973344404, 0.23514459283216355,
382 ✗ 0.7573015903787429, 0.8141495491430896, 0.1383820163733335,
383 ✗ 0.3806970356973106, 0.4160296818567576;
384 ✗ q_new << 0.16026892741853033, 0.33925098736439646, 0.8976880203169203,
385 ✗ -0.040130835169737825, 0.37473431876437147, 0.4405275981290593,
386 ✗ 0.8148000624051422, 0.43787674119234027, 0.18316291571416676,
387 ✗ 0.7189377922181226, 0.7699579340925136, 0.1989432638510445,
388 ✗ 0.35960786236482944, 0.4881275886709128;
389 }
390 ✗ s2.rightHandSideFromConfig(q_near);
391 ✗ vector6_t error;
392 ✗ BOOST_CHECK(s1.isSatisfied(q_near, error));
393 hppDout(info, error);
394 ✗ BOOST_CHECK(s2.isSatisfied(q_near, error));
395 hppDout(info, error);
396 ✗ BOOST_CHECK(s1.isSatisfied(q_new, error));
397 hppDout(info, error);
398 ✗ BOOST_CHECK(s2.isSatisfied(q_new, error));
399 hppDout(info, error);
400
401 hppDout(info, s1);
402 hppDout(info, s2);
403 }
404 2 }
405
406
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(equality) {
407 DevicePtr_t device = hpp::pinocchio::unittest::makeDevice(
408
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 hpp::pinocchio::unittest::HumanoidSimple);
409
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 JointPtr_t ee1 = device->getJointByName("lleg5_joint"),
410
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 ee2 = device->getJointByName("rleg5_joint");
411
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
 2 BOOST_REQUIRE(device);
412 2 BasicConfigurationShooter cs(device);
413
414
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 device->currentConfiguration(cs.shoot());
415
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 device->computeForwardKinematics(JOINT_POSITION);
416
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Transform3s tf1(ee1->currentTransformation());
417
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Transform3s tf2(ee2->currentTransformation());
418
419 2 std::vector<DifferentiableFunctionPtr_t> functions;
420
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 functions.push_back(Orientation::create("Orientation", device, ee2, tf2));
421
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 functions.push_back(RelativeTransformation::create("Orientation", device, ee1,
422 ee2, tf1, tf2));
423
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 functions.push_back(RelativeTransformation::create(
424 "RelativeTransformation", device, ee1, ee2, tf1, tf2));
425
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 functions.push_back(RelativeTransformation::create(
426 "RelativeTransformation", device, ee1, ee2, tf1, tf2));
427
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 functions.push_back(RelativeTransformation::create(
428 "othername_____________", device, ee1, ee2, tf1, tf2));
429 // functions[2] and functions[3] are meant to have the same value with
430 // different pointers
431
432
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 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.
 2 BOOST_CHECK(functions[2].get() !=
433 functions[3].get()); // boost implementation for ==
434
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 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.
 2 BOOST_CHECK(*functions[2] ==
435 *functions[3]); // uses operator== defined in DiffFunc
436
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 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.
 2 BOOST_CHECK(*functions[0] != *functions[2]); // a lot of things are different
437
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 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.
 2 BOOST_CHECK(*functions[2] != *functions[4]); // only the names are different
438
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 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.
 2 BOOST_CHECK(*functions[0] != *functions[1]); // only the names are equal
439 2 }
440
441 /* Create a robot with the following kinematic chain. All joints are
442 translations along x.
443
444 universe
445 |Px
446 test_x
447 /Px \Px
448 joint_a0 joint_b0
449 |Px |Px
450 joint_a1 joint_b1
451 |FF
452 joint_b2
453
454 */
455 1 DevicePtr_t createRobot() {
456 std::string urdf(
457 "<robot name='test'>"
458 "<link name='base_link'/>"
459 "<link name='link_test_x'/>"
460 "<joint name='test_x' type='prismatic'>"
461 "<parent link='base_link'/>"
462 "<child link='link_test_x'/>"
463 "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
464 "</joint>"
465
466 "<link name='link_a0'/>"
467 "<link name='link_a1'/>"
468 "<joint name='joint_a0' type='prismatic'>"
469 "<parent link='link_test_x'/>"
470 "<child link='link_a0'/>"
471 "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
472 "</joint>"
473 "<joint name='joint_a1' type='prismatic'>"
474 "<parent link='link_a0'/>"
475 "<child link='link_a1'/>"
476 "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
477 "</joint>"
478
479 "<link name='link_b0'/>"
480 "<link name='link_b1'/>"
481 "<link name='link_b2'/>"
482 "<joint name='joint_b0' type='prismatic'>"
483 "<parent link='link_test_x'/>"
484 "<child link='link_b0'/>"
485 "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
486 "</joint>"
487 "<joint name='joint_b1' type='prismatic'>"
488 "<parent link='link_b0'/>"
489 "<child link='link_b1'/>"
490 "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
491 "</joint>"
492 "<joint name='joint_b2' type='floating'>"
493 "<parent link='link_b1'/>"
494 "<child link='link_b2'/>"
495 "</joint>"
496
497
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "</robot>");
498
499
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 DevicePtr_t robot = Device::create("test");
500
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 1 loadModelFromString(robot, 0, "", "anchor", urdf, "");
501 2 return robot;
502 1 }
503
504
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(dependsOnRelPoseBetween) {
505
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 DevicePtr_t device = createRobot();
506
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
 2 BOOST_REQUIRE(device);
507
508
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 JointPtr_t ee1 = device->getJointByName("joint_a1"),
509
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 ee2 = device->getJointByName("joint_b2");
510
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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
 2 BOOST_REQUIRE(device);
511 // ensure that the joint indices are as expected
512
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 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.
 2 BOOST_REQUIRE(ee1->index() < ee2->index());
513
514
3/6
✓ 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.
 2 device->currentConfiguration(device->neutralConfiguration());
515
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 device->computeForwardKinematics(JOINT_POSITION);
516
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Transform3s tf1(ee1->currentTransformation());
517
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Transform3s tf2(ee2->currentTransformation());
518
519 2 DifferentiableFunctionPtr_t function;
520 2 std::pair<JointConstPtr_t, JointConstPtr_t> joints;
521 2 std::pair<JointConstPtr_t, JointConstPtr_t> jointsConstrained;
522 2 ImplicitPtr_t constraint;
523
524
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 2 function = Orientation::create("Orientation", device, ee2, tf2);
525
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 joints = function->dependsOnRelPoseBetween(nullptr);
526
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.
 2 BOOST_CHECK(!joints.first);
527
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(joints.second->index(), ee2->index());
528 // constraint does not fully constrain the relative pose
529 // since it only involves the orientation
530
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 constraint = Implicit::create(
531
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
532
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 6 std::vector<bool>(function->outputSpace()->nv(), true));
533
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
534
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.
 2 BOOST_CHECK(!jointsConstrained.first);
535
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.
 2 BOOST_CHECK(!jointsConstrained.second);
536
537
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 2 function = Position::create("Position", device, ee2, tf2, tf1);
538
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 joints = function->dependsOnRelPoseBetween(nullptr);
539
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.
 2 BOOST_CHECK(!joints.first);
540
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(joints.second->index(), ee2->index());
541 // constraint does not fully constrain the relative pose
542 // since it only involves the position
543
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 constraint = Implicit::create(
544
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
545
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 6 std::vector<bool>(function->outputSpace()->nv(), true));
546
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
547
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.
 2 BOOST_CHECK(!jointsConstrained.first);
548
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.
 2 BOOST_CHECK(!jointsConstrained.second);
549
550
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 2 function = Transformation::create("Transformation", device, ee1, tf1);
551
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 joints = function->dependsOnRelPoseBetween(nullptr);
552
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.
 2 BOOST_CHECK(!joints.first);
553
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(joints.second->index(), ee1->index());
554 // constraint does not fully constrain the relative pose
555 // since the mask is not full
556
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 constraint = Implicit::create(
557
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
558
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 6 std::vector<bool>(function->outputSpace()->nv(), false));
559
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
560
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.
 2 BOOST_CHECK(!jointsConstrained.first);
561
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.
 2 BOOST_CHECK(!jointsConstrained.second);
562 // constraint fully constrains the relative pose
563
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 constraint = Implicit::create(
564
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
565
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 6 std::vector<bool>(function->outputSpace()->nv(), true));
566
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
567
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.
 2 BOOST_CHECK(!jointsConstrained.first);
568
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(jointsConstrained.second->index(), ee1->index());
569
570 function =
571
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 RelativeOrientation::create("RelativeOrientation", device, ee1, ee2, tf1);
572
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 joints = function->dependsOnRelPoseBetween(nullptr);
573
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(joints.first->index(), ee1->index());
574
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(joints.second->index(), ee2->index());
575 // constraint does not fully constrain the relative pose
576
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 constraint = Implicit::create(
577
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
578
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 6 std::vector<bool>(function->outputSpace()->nv(), true));
579
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
580
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.
 2 BOOST_CHECK(!jointsConstrained.first);
581
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.
 2 BOOST_CHECK(!jointsConstrained.second);
582
583 function =
584
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 RelativePosition::create("RelativePosition", device, ee1, ee2, tf1, tf2);
585
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 joints = function->dependsOnRelPoseBetween(nullptr);
586
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(joints.first->index(), ee1->index());
587
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(joints.second->index(), ee2->index());
588 // constraint does not fully constrain the relative pose
589
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 constraint = Implicit::create(
590
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
591
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 6 std::vector<bool>(function->outputSpace()->nv(), true));
592
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
593
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.
 2 BOOST_CHECK(!jointsConstrained.first);
594
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.
 2 BOOST_CHECK(!jointsConstrained.second);
595
596
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 4 function = RelativeTransformation::create("RelativeTransformation", device,
597 2 ee1, ee2, tf1, tf2);
598
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 joints = function->dependsOnRelPoseBetween(nullptr);
599
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(joints.first->index(), ee1->index());
600
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(joints.second->index(), ee2->index());
601 // constraint does not fully constrain the relative pose
602 // since mask is not full
603
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 constraint = Implicit::create(
604
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
605
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 6 std::vector<bool>(function->outputSpace()->nv(), false));
606
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
607
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.
 2 BOOST_CHECK(!jointsConstrained.first);
608
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.
 2 BOOST_CHECK(!jointsConstrained.second);
609 // constraint fully constrains the relative pose
610
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 constraint = Implicit::create(
611
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
612
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 6 std::vector<bool>(function->outputSpace()->nv(), true));
613
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
614
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(jointsConstrained.first->index(), ee1->index());
615
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(jointsConstrained.second->index(), ee2->index());
616
617
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 8 function = RelativeOrientation::create("RelativeOrientation", device, ee1,
618 6 JointPtr_t(), tf1);
619
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 joints = function->dependsOnRelPoseBetween(nullptr);
620
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.
 2 BOOST_CHECK(!joints.first);
621
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(joints.second->index(), ee1->index());
622
623
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 8 function = RelativePosition::create("RelativePosition", device, ee1,
624 6 JointPtr_t(), tf1, tf2);
625
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 joints = function->dependsOnRelPoseBetween(nullptr);
626
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.
 2 BOOST_CHECK(!joints.first);
627
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(joints.second->index(), ee1->index());
628
629
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 8 function = RelativeTransformationR3xSO3::create(
630 6 "RelativeTransformationR3xSO3", device, ee1, JointPtr_t(), tf1, tf2);
631
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 joints = function->dependsOnRelPoseBetween(nullptr);
632
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.
 2 BOOST_CHECK(!joints.first);
633
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(joints.second->index(), ee1->index());
634 // constraint fully constrains the relative pose
635
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 constraint = Implicit::create(
636
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 function, ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
637
1/2
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 6 std::vector<bool>(function->outputSpace()->nv(), true));
638
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
639
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.
 2 BOOST_CHECK(!jointsConstrained.first);
640
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(jointsConstrained.second->index(), ee1->index());
641
642 /// test the locked joint constraint as well
643
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 constraint = LockedJoint::create(ee2, ee2->configurationSpace()->neutral());
644
645
2/4
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 std::cout << constraint->functionPtr()->name() << std::endl;
646
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 joints = constraint->functionPtr()->dependsOnRelPoseBetween(device);
647
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 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.
 2 BOOST_CHECK_EQUAL(joints.first->index(), ee2->parentJoint()->index());
648
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(joints.second->index(), ee2->index());
649 // constraint fully locks the joint
650
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
651
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(jointsConstrained.second->index(), ee2->index());
652 2 }
653