GCC Code Coverage Report

Directory: ./
File: tests/relative-motion.cc
Date: 2024-08-10 11:29:48
Exec Total Coverage
Lines: 85 85 100.0%
Branches: 330 660 50.0%
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 BOOST_TEST_MODULE relativeMotion
30 #include <boost/mpl/list.hpp>
31 #include <boost/test/included/unit_test.hpp>
32 #include <hpp/constraints/comparison-types.hh>
33 #include <hpp/constraints/explicit/relative-pose.hh>
34 #include <hpp/constraints/generic-transformation.hh>
35 #include <hpp/constraints/implicit.hh>
36 #include <hpp/constraints/locked-joint.hh>
37 #include <hpp/core/config-projector.hh>
38 #include <hpp/core/constraint-set.hh>
39 #include <hpp/core/relative-motion.hh>
40 #include <hpp/pinocchio/configuration.hh>
41 #include <hpp/pinocchio/device.hh>
42 #include <hpp/pinocchio/joint.hh>
43 #include <hpp/pinocchio/urdf/util.hh>
44 #include <pinocchio/fwd.hpp>
45
46 using hpp::constraints::Equality;
47 using hpp::constraints::EqualToZero;
48 using hpp::constraints::Implicit;
49 using hpp::constraints::RelativeTransformation;
50 using hpp::constraints::explicit_::RelativePose;
51
52 using namespace hpp::core;
53 using namespace hpp::pinocchio;
54
55 bool verbose = true;
56
57 #define TOSTR(x) \
58 static_cast<std::ostringstream&>((std::ostringstream() << x)).str()
59
60 /* Create a robot with the following kinematic chain. All joints are
61 translations along x.
62
63 universe
64 |Px
65 test_x
66 /Px \Px
67 joint_a0 joint_b0
68 |Px |Px
69 joint_a1 joint_b1
70 |FF
71 joint_b2
72
73 */
74 1 DevicePtr_t createRobot() {
75 std::string urdf(
76 "<robot name='test'>"
77 "<link name='base_link'/>"
78 "<link name='link_test_x'/>"
79 "<joint name='test_x' type='prismatic'>"
80 "<parent link='base_link'/>"
81 "<child link='link_test_x'/>"
82 "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
83 "</joint>"
84
85 "<link name='link_a0'/>"
86 "<link name='link_a1'/>"
87 "<joint name='joint_a0' type='prismatic'>"
88 "<parent link='link_test_x'/>"
89 "<child link='link_a0'/>"
90 "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
91 "</joint>"
92 "<joint name='joint_a1' type='prismatic'>"
93 "<parent link='link_a0'/>"
94 "<child link='link_a1'/>"
95 "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
96 "</joint>"
97
98 "<link name='link_b0'/>"
99 "<link name='link_b1'/>"
100 "<link name='link_b2'/>"
101 "<joint name='joint_b0' type='prismatic'>"
102 "<parent link='link_test_x'/>"
103 "<child link='link_b0'/>"
104 "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
105 "</joint>"
106 "<joint name='joint_b1' type='prismatic'>"
107 "<parent link='link_b0'/>"
108 "<child link='link_b1'/>"
109 "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
110 "</joint>"
111 "<joint name='joint_b2' type='floating'>"
112 "<parent link='link_b1'/>"
113 "<child link='link_b2'/>"
114 "</joint>"
115
116
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "</robot>");
117
118
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");
119
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 urdf::loadModelFromString(robot, 0, "", "anchor", urdf, "");
120 2 return robot;
121 1 }
122
123 3 void lockJoint(ConfigProjectorPtr_t proj, DevicePtr_t dev, std::string name) {
124
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 JointPtr_t j = dev->getJointByName(name);
125
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 LiegroupSpacePtr_t space(j->configurationSpace());
126
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 LiegroupElement lge(space);
127
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 Configuration_t q = dev->currentConfiguration();
128
4/8
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
 3 lge.vector() = q.segment(j->rankInConfiguration(), j->configSize());
129 3 lge.check();
130
3/6
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 3 proj->add(LockedJoint::create(dev->getJointByName(name), lge));
131 3 }
132
133 struct Jidx {
134 DevicePtr_t dev;
135 35 size_type operator()(const std::string& jointname) {
136 35 return Joint::index(dev->getJointByName(jointname));
137 }
138 };
139
140
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(relativeMotion) {
141
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 DevicePtr_t dev = createRobot();
142
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(dev);
143 2 Jidx jointid;
144 2 jointid.dev = dev;
145
146
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 JointConstPtr_t ja1 = dev->getJointByName("joint_a1"),
147
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 jb2 = dev->getJointByName("joint_b2");
148
149
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 RelativeMotion::matrix_type m;
150
151
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 ConfigProjectorPtr_t proj = ConfigProjector::create(dev, "test", 1e-3, 10);
152
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 ConstraintSetPtr_t constraints = ConstraintSet::create(dev, "test");
153
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 constraints->addConstraint(proj);
154
155 // root, x, a0, a1, b0, b1, b2
156 // 0, 1, 2, 3, 4, 5, 6
157
7/14
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 2 BOOST_CHECK_EQUAL(jointid("universe"), 0);
158
7/14
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 2 BOOST_CHECK_EQUAL(jointid("test_x"), 1);
159
7/14
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 2 BOOST_CHECK_EQUAL(jointid("joint_a0"), 2);
160
7/14
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 2 BOOST_CHECK_EQUAL(jointid("joint_a1"), 3);
161
7/14
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 2 BOOST_CHECK_EQUAL(jointid("joint_b0"), 4);
162
7/14
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 2 BOOST_CHECK_EQUAL(jointid("joint_b1"), 5);
163
7/14
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 2 BOOST_CHECK_EQUAL(jointid("joint_b2"), 6);
164
165 // Lock some joints
166
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 lockJoint(proj, dev, "joint_b1");
167
168
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 m = RelativeMotion::matrix(dev);
169
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 RelativeMotion::fromConstraint(m, dev, constraints);
170
171
10/20
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
 2 BOOST_CHECK_EQUAL(m(jointid("joint_b0"), jointid("joint_b1")),
172 RelativeMotion::Parameterized); // lock b1
173
174
4/8
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ 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 if (verbose) std::cout << '\n' << m << std::endl;
175
176 // Lock some joints
177
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 lockJoint(proj, dev, "joint_b2");
178
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 lockJoint(proj, dev, "joint_a1");
179
180
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 m = RelativeMotion::matrix(dev);
181
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 RelativeMotion::fromConstraint(m, dev, constraints);
182
183
10/20
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
 2 BOOST_CHECK_EQUAL(m(jointid("joint_a0"), jointid("joint_a1")),
184 RelativeMotion::Parameterized); // lock a1
185
10/20
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
 2 BOOST_CHECK_EQUAL(m(jointid("joint_b0"), jointid("joint_b1")),
186 RelativeMotion::Parameterized); // lock b1
187
10/20
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
 2 BOOST_CHECK_EQUAL(m(jointid("joint_b1"), jointid("joint_b2")),
188 RelativeMotion::Parameterized); // lock b2
189
190
10/20
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
 2 BOOST_CHECK_EQUAL(m(jointid("joint_b0"), jointid("joint_b2")),
191 RelativeMotion::Parameterized); // lock b1+b2
192
193
4/8
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ 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 if (verbose) std::cout << '\n' << m << std::endl;
194
195 /// Add a relative transformation
196
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 Configuration_t q = dev->neutralConfiguration();
197
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 dev->currentConfiguration(q);
198
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 dev->computeForwardKinematics(hpp::pinocchio::JOINT_POSITION);
199
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Transform3f tf1(ja1->currentTransformation());
200
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Transform3f tf2(jb2->currentTransformation());
201 // mask is not full, relative motion not fully constrained
202
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 proj->add(Implicit::create(
203
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.
 4 RelativeTransformation::create("joint_a1 <->joint_b2 not full", dev, ja1,
204 jb2, tf1, tf2),
205
3/6
✓ 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.
 4 EqualToZero << EqualToZero << 3 * Equality << EqualToZero,
206
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 4 std::vector<bool>(6, false)));
207
208
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 m = RelativeMotion::matrix(dev);
209
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 RelativeMotion::fromConstraint(m, dev, constraints);
210
211
10/20
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
 2 BOOST_CHECK_EQUAL(m(jointid("joint_a1"), jointid("joint_b2")),
212 RelativeMotion::Unconstrained);
213
10/20
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
 2 BOOST_CHECK_EQUAL(m(jointid("joint_a0"), jointid("joint_b2")),
214 RelativeMotion::Unconstrained);
215
10/20
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
 2 BOOST_CHECK_EQUAL(m(jointid("joint_b0"), jointid("joint_a1")),
216 RelativeMotion::Unconstrained);
217
10/20
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
 2 BOOST_CHECK_EQUAL(m(jointid("joint_b0"), jointid("joint_a0")),
218 RelativeMotion::Unconstrained);
219
220
4/8
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ 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 if (verbose) std::cout << '\n' << m << std::endl;
221 // full mask for relative transformation
222
2/4
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 4 proj->add(Implicit::create(
223
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.
 4 RelativeTransformation::create("joint_a1 <->joint_b2", dev, ja1, jb2, tf1,
224 tf2),
225
3/6
✓ 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.
 4 EqualToZero << EqualToZero << 3 * Equality << EqualToZero));
226
227
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 m = RelativeMotion::matrix(dev);
228
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 RelativeMotion::fromConstraint(m, dev, constraints);
229
230
10/20
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
 2 BOOST_CHECK_EQUAL(m(jointid("joint_a1"), jointid("joint_b2")),
231 RelativeMotion::Parameterized); // lock rt
232
10/20
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
 2 BOOST_CHECK_EQUAL(m(jointid("joint_a0"), jointid("joint_b2")),
233 RelativeMotion::Parameterized); // lock a1 + rt
234
10/20
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
 2 BOOST_CHECK_EQUAL(m(jointid("joint_b0"), jointid("joint_a1")),
235 RelativeMotion::Parameterized); // lock b1+b2+rt
236
10/20
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
 2 BOOST_CHECK_EQUAL(m(jointid("joint_b0"), jointid("joint_a0")),
237 RelativeMotion::Parameterized); // lock b1+b2+rt+a1
238
239
4/8
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ 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 if (verbose) std::cout << '\n' << m << std::endl;
240
241
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 proj = ConfigProjector::create(dev, "test", 1e-3, 10);
242
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 constraints = ConstraintSet::create(dev, "test");
243
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 constraints->addConstraint(proj);
244
3/6
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 4 proj->add(RelativePose::create("explicit joint_a1 <-> joint_b2", dev, ja1,
245 jb2, tf1, tf2,
246
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 2 * Equality << 3 * EqualToZero << Equality));
247
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 m = RelativeMotion::matrix(dev);
248
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 RelativeMotion::fromConstraint(m, dev, constraints);
249
10/20
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
 2 BOOST_CHECK_EQUAL(m(jointid("joint_a1"), jointid("joint_b2")),
250 RelativeMotion::Parameterized); // lock ert
251
252
4/8
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ 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 if (verbose) std::cout << '\n' << m << std::endl;
253 2 }
254