hpp-constraints  4.13.0
Definition of basic geometric constraints for motion planning
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 
36 #include <hpp/constraints/fwd.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  Transform3f 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 Transform3f& 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 Transform3f& frame2, const Transform3f& 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 Transform3f& 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 Transform3f& frame1,
247  const Transform3f& frame2,
248  std::vector<bool> mask = std::vector<bool>(DerSize,
249  true));
250 
252 
255  inline void reference(const Transform3f& reference) {
256  m_.F1inJ1 = reference;
257  m_.checkIsIdentity1();
258  m_.F2inJ2.setIdentity();
259  m_.checkIsIdentity2();
260  }
261 
263  inline Transform3f 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 Transform3f& M) {
288  m_.F1inJ1 = M;
289  m_.checkIsIdentity1();
290  }
292  inline const Transform3f& frame1InJoint1() const { return m_.F1inJ1; }
294  inline void frame2InJoint2(const Transform3f& M) {
295  m_.F2inJ2 = M;
296  m_.checkIsIdentity2();
297  }
299  inline const Transform3f& frame2InJoint2() const { return m_.F2inJ2; }
300 
301  virtual std::ostream& print(std::ostream& o) const;
302 
310  GenericTransformation(const std::string& name, const DevicePtr_t& robot,
311  std::vector<bool> mask);
312 
313  protected:
314  void init(const WkPtr_t& self) {
315  self_ = self;
316  computeActiveParams();
317  }
318 
323  virtual void impl_compute(LiegroupElementRef result,
324  ConfigurationIn_t argument) const;
325  virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const;
326 
327  bool isEqual(const DifferentiableFunction& other) const {
328  const GenericTransformation& castother =
329  dynamic_cast<const GenericTransformation&>(other);
330  if (!DifferentiableFunction::isEqual(other)) return false;
331  if (robot_ != castother.robot_) return false;
332  if (mask_ != castother.mask_) return false;
333 
334  return true;
335  }
336 
337  private:
338  void computeActiveParams();
339  DevicePtr_t robot_;
340  GenericTransformationModel<IsRelative> m_;
341  Eigen::RowBlockIndices Vindices_;
342  const std::vector<bool> mask_;
343  WkPtr_t self_;
344 
345  GenericTransformation() : m_(0) {}
346  HPP_SERIALIZABLE();
347 }; // class GenericTransformation
349 } // namespace constraints
350 } // namespace hpp
351 #endif // HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH
void joint2(const JointConstPtr_t &joint)
Set joint 2.
Definition: generic-transformation.hh:277
Definition: active-set-differentiable-function.hh:36
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:108
void init(const WkPtr_t &self)
Definition: generic-transformation.hh:314
weak_ptr< GenericTransformation > WkPtr_t
Definition: generic-transformation.hh:138
void frame2InJoint2(const Transform3f &M)
Set position of frame 2 in joint 2.
Definition: generic-transformation.hh:294
bool isEqual(const DifferentiableFunction &other) const
Definition: generic-transformation.hh:327
virtual bool isEqual(const DifferentiableFunction &other) const
Definition: differentiable-function.hh:190
const Transform3f & frame1InJoint1() const
Get position of frame 1 in joint 1.
Definition: generic-transformation.hh:292
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:105
assert(d.lhs()._blocks()==d.rhs()._blocks())
void reference(const Transform3f &reference)
Definition: generic-transformation.hh:255
pinocchio::JointConstPtr_t JointConstPtr_t
Definition: fwd.hh:50
void joint1(const JointConstPtr_t &joint)
Set joint 1.
Definition: generic-transformation.hh:266
Definition: differentiable-function.hh:63
const Transform3f & frame2InJoint2() const
Get position of frame 2 in joint 2.
Definition: generic-transformation.hh:299
JointConstPtr_t joint1() const
Get joint 1.
Definition: generic-transformation.hh:274
JointConstPtr_t joint2() const
Get joint 2.
Definition: generic-transformation.hh:284
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:65
virtual ~GenericTransformation()
Definition: generic-transformation.hh:251
pinocchio::size_type size_type
Definition: fwd.hh:47
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:57
shared_ptr< GenericTransformation > Ptr_t
Definition: generic-transformation.hh:137
Transform3f reference() const
Get desired relative orientation.
Definition: generic-transformation.hh:263
void frame1InJoint1(const Transform3f &M)
Set position of frame 1 in joint 1.
Definition: generic-transformation.hh:287
pinocchio::Transform3f Transform3f
Definition: fwd.hh:63