hpp-constraints: include/hpp/constraints/generic-transformation.hh Source File

hpp-constraints 9.0.2 Definition of basic geometric constraints for motion planning

Loading...
Searching...
No Matches
generic-transformation.hh
Go to the documentation of this file.
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 {
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 inline JointConstPtr_t getJoint1() const { return JointConstPtr_t(); }
55 inline void setJoint1(const JointConstPtr_t&) {}
56 GenericTransformationModel(const size_type nCols)
57 : joint2(),
58 R1isID(true),
59 R2isID(true),
60 t1isZero(true),
61 t2isZero(true),
62 fullPos(false),
63 fullOri(false),
64 cols(nCols) {
65 F1inJ1.setIdentity();
66 F2inJ2.setIdentity();
67 }
68 void checkIsIdentity1() {
69 R1isID = F1inJ1.rotation().isIdentity();
70 t1isZero = F1inJ1.translation().isZero();
71 }
72 void checkIsIdentity2() {
73 R2isID = F2inJ2.rotation().isIdentity();
74 t2isZero = F2inJ2.translation().isZero();
75 }
76};
77 template <>
78 struct GenericTransformationModel<true> : GenericTransformationModel<false> {
79 JointConstPtr_t joint1;
80 inline JointConstPtr_t getJoint1() const { return joint1; }
81 void setJoint1(const JointConstPtr_t& j);
82 GenericTransformationModel(const size_type nCols)
83 : GenericTransformationModel<false>(nCols), joint1() {}
84};
86
89
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
168 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
170
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
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
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
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 virtual ~GenericTransformation() {}
252
255 inline void reference(const Transform3s& reference) {
256 m_.F1inJ1 = reference;
257 m_.checkIsIdentity1();
258 m_.F2inJ2.setIdentity();
259 m_.checkIsIdentity2();
260 }
261
263 inline Transform3s reference() const { return m_.F1inJ1.actInv(m_.F2inJ2); }
264
266 inline void joint1(const JointConstPtr_t& joint) {
267 // static_assert(IsRelative);
268 m_.setJoint1(joint);
269 computeActiveParams();
270 assert(!joint || joint->robot() == robot_);
271 }
272
274 inline JointConstPtr_t joint1() const { return m_.getJoint1(); }
275
277 inline void joint2(const JointConstPtr_t& joint) {
278 m_.joint2 = joint;
279 computeActiveParams();
280 assert(!joint || (joint->index() > 0 && joint->robot() == robot_));
281 }
282
284 inline JointConstPtr_t joint2() const { return m_.joint2; }
285
287 inline void frame1InJoint1(const Transform3s& M) {
288 m_.F1inJ1 = M;
289 m_.checkIsIdentity1();
290 }
292 inline const Transform3s& frame1InJoint1() const { return m_.F1inJ1; }
294 inline void frame2InJoint2(const Transform3s& M) {
295 m_.F2inJ2 = M;
296 m_.checkIsIdentity2();
297 }
299 inline const Transform3s& frame2InJoint2() const { return m_.F2inJ2; }
300
310 std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
311 DeviceConstPtr_t /*robot*/) const {
312 JointConstPtr_t j1 = joint1();
313 JointConstPtr_t j2 = joint2();
314 size_type index1 = Joint::index(j1);
315 size_type index2 = Joint::index(j2);
316 if (index1 <= index2) {
317 return std::pair<JointConstPtr_t, JointConstPtr_t>(j1, j2);
318 } else {
319 return std::pair<JointConstPtr_t, JointConstPtr_t>(j2, j1);
320 }
321 };
322
323 virtual std::ostream& print(std::ostream& o) const;
324
332 GenericTransformation(const std::string& name, const DevicePtr_t& robot,
333 std::vector<bool> mask);
334
335 protected:
336 void init(const WkPtr_t& self) {
337 self_ = self;
338 computeActiveParams();
339 }
340
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 bool isEqual(const DifferentiableFunction& other) const {
350 const GenericTransformation& castother =
351 dynamic_cast<const GenericTransformation&>(other);
352 if (!DifferentiableFunction::isEqual(other)) return false;
353 if (robot_ != castother.robot_) return false;
354 if (mask_ != castother.mask_) return false;
355
356 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 GenericTransformation() : m_(0) {}
368 HPP_SERIALIZABLE();
369}; // class GenericTransformation
371} // namespace constraints
372} // namespace hpp
373#endif // HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH
Eigen::MatrixBlocks< false, true >
hpp::constraints::DifferentiableFunction
Definition differentiable-function.hh:63
hpp::constraints::GenericTransformation
Definition generic-transformation.hh:135
hpp::constraints::GenericTransformation::print
virtual std::ostream & print(std::ostream &o) const
Display object in a stream.
hpp::constraints::GenericTransformation::GenericTransformation
GenericTransformation(const std::string &name, const DevicePtr_t &robot, std::vector< bool > mask)
hpp::constraints::GenericTransformation::dependsOnRelPoseBetween
std::pair< JointConstPtr_t, JointConstPtr_t > dependsOnRelPoseBetween(DeviceConstPtr_t) const
Definition generic-transformation.hh:310
hpp::constraints::GenericTransformation::create
static Ptr_t create(const std::string &name, const DevicePtr_t &robot, const JointConstPtr_t &joint2, const Transform3s &reference, std::vector< bool > mask=std::vector< bool >(DerSize, true))
hpp::constraints::GenericTransformation::impl_compute
virtual void impl_compute(LiegroupElementRef result, ConfigurationIn_t argument) const
hpp::constraints::GenericTransformation::joint2
JointConstPtr_t joint2() const
Get joint 2.
Definition generic-transformation.hh:284
hpp::constraints::GenericTransformation::frame2InJoint2
void frame2InJoint2(const Transform3s &M)
Set position of frame 2 in joint 2.
Definition generic-transformation.hh:294
hpp::constraints::GenericTransformation::create
static Ptr_t create(const std::string &name, const DevicePtr_t &robot, const JointConstPtr_t &joint1, const JointConstPtr_t &joint2, const Transform3s &reference, std::vector< bool > mask=std::vector< bool >(DerSize, true))
hpp::constraints::GenericTransformation::frame2InJoint2
const Transform3s & frame2InJoint2() const
Get position of frame 2 in joint 2.
Definition generic-transformation.hh:299
hpp::constraints::GenericTransformation::init
void init(const WkPtr_t &self)
Definition generic-transformation.hh:336
hpp::constraints::GenericTransformation::~GenericTransformation
virtual ~GenericTransformation()
Definition generic-transformation.hh:251
hpp::constraints::GenericTransformation::reference
Transform3s reference() const
Get desired relative orientation.
Definition generic-transformation.hh:263
hpp::constraints::GenericTransformation::WkPtr_t
weak_ptr< GenericTransformation > WkPtr_t
Definition generic-transformation.hh:138
hpp::constraints::GenericTransformation::frame1InJoint1
const Transform3s & frame1InJoint1() const
Get position of frame 1 in joint 1.
Definition generic-transformation.hh:292
hpp::constraints::GenericTransformation::impl_jacobian
virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const
hpp::constraints::GenericTransformation::reference
void reference(const Transform3s &reference)
Definition generic-transformation.hh:255
hpp::constraints::GenericTransformation::create
static Ptr_t create(const std::string &name, const DevicePtr_t &robot, const JointConstPtr_t &joint1, const JointConstPtr_t &joint2, const Transform3s &frame1, const Transform3s &frame2, std::vector< bool > mask=std::vector< bool >(DerSize, true))
hpp::constraints::GenericTransformation::joint1
void joint1(const JointConstPtr_t &joint)
Set joint 1.
Definition generic-transformation.hh:266
hpp::constraints::GenericTransformation::joint2
void joint2(const JointConstPtr_t &joint)
Set joint 2.
Definition generic-transformation.hh:277
hpp::constraints::GenericTransformation::joint1
JointConstPtr_t joint1() const
Get joint 1.
Definition generic-transformation.hh:274
hpp::constraints::GenericTransformation::create
static Ptr_t create(const std::string &name, const DevicePtr_t &robot, const JointConstPtr_t &joint2, const Transform3s &frame2, const Transform3s &frame1, std::vector< bool > mask=std::vector< bool >(DerSize, true))
hpp::constraints::GenericTransformation::isEqual
bool isEqual(const DifferentiableFunction &other) const
Definition generic-transformation.hh:349
hpp::constraints::GenericTransformation::Ptr_t
shared_ptr< GenericTransformation > Ptr_t
Definition generic-transformation.hh:137
hpp::constraints::GenericTransformation::frame1InJoint1
void frame1InJoint1(const Transform3s &M)
Set position of frame 1 in joint 1.
Definition generic-transformation.hh:287
hpp::pinocchio::LiegroupElementBase
config.hh
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
differentiable-function.hh
fwd.hh
joint.hh
matrix-view.hh
hpp::constraints::Transform3s
pinocchio::Transform3s Transform3s
Definition fwd.hh:65
hpp::constraints::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:111
hpp::constraints::DeviceConstPtr_t
pinocchio::DeviceConstPtr_t DeviceConstPtr_t
Definition fwd.hh:112
hpp::constraints::size_type
pinocchio::size_type size_type
Definition fwd.hh:48
hpp::constraints::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::constraints::matrixOut_t
Eigen::Ref< matrix_t > matrixOut_t
Definition fwd.hh:59
hpp::constraints::JointConstPtr_t
pinocchio::JointConstPtr_t JointConstPtr_t
Definition fwd.hh:51
hpp::pinocchio::size_type
matrix_t::Index size_type
hpp::pinocchio::Transform3s
::pinocchio::SE3 Transform3s
hpp::pinocchio::JointConstPtr_t
shared_ptr< const Joint > JointConstPtr_t
hpp
serialization-fwd.hh
HPP_SERIALIZABLE
#define HPP_SERIALIZABLE()