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 |