GCC Code Coverage Report

Directory: ./
File: src/explicit/relative-transformation.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 63 133 47.4%
Branches: 82 436 18.8%
Line Branch Exec Source
1 // Copyright (c) 2017 - 2018, 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 #include <boost/serialization/utility.hpp>
30 #include <boost/serialization/vector.hpp>
31 #include <boost/serialization/weak_ptr.hpp>
32 #include <hpp/constraints/explicit/relative-transformation.hh>
33 #include <hpp/constraints/serialization.hh>
34 #include <hpp/pinocchio/device.hh>
35 #include <hpp/util/serialization.hh>
36 #include <pinocchio/serialization/se3.hpp>
37 #include <pinocchio/spatial/explog.hpp>
38 #include <pinocchio/spatial/skew.hpp>
39
40 #include "../serialization.hh"
41
42 namespace hpp {
43 namespace constraints {
44 namespace explicit_ {
45 namespace {
46 typedef JointJacobian_t::ConstNRowsBlockXpr<3>::Type ConstHalfJacobian_t;
47 ✗ inline ConstHalfJacobian_t omega(const JointJacobian_t& j) {
48 ✗ return j.bottomRows<3>();
49 }
50 ✗ inline ConstHalfJacobian_t trans(const JointJacobian_t& j) {
51 ✗ return j.topRows<3>();
52 }
53
54 16 void inputVariable(JointConstPtr_t joint, std::vector<bool>& conf,
55 std::vector<bool>& vel) {
56
5/6
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 16 times.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✓ Branch 8 taken 16 times.
 22 while (joint && joint->index() != 0) {
57
2/2
✓ Branch 2 taken 42 times.
✓ Branch 3 taken 6 times.
 48 for (size_type i = 0; i < joint->configSize(); ++i)
58
2/4
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
 42 conf[joint->rankInConfiguration() + i] =
59 84 !conf[joint->rankInConfiguration() + i];
60
2/2
✓ Branch 2 taken 36 times.
✓ Branch 3 taken 6 times.
 42 for (size_type i = 0; i < joint->numberDof(); ++i)
61
2/4
✓ Branch 6 taken 36 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 36 times.
✗ Branch 10 not taken.
 36 vel[joint->rankInVelocity() + i] = !vel[joint->rankInVelocity() + i];
62 hppDout(info, "Adding joint " << joint->name() << " as input variable.");
63 6 joint = joint->parentJoint();
64 }
65 16 }
66
67 16 BlockIndex::segments_t vectorOfBoolToIntervals(std::vector<bool>& v) {
68 16 BlockIndex::segments_t ret;
69
2/2
✓ Branch 1 taken 286 times.
✓ Branch 2 taken 16 times.
 302 for (std::size_t i = 0; i < v.size(); ++i)
70
4/6
✓ Branch 1 taken 286 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78 times.
✓ Branch 5 taken 208 times.
✓ Branch 8 taken 78 times.
✗ Branch 9 not taken.
 286 if (v[i]) ret.push_back(BlockIndex::segment_t(i, 1));
71
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 BlockIndex::shrink(ret);
72 16 return ret;
73 }
74
75 8 BlockIndex::segments_t jointConfInterval(JointConstPtr_t j) {
76 return BlockIndex::segments_t(
77
3/6
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 8 times.
✗ Branch 12 not taken.
 16 1, BlockIndex::segment_t(j->rankInConfiguration(), j->configSize()));
78 }
79 8 BlockIndex::segments_t jointVelInterval(JointConstPtr_t j) {
80 return BlockIndex::segments_t(
81
3/6
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 8 times.
✗ Branch 12 not taken.
 16 1, BlockIndex::segment_t(j->rankInVelocity(), j->numberDof()));
82 }
83 } // namespace
84
85 8 RelativeTransformationPtr_t RelativeTransformation::create(
86 const std::string& name, const DevicePtr_t& robot,
87 const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
88 const Transform3s& frame1, const Transform3s& frame2) {
89
2/4
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
 8 std::vector<bool> conf(robot->configSize(), false);
90
2/4
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
 8 std::vector<bool> vel(robot->numberDof(), false);
91
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 inputVariable(joint1, conf, vel);
92
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
 8 inputVariable(joint2->parentJoint(), conf, vel);
93
94 RelativeTransformation* ptr = new RelativeTransformation(
95 name, robot, joint1, joint2, frame1, frame2,
96
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 24 vectorOfBoolToIntervals(conf), jointConfInterval(joint2),
97
5/10
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
 16 vectorOfBoolToIntervals(vel), jointVelInterval(joint2));
98
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 RelativeTransformationPtr_t shPtr(ptr);
99 8 ptr->init(shPtr);
100 16 return shPtr;
101 8 }
102
103 8 RelativeTransformation::RelativeTransformation(
104 const std::string& name, const DevicePtr_t& robot,
105 const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
106 const Transform3s& frame1, const Transform3s& frame2,
107 const segments_t inConf, const segments_t outConf, const segments_t inVel,
108 8 const segments_t outVel, std::vector<bool> /*mask*/)
109 : DifferentiableFunction(BlockIndex::cardinal(inConf),
110 BlockIndex::cardinal(inVel),
111
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 pinocchio::LiegroupSpace::SE3(), name),
112 8 robot_(robot),
113
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 parentJoint_(joint2->parentJoint()),
114 8 joint1_(joint1),
115 8 joint2_(joint2),
116
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 frame1_(frame1),
117
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 frame2_(frame2),
118
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 inConf_(inConf),
119
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 inVel_(inVel),
120
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 outConf_(outConf),
121
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 outVel_(outVel),
122
9/18
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 8 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 8 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 8 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 8 times.
✗ Branch 30 not taken.
 32 F1inJ1_invF2inJ2_(frame1 * frame2.inverse()) {}
123
124 6800 void RelativeTransformation::forwardKinematics(vectorIn_t arg) const {
125
1/2
✓ Branch 4 taken 6800 times.
✗ Branch 5 not taken.
 6800 qsmall_ = inConf_.rview(robot_->currentConfiguration());
126
2/2
✓ Branch 1 taken 199 times.
✓ Branch 2 taken 6601 times.
 6800 if (qsmall_ != arg) {
127 199 q_ = robot_->currentConfiguration();
128
1/2
✓ Branch 2 taken 199 times.
✗ Branch 3 not taken.
 199 inConf_.lview(q_) = arg;
129
1/2
✓ Branch 3 taken 199 times.
✗ Branch 4 not taken.
 199 robot_->currentConfiguration(q_);
130 }
131 6800 robot_->computeForwardKinematics(pinocchio::JOINT_POSITION);
132 6800 }
133
134 6800 void RelativeTransformation::impl_compute(LiegroupElementRef result,
135 vectorIn_t argument) const {
136
2/4
✓ Branch 1 taken 6800 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6800 times.
✗ Branch 5 not taken.
 6800 forwardKinematics(argument);
137
138
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 6800 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 6800 bool hasParent = (parentJoint_ && parentJoint_->index() > 0);
139
140 // J1 * M1/J1 = J2 * M2/J2
141 // J2 = J1 * M1/J1 * M2/J2^{-1}
142 // J2 = J2_{parent} * T
143 // T = J2_{parent}^{-1} * J2
144 // T = J2_{parent}^{-1} * J1 * F1/J1 * F2/J2^{-1}
145
1/2
✓ Branch 1 taken 6800 times.
✗ Branch 2 not taken.
 6800 Transform3s freeflyerPose;
146
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6800 times.
 6800 if (!joint1_)
147 ✗ freeflyerPose = F1inJ1_invF2inJ2_;
148 else
149
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 freeflyerPose = joint1_->currentTransformation() * F1inJ1_invF2inJ2_;
150
151
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6800 times.
 6800 if (hasParent)
152 ✗ freeflyerPose = parentJoint_->currentTransformation().actInv(freeflyerPose);
153
154
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 freeflyerPose = joint2_->positionInParentFrame().actInv(freeflyerPose);
155
156 typedef Transform3s::Quaternion Q_t;
157
3/6
✓ Branch 1 taken 6800 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6800 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6800 times.
✗ Branch 9 not taken.
 6800 result.vector().head<3>() = freeflyerPose.translation();
158
4/8
✓ Branch 1 taken 6800 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6800 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 6800 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6800 times.
✗ Branch 13 not taken.
 6800 result.vector().tail<4>() = Q_t(freeflyerPose.rotation()).coeffs();
159 6800 }
160
161 ✗ void RelativeTransformation::impl_jacobian(matrixOut_t jacobian,
162 vectorIn_t arg) const {
163 ✗ LiegroupElement result(outputSpace());
164 ✗ impl_compute(result, arg);
165 ✗ Configuration_t q(robot_->currentConfiguration());
166 ✗ outConf_.lview(q) = result.vector();
167 ✗ robot_->currentConfiguration(q);
168 ✗ robot_->computeForwardKinematics(pinocchio::JOINT_POSITION |
169 pinocchio::JACOBIAN);
170
171 ✗ bool absolute = !joint1_;
172 ✗ bool hasParent = (parentJoint_ && parentJoint_->index() > 0);
173
174 ✗ static const JointJacobian_t Jabs;
175 ✗ const JointJacobian_t& J1(absolute ? Jabs : joint1_->jacobian());
176 // const JointJacobian_t& J2_parent (parentJoint_->jacobian());
177
178 ✗ const matrix3_t& R1(absolute ? matrix3_t::Identity().eval()
179 ✗ : joint1_->currentTransformation().rotation());
180 ✗ const matrix3_t& R2(joint2_->currentTransformation().rotation());
181 const matrix3_t& R2_inParentFrame(
182 ✗ joint2_->positionInParentFrame().rotation());
183
184 const vector3_t& t1(absolute
185 ✗ ? vector3_t::Zero().eval()
186 ✗ : joint1_->currentTransformation().translation());
187
188 matrix3_t cross1 = ::pinocchio::skew(
189 ✗ (R1 * F1inJ1_invF2inJ2_.translation()).eval()),
190 ✗ cross2;
191 ✗ if (hasParent) {
192 const vector3_t& t2_parent(
193 ✗ parentJoint_->currentTransformation().translation());
194 ✗ cross2 = ::pinocchio::skew((t2_parent - t1).eval());
195
196 ✗ if (absolute)
197 ✗ J2_parent_minus_J1_.noalias() = parentJoint_->jacobian();
198 else
199 ✗ J2_parent_minus_J1_.noalias() = parentJoint_->jacobian() - J1;
200 } else {
201 ✗ cross2 = -::pinocchio::skew(t1);
202 // J2_parent_minus_J1_ = - J1;
203 }
204
205 // Express velocity of J1 * M1/J1 * M2/J2^{-1} in J2_{parent}.
206 ✗ if (hasParent) {
207 const matrix3_t& R2_parent(
208 ✗ parentJoint_->currentTransformation().rotation());
209 ✗ const JointJacobian_t& J2_parent(parentJoint_->jacobian());
210
211 ✗ tmpJac_.noalias() =
212 ✗ (R2_inParentFrame.transpose() * R2_parent.transpose()) *
213 ✗ (cross1 * (omega(J2_parent_minus_J1_))-cross2 * omega(J2_parent) -
214 ✗ trans(J2_parent_minus_J1_));
215 ✗ jacobian.topRows<3>() = inVel_.rview(tmpJac_);
216 } else {
217 ✗ if (absolute)
218 ✗ jacobian.topRows<3>().setZero();
219 else {
220 ✗ tmpJac_.noalias() =
221 ✗ R2.transpose() * ((-cross1 * R1) * omega(J1) + R1 * trans(J1));
222 ✗ jacobian.topRows<3>() = inVel_.rview(tmpJac_);
223 }
224 }
225
226 ✗ if (hasParent) {
227 const matrix3_t& R2_parent(
228 ✗ parentJoint_->currentTransformation().rotation());
229 ✗ const JointJacobian_t& J2_parent(parentJoint_->jacobian());
230
231 // J = p2RT2 * 0RTp2 * [ p2
232 ✗ tmpJac_.noalias() = (R2.transpose() * R2_parent) * omega(J2_parent);
233 ✗ if (!absolute) tmpJac_.noalias() -= (R2.transpose() * R1) * omega(J1);
234 ✗ jacobian.bottomRows<3>() = inVel_.rview(tmpJac_);
235 } else {
236 ✗ if (absolute)
237 ✗ jacobian.bottomRows<3>().setZero();
238 else {
239 ✗ tmpJac_.noalias() = (R2.transpose() * R1) * omega(J1);
240 ✗ jacobian.bottomRows<3>() = inVel_.rview(tmpJac_);
241 }
242 }
243 }
244
245 template <class Archive>
246 ✗ void RelativeTransformation::serialize(Archive& ar,
247 const unsigned int version) {
248 (void)version;
249 ✗ ar& boost::serialization::make_nvp(
250 ✗ "base", boost::serialization::base_object<DifferentiableFunction>(*this));
251 ✗ ar& BOOST_SERIALIZATION_NVP(robot_);
252 ✗ internal::serialize_joint(ar, "parentJoint_", parentJoint_);
253 ✗ internal::serialize_joint(ar, "joint1_", joint1_);
254 ✗ internal::serialize_joint(ar, "joint2_", joint2_);
255 ✗ ar& BOOST_SERIALIZATION_NVP(frame1_);
256 ✗ ar& BOOST_SERIALIZATION_NVP(frame2_);
257
258 ✗ ar& BOOST_SERIALIZATION_NVP(inConf_);
259 ✗ ar& BOOST_SERIALIZATION_NVP(inVel_);
260 ✗ ar& BOOST_SERIALIZATION_NVP(outConf_);
261 ✗ ar& BOOST_SERIALIZATION_NVP(outVel_);
262 ✗ ar& BOOST_SERIALIZATION_NVP(F1inJ1_invF2inJ2_);
263 ✗ ar& BOOST_SERIALIZATION_NVP(weak_);
264 }
265
266 HPP_SERIALIZATION_IMPLEMENT(RelativeTransformation);
267 } // namespace explicit_
268 } // namespace constraints
269 } // namespace hpp
270
271 BOOST_CLASS_EXPORT_IMPLEMENT(
272 hpp::constraints::explicit_::RelativeTransformation)
273