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 |