GCC Code Coverage Report


Directory: ./
File: include/hpp/constraints/generic-transformation.hh
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 71 72 98.6%
Branches: 31 48 64.6%

Line Branch Exec Source
1 //
2 // Copyright (c) 2016 CNRS
3 // Authors: Joseph Mirabel
4 //
5 //
6
7 // Redistribution and use in source and binary forms, with or without
8 // modification, are permitted provided that the following conditions are
9 // met:
10 //
11 // 1. Redistributions of source code must retain the above copyright
12 // notice, this list of conditions and the following disclaimer.
13 //
14 // 2. Redistributions in binary form must reproduce the above copyright
15 // notice, this list of conditions and the following disclaimer in the
16 // documentation and/or other materials provided with the distribution.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
21 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
22 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
23 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
24 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
29 // DAMAGE.
30
31 #ifndef HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH
32 #define HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH
33
34 #include <hpp/constraints/config.hh>
35 #include <hpp/constraints/differentiable-function.hh>
36 #include <hpp/constraints/fwd.hh>
37 #include <hpp/constraints/matrix-view.hh>
38 #include <hpp/pinocchio/joint.hh>
39 #include <hpp/util/serialization-fwd.hh>
40 #include <pinocchio/spatial/se3.hpp>
41
42 namespace hpp {
43 namespace constraints {
44 /// \cond DEVEL
45 template <bool rel>
46 struct GenericTransformationModel {
47 JointConstPtr_t joint2;
48 bool R1isID, R2isID, t1isZero, t2isZero;
49 Transform3s F1inJ1, F2inJ2;
50 bool fullPos, fullOri;
51 size_type rowOri;
52 const size_type cols;
53
54 112 inline JointConstPtr_t getJoint1() const { return JointConstPtr_t(); }
55 38 inline void setJoint1(const JointConstPtr_t&) {}
56 112 GenericTransformationModel(const size_type nCols)
57 112 : joint2(),
58 112 R1isID(true),
59 112 R2isID(true),
60 112 t1isZero(true),
61 112 t2isZero(true),
62 112 fullPos(false),
63 112 fullOri(false),
64
2/4
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
112 cols(nCols) {
65
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
112 F1inJ1.setIdentity();
66
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
112 F2inJ2.setIdentity();
67 112 }
68 38650 void checkIsIdentity1() {
69
1/2
✓ Branch 3 taken 38650 times.
✗ Branch 4 not taken.
38650 R1isID = F1inJ1.rotation().isIdentity();
70
1/2
✓ Branch 3 taken 38650 times.
✗ Branch 4 not taken.
38650 t1isZero = F1inJ1.translation().isZero();
71 38650 }
72 38645 void checkIsIdentity2() {
73
1/2
✓ Branch 3 taken 38649 times.
✗ Branch 4 not taken.
38645 R2isID = F2inJ2.rotation().isIdentity();
74
1/2
✓ Branch 3 taken 38651 times.
✗ Branch 4 not taken.
38649 t2isZero = F2inJ2.translation().isZero();
75 38651 }
76 };
77 template <>
78 struct GenericTransformationModel<true> : GenericTransformationModel<false> {
79 JointConstPtr_t joint1;
80 163222 inline JointConstPtr_t getJoint1() const { return joint1; }
81 void setJoint1(const JointConstPtr_t& j);
82 69 GenericTransformationModel(const size_type nCols)
83 69 : GenericTransformationModel<false>(nCols), joint1() {}
84 };
85 /// \endcond DEVEL
86
87 /// \addtogroup constraints
88 /// \{
89
90 /** GenericTransformation class encapsulates 10 possible differentiable
91 * functions: Position, Orientation, OrientationSO3, Transformation,
92 * TransformationR3xSO3 and their relative
93 * versions RelativeTransformationR3xSO3, RelativePosition,
94 * RelativeOrientation, RelativeOrientationSO3, RelativeTransformation.
95 *
96 * These functions compute the position of frame
97 * GenericTransformation::frame2InJoint2 in joint
98 * GenericTransformation::joint2 frame, in the frame
99 * GenericTransformation::frame1InJoint1 in GenericTransformation::joint1
100 * frame. For absolute functions, GenericTransformation::joint1 is
101 * NULL and joint1 frame is the world frame.
102 *
103 * The value of the RelativeTransformation function is
104 * a 6-dimensional
105 * vector. The 3 first coordinates are the position of the center of the
106 * second frame expressed in the first frame.
107 * The 3 last coordinates are the log of the orientation of frame 2 in
108 * frame 1.
109 * The values of RelativePosition and RelativeOrientation are respectively
110 * the 3 first and the 3 last components of the above 6 dimensional vector.
111 *
112 * \f{equation*}
113 * f (\mathbf{q}) = \left(\begin{array}{c}
114 * \mathbf{translation}\left(T_{1/J_1}^T T_1^T T_2 T_{2/J_2}\right)\\
115 * \log \left((R_1 R_{1/J_1})^T R_2 R_{2/J_2}\right) \end{array}\right)
116 * \f}
117 *
118 * The Jacobian is given by
119 *
120 * \f{equation*}
121 * \left(\begin{array}{c}
122 * (R_1 R_{1/J_1})^T \left(\left[R_2 t_{2/J_2} + t_2 - t_1\right]_{\times}
123 * R_1 J_{1\,\omega} - \left[R_2 t_{2/J_2}\right]_{\times} R_2 J_{2\,\omega} +
124 * R_2 J_{2\,\mathbf{v}} - R_1 J_{1\,\mathbf{v}}\right) \\
125 * J_{log}\left((R_1 R_{1/J_1})^T R_2 R_{2/J_2}\right)(R_1 R_{1/J_1})^T
126 * (R_2 J_{2\,\omega} - R_1 J_{1\,\omega})
127 * \end{array}\right)
128 * \f}
129 *
130 * For RelativeTransformationR3xSO3, OrientationSO3, the 3 last components
131 * are replaced by a quaternion.
132 */
133 template <int _Options>
134 class HPP_CONSTRAINTS_DLLAPI GenericTransformation
135 : public DifferentiableFunction {
136 public:
137 typedef shared_ptr<GenericTransformation> Ptr_t;
138 typedef weak_ptr<GenericTransformation> WkPtr_t;
139 #if __cplusplus >= 201103L
140 static constexpr bool IsRelative = _Options & RelativeBit,
141 ComputeOrientation = _Options & OrientationBit,
142 ComputePosition = _Options & PositionBit,
143 OutputR3xSO3 = _Options & OutputR3xSO3Bit,
144 IsPosition = ComputePosition && !ComputeOrientation,
145 IsOrientation = !ComputePosition && ComputeOrientation,
146 IsTransform = ComputePosition && ComputeOrientation;
147 static constexpr int ValueSize =
148 (ComputePosition ? 3 : 0) +
149 (ComputeOrientation ? (OutputR3xSO3 ? 4 : 3) : 0),
150 DerSize = (ComputePosition ? 3 : 0) +
151 (ComputeOrientation ? 3 : 0);
152 #else
153 enum {
154 IsRelative = _Options & RelativeBit,
155 ComputeOrientation = _Options & OrientationBit,
156 ComputePosition = _Options & PositionBit,
157 OutputR3xSO3 = _Options & OutputR3xSO3Bit,
158 IsPosition = ComputePosition && !ComputeOrientation,
159 IsOrientation = !ComputePosition && ComputeOrientation,
160 IsTransform = ComputePosition && ComputeOrientation,
161 ValueSize = (ComputePosition ? 3 : 0) +
162 (ComputeOrientation ? (OutputR3xSO3 ? 4 : 3) : 0),
163 DerSize = (ComputePosition ? 3 : 0) + (ComputeOrientation ? 3 : 0)
164 };
165 #endif
166
167 /// \cond
168 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
169 /// \endcond
170
171 /// Object builder for absolute functions.
172 ///
173 /// \param name the name of the constraints,
174 /// \param robot the robot the constraints is applied to,
175 /// \param joint2 the second joint the transformation of which is
176 /// constrained,
177 /// \param reference desired relative transformation
178 /// \f$T_2(\mathbf{q})\f$ between the joints.
179 /// \param mask which component of the error vector to take into
180 /// account.
181 static Ptr_t create(const std::string& name, const DevicePtr_t& robot,
182 const JointConstPtr_t& joint2,
183 const Transform3s& reference,
184 std::vector<bool> mask = std::vector<bool>(DerSize,
185 true));
186
187 /// Object builder for absolute functions.
188 ///
189 /// \param name the name of the constraints,
190 /// \param robot the robot the constraints is applied to,
191 /// \param joint2 the second joint the transformation of which is
192 /// constrained,
193 /// \param frame2 position of a fixed frame in joint 2,
194 /// \param frame1 position of a fixed frame in the world,
195 /// \param mask vector of 6 boolean defining which coordinates of the
196 /// error vector to take into account.
197 ///
198 /// \note For Position, the rotation part of frame1 defines the
199 /// frame in which the error is expressed and the rotation of frame2
200 /// has no effect.
201 static Ptr_t create(const std::string& name, const DevicePtr_t& robot,
202 /* World frame */ const JointConstPtr_t& joint2,
203 const Transform3s& frame2, const Transform3s& frame1,
204 std::vector<bool> mask = std::vector<bool>(DerSize,
205 true));
206
207 /// Object builder for relative functions.
208 ///
209 /// \param name the name of the constraints,
210 /// \param robot the robot the constraints is applied to,
211 /// \param joint1 the first joint the transformation of which is
212 /// constrained,
213 /// \param joint2 the second joint the transformation of which is
214 /// constrained,
215 /// \param reference desired relative transformation
216 /// \f$T_1(\mathbf{q})^{-1} T_2(\mathbf{q})\f$ between the joints.
217 /// \param mask which component of the error vector to take into
218 /// account.
219 static Ptr_t create(const std::string& name, const DevicePtr_t& robot,
220 const JointConstPtr_t& joint1,
221 const JointConstPtr_t& joint2,
222 const Transform3s& reference,
223 std::vector<bool> mask = std::vector<bool>(DerSize,
224 true));
225
226 /// Object builder for relative functions.
227 ///
228 /// \param name the name of the constraints,
229 /// \param robot the robot the constraints is applied to,
230 /// \param joint1 the first joint the transformation of which is
231 /// constrained,
232 /// \param joint2 the second joint the transformation of which is
233 /// constrained,
234 /// \param frame1 position of a fixed frame in joint 1,
235 /// \param frame2 position of a fixed frame in joint 2,
236 /// \param mask vector of 6 boolean defining which coordinates of the
237 /// error vector to take into account.
238 /// \note if joint1 is 0x0, joint 1 frame is considered to be the global
239 /// frame.
240 ///
241 /// \note For RelativePosition, the rotation part of frame1 defines the
242 /// frame in which the error is expressed and the rotation of frame2
243 /// has no effect.
244 static Ptr_t create(const std::string& name, const DevicePtr_t& robot,
245 const JointConstPtr_t& joint1,
246 const JointConstPtr_t& joint2, const Transform3s& frame1,
247 const Transform3s& frame2,
248 std::vector<bool> mask = std::vector<bool>(DerSize,
249 true));
250
251 420 virtual ~GenericTransformation() {}
252
253 /// Set desired relative transformation of joint2 in joint1
254 ///
255 84 inline void reference(const Transform3s& reference) {
256 84 m_.F1inJ1 = reference;
257 84 m_.checkIsIdentity1();
258 84 m_.F2inJ2.setIdentity();
259 84 m_.checkIsIdentity2();
260 }
261
262 /// Get desired relative orientation
263 inline Transform3s reference() const { return m_.F1inJ1.actInv(m_.F2inJ2); }
264
265 /// Set joint 1
266 188 inline void joint1(const JointConstPtr_t& joint) {
267 // static_assert(IsRelative);
268 188 m_.setJoint1(joint);
269 188 computeActiveParams();
270
5/6
✓ Branch 1 taken 52 times.
✓ Branch 2 taken 42 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 52 times.
✓ Branch 9 taken 52 times.
✓ Branch 10 taken 42 times.
188 assert(!joint || joint->robot() == robot_);
271 188 }
272
273 /// Get joint 1
274 556 inline JointConstPtr_t joint1() const { return m_.getJoint1(); }
275
276 /// Set joint 2
277 188 inline void joint2(const JointConstPtr_t& joint) {
278 188 m_.joint2 = joint;
279 188 computeActiveParams();
280
6/8
✓ Branch 1 taken 85 times.
✓ Branch 2 taken 9 times.
✓ Branch 5 taken 85 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 85 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 85 times.
✓ Branch 14 taken 9 times.
358 assert(!joint || (joint->index() > 0 && joint->robot() == robot_));
281 188 }
282
283 /// Get joint 2
284 596 inline JointConstPtr_t joint2() const { return m_.joint2; }
285
286 /// Set position of frame 1 in joint 1
287 104 inline void frame1InJoint1(const Transform3s& M) {
288 104 m_.F1inJ1 = M;
289 104 m_.checkIsIdentity1();
290 }
291 /// Get position of frame 1 in joint 1
292 56 inline const Transform3s& frame1InJoint1() const { return m_.F1inJ1; }
293 /// Set position of frame 2 in joint 2
294 104 inline void frame2InJoint2(const Transform3s& M) {
295 104 m_.F2inJ2 = M;
296 104 m_.checkIsIdentity2();
297 }
298 /// Get position of frame 2 in joint 2
299 56 inline const Transform3s& frame2InJoint2() const { return m_.F2inJ2; }
300
301 /// Return pair of joints the relative pose between which
302 /// modifies the function value if any
303 ///
304 /// This method is useful to know whether an instance of Implicit constrains
305 /// the relative pose between two joints.
306 /// \return the pair of joints involved, arranged in order of increasing
307 /// joint index, or a pair of empty shared pointers. or a pair of empty shared
308 /// pointers. \note if absolute pose (relative pose with respect to
309 /// "universe"), "universe" is returned as empty shared pointer
310 36 std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
311 DeviceConstPtr_t /*robot*/) const {
312
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
36 JointConstPtr_t j1 = joint1();
313
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
36 JointConstPtr_t j2 = joint2();
314 36 size_type index1 = Joint::index(j1);
315 36 size_type index2 = Joint::index(j2);
316
2/2
✓ Branch 0 taken 14 times.
✓ Branch 1 taken 4 times.
36 if (index1 <= index2) {
317 28 return std::pair<JointConstPtr_t, JointConstPtr_t>(j1, j2);
318 } else {
319 8 return std::pair<JointConstPtr_t, JointConstPtr_t>(j2, j1);
320 }
321 36 };
322
323 virtual std::ostream& print(std::ostream& o) const;
324
325 /// Constructor
326 ///
327 /// \param name the name of the constraints,
328 /// \param robot the robot the constraints is applied to,
329 /// \f$T_1(\mathbf{q})^{-1} T_2(\mathbf{q})\f$ between the joints.
330 /// \param mask vector of 6 boolean defining which coordinates of the
331 /// error vector to take into account.
332 GenericTransformation(const std::string& name, const DevicePtr_t& robot,
333 std::vector<bool> mask);
334
335 protected:
336 84 void init(const WkPtr_t& self) {
337 84 self_ = self;
338 84 computeActiveParams();
339 }
340
341 /// Compute value of error
342 ///
343 /// \param argument configuration of the robot,
344 /// \retval result error vector
345 virtual void impl_compute(LiegroupElementRef result,
346 ConfigurationIn_t argument) const;
347 virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const;
348
349 52490 bool isEqual(const DifferentiableFunction& other) const {
350 42466 const GenericTransformation& castother =
351
2/2
✓ Branch 0 taken 21233 times.
✓ Branch 1 taken 5012 times.
52490 dynamic_cast<const GenericTransformation&>(other);
352
2/2
✓ Branch 1 taken 11 times.
✓ Branch 2 taken 21222 times.
42466 if (!DifferentiableFunction::isEqual(other)) return false;
353
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 21222 times.
42444 if (robot_ != castother.robot_) return false;
354
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 21222 times.
42444 if (mask_ != castother.mask_) return false;
355
356 42444 return true;
357 }
358
359 private:
360 void computeActiveParams();
361 DevicePtr_t robot_;
362 GenericTransformationModel<IsRelative> m_;
363 Eigen::RowBlockIndices Vindices_;
364 const std::vector<bool> mask_;
365 WkPtr_t self_;
366
367
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
22 GenericTransformation() : m_(0) {}
368 HPP_SERIALIZABLE();
369 }; // class GenericTransformation
370 /// \}
371 } // namespace constraints
372 } // namespace hpp
373 #endif // HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH
374