GCC Code Coverage Report

Directory: ./
File: src/explicit/relative-pose.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 59 83 71.1%
Branches: 68 236 28.8%
Line Branch Exec Source
1 // Copyright (c) 2018, LAAS-CNRS
2 // Authors: Florent Lamiraux
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 #include <boost/serialization/weak_ptr.hpp>
30 #include <hpp/constraints/explicit/relative-pose.hh>
31 #include <hpp/constraints/explicit/relative-transformation.hh>
32 #include <hpp/pinocchio/device.hh>
33 #include <hpp/util/serialization.hh>
34 #include <pinocchio/serialization/se3.hpp>
35
36 #include "../src/explicit/input-configurations.hh"
37
38 namespace hpp {
39 namespace constraints {
40 namespace explicit_ {
41 LiegroupSpacePtr_t RelativePose::SE3(LiegroupSpace::SE3());
42 LiegroupSpacePtr_t RelativePose::R3xSO3(LiegroupSpace::R3xSO3());
43
44 8 RelativePosePtr_t RelativePose::create(
45 const std::string& name, const DevicePtr_t& robot,
46 const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
47 const Transform3s& frame1, const Transform3s& frame2,
48 ComparisonTypes_t comp, std::vector<bool> mask) {
49
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 6 times.
 8 if (mask.empty()) {
50
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 mask = std::vector<bool>(6, true);
51 }
52 RelativePose* ptr(new RelativePose(name, robot, joint1, joint2, frame1,
53
4/8
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
 8 frame2, comp, mask));
54
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 RelativePosePtr_t shPtr(ptr);
55 8 RelativePoseWkPtr_t wkPtr(shPtr);
56
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 ptr->init(wkPtr);
57 16 return shPtr;
58 8 }
59
60 3 RelativePosePtr_t RelativePose::createCopy(const RelativePosePtr_t& other) {
61
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 RelativePose* ptr(new RelativePose(*other));
62
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 RelativePosePtr_t shPtr(ptr);
63 3 RelativePoseWkPtr_t wkPtr(shPtr);
64
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 ptr->init(wkPtr);
65 6 return shPtr;
66 3 }
67
68
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 ImplicitPtr_t RelativePose::copy() const { return createCopy(weak_.lock()); }
69
70 6800 void RelativePose::outputValue(LiegroupElementRef result, vectorIn_t qin,
71 LiegroupElementConstRef rhs) const {
72
1/2
✓ Branch 6 taken 6800 times.
✗ Branch 7 not taken.
 6800 vector_t rhsExplicit(explicitFunction()->outputSpace()->nv());
73
3/6
✓ Branch 1 taken 6800 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6800 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6800 times.
✗ Branch 8 not taken.
 6800 implicitToExplicitRhs(rhs, rhsExplicit);
74
3/6
✓ Branch 3 taken 6800 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6800 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6800 times.
✗ Branch 10 not taken.
 6800 explicitFunction()->value(result, qin);
75
2/4
✓ Branch 1 taken 6800 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6800 times.
✗ Branch 5 not taken.
 6800 result += rhsExplicit;
76 6800 }
77
78 ✗ void RelativePose::jacobianOutputValue(vectorIn_t qin,
79 LiegroupElementConstRef f_value,
80 LiegroupElementConstRef rhs,
81 matrixOut_t jacobian) const {
82 // Todo: suppress dynamic allocation.
83 ✗ vector_t rhsExplicit(explicitFunction()->outputSpace()->nv());
84 ✗ explicitFunction()->jacobian(jacobian, qin);
85 ✗ if (rhs != rhs.space()->neutral()) {
86 ✗ implicitToExplicitRhs(rhs, rhsExplicit);
87 ✗ explicitFunction()
88 ✗ ->outputSpace()
89 ✗ ->dIntegrate_dq<pinocchio::DerivativeTimesInput>(f_value, rhsExplicit,
90 jacobian);
91 }
92 }
93
94 6800 void RelativePose::implicitToExplicitRhs(LiegroupElementConstRef implicitRhs,
95 vectorOut_t explicitRhs) const {
96
2/8
✓ Branch 4 taken 6800 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 6800 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 6800 assert(*(implicitRhs.space()) == *R3xSO3 || *(implicitRhs.space()) == *SE3);
97
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6800 times.
 6800 assert(explicitRhs.size() == 6);
98 // convert implicitRhs to Transform3s M1
99 Transform3s M1(
100
3/6
✓ Branch 2 taken 6800 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6800 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6800 times.
✗ Branch 9 not taken.
 6800 (Quaternion_t(implicitRhs.vector().tail<4>())).toRotationMatrix(),
101
2/4
✓ Branch 2 taken 6800 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6800 times.
✗ Branch 6 not taken.
 13600 implicitRhs.vector().head<3>());
102 // M2 = F_{2/J_2} M1 F_{2/J_2}^{-1}
103
3/6
✓ Branch 1 taken 6800 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6800 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6800 times.
✗ Branch 8 not taken.
 6800 Transform3s M2(frame2_ * M1 * frame2_.inverse());
104 // convert M2 to LiegroupElement p2
105
1/2
✓ Branch 1 taken 6800 times.
✗ Branch 2 not taken.
 6800 vector7_t v2;
106
3/6
✓ Branch 1 taken 6800 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6800 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6800 times.
✗ Branch 8 not taken.
 6800 v2.head<3>() = M2.translation();
107
4/8
✓ Branch 1 taken 6800 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6800 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 6800 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6800 times.
✗ Branch 12 not taken.
 6800 v2.tail<4>() = Quaternion_t(M2.rotation()).coeffs();
108
2/4
✓ Branch 1 taken 6800 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6800 times.
✗ Branch 5 not taken.
 6800 LiegroupElement p2(v2, SE3);
109
3/6
✓ Branch 2 taken 6800 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6800 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6800 times.
✗ Branch 9 not taken.
 6800 explicitRhs = p2 - SE3->neutral();
110 6800 }
111
112 ✗ void RelativePose::explicitToImplicitRhs(vectorIn_t explicitRhs,
113 LiegroupElementRef implicitRhs) const {
114 ✗ assert(*(implicitRhs.space()) == *(R3xSO3) || *(implicitRhs.space()) == *SE3);
115 ✗ assert(explicitRhs.size() == 6);
116 // p1 = exp_{SE(3)} (explicitRhs)
117 ✗ LiegroupElement p1(SE3->exp(explicitRhs));
118 // convert p1 to Transform3s M1
119 ✗ Transform3s M1((Quaternion_t(p1.vector().tail<4>())).toRotationMatrix(),
120 ✗ p1.vector().head<3>());
121 // M2 = F_{2/J_2}^{-1} M1 F_{2/J_2}
122 ✗ Transform3s M2(frame2_.inverse() * M1 * frame2_);
123 // convert M2 to LiegroupElement p2
124 ✗ vector7_t v2;
125 ✗ implicitRhs.vector().head<3>() = M2.translation();
126 ✗ implicitRhs.vector().tail<4>() = Quaternion_t(M2.rotation()).coeffs();
127 }
128
129 8 RelativePose::RelativePose(const std::string& name, const DevicePtr_t& robot,
130 const JointConstPtr_t& joint1,
131 const JointConstPtr_t& joint2,
132 const Transform3s& frame1, const Transform3s& frame2,
133 8 ComparisonTypes_t comp, std::vector<bool> mask)
134
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 : Explicit(RelativeTransformationR3xSO3::create(name, robot, joint1, joint2,
135 frame1, frame2,
136
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 16 std::vector<bool>(6, true)),
137
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 RelativeTransformation::create(name, robot, joint1, joint2,
138 frame1, frame2),
139
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 16 relativePose::inputConfVariables(robot, joint1, joint2),
140
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 16 relativePose::jointConfInterval(joint2),
141
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 16 relativePose::inputVelocityVariables(robot, joint1, joint2),
142
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 16 relativePose::jointVelInterval(joint2), comp, mask),
143 8 joint1_(joint1),
144 8 joint2_(joint2),
145
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 frame1_(frame1),
146
2/4
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
 48 frame2_(frame2) {
147
8/32
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 8 times.
✗ Branch 32 not taken.
✓ Branch 33 taken 8 times.
✓ Branch 35 taken 8 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 8 times.
✗ Branch 39 not taken.
✗ Branch 41 not taken.
✗ Branch 42 not taken.
✗ Branch 44 not taken.
✗ Branch 45 not taken.
✗ Branch 47 not taken.
✗ Branch 48 not taken.
✗ Branch 50 not taken.
✗ Branch 51 not taken.
 8 assert(
148 (*joint2->configurationSpace() == *pinocchio::LiegroupSpace::SE3()) ||
149 (*joint2->configurationSpace() == *pinocchio::LiegroupSpace::R3xSO3()));
150 8 }
151
152 3 RelativePose::RelativePose(const RelativePose& other)
153 : Explicit(other),
154 3 joint1_(other.joint1_),
155 3 joint2_(other.joint2_),
156
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 frame1_(other.frame1_),
157
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 frame2_(other.frame2_) {}
158
159 11 void RelativePose::init(RelativePoseWkPtr_t weak) {
160
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 Explicit::init(weak);
161 11 weak_ = weak;
162 11 }
163
164 template <class Archive>
165 ✗ void RelativePose::serialize(Archive& ar, const unsigned int version) {
166 (void)version;
167 ✗ ar& boost::serialization::make_nvp(
168 ✗ "base", boost::serialization::base_object<Explicit>(*this));
169 ✗ ar& BOOST_SERIALIZATION_NVP(frame1_);
170 ✗ ar& BOOST_SERIALIZATION_NVP(frame2_);
171 ✗ ar& BOOST_SERIALIZATION_NVP(weak_);
172 }
173
174 HPP_SERIALIZATION_IMPLEMENT(RelativePose);
175 } // namespace explicit_
176 } // namespace constraints
177 } // namespace hpp
178
179 BOOST_CLASS_EXPORT_IMPLEMENT(hpp::constraints::explicit_::RelativePose)
180