hpp-constraints  4.12.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 // This file is part of hpp-constraints.
7 // hpp-constraints is free software: you can redistribute it
8 // and/or modify it under the terms of the GNU Lesser General Public
9 // License as published by the Free Software Foundation, either version
10 // 3 of the License, or (at your option) any later version.
11 //
12 // hpp-constraints is distributed in the hope that it will be
13 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
14 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Lesser Public License for more details. You should have
16 // received a copy of the GNU Lesser General Public License along with
17 // hpp-constraints. If not, see
18 // <http://www.gnu.org/licenses/>.
19 
20 #ifndef HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH
21 # define HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH
22 
23 # include <pinocchio/spatial/se3.hpp>
24 
25 # include <hpp/util/serialization-fwd.hh>
26 
27 # include <hpp/pinocchio/joint.hh>
28 
29 # include <hpp/constraints/fwd.hh>
30 # include <hpp/constraints/config.hh>
33 
34 namespace hpp {
35  namespace constraints {
37  template <bool rel> struct GenericTransformationModel
38  {
39  JointConstPtr_t joint2;
40  bool R1isID, R2isID, t1isZero, t2isZero;
41  Transform3f F1inJ1, F2inJ2;
42  bool fullPos, fullOri;
43  size_type rowOri;
44  const size_type cols;
45 
46  inline JointConstPtr_t getJoint1() const { return JointConstPtr_t(); }
47  inline void setJoint1(const JointConstPtr_t&) {}
48  GenericTransformationModel (const size_type nCols) :
49  joint2(), R1isID(true), R2isID(true), t1isZero(true), t2isZero(true),
50  fullPos(false), fullOri(false), cols (nCols)
51  { F1inJ1.setIdentity(); F2inJ2.setIdentity(); }
52  void checkIsIdentity1() {
53  R1isID = F1inJ1.rotation().isIdentity();
54  t1isZero = F1inJ1.translation().isZero();
55  }
56  void checkIsIdentity2() {
57  R2isID = F2inJ2.rotation().isIdentity();
58  t2isZero = F2inJ2.translation().isZero();
59  }
60  };
61  template <> struct GenericTransformationModel<true> :
62  GenericTransformationModel<false>
63  {
64  JointConstPtr_t joint1;
65  inline JointConstPtr_t getJoint1() const { return joint1; }
66  void setJoint1(const JointConstPtr_t& j);
67  GenericTransformationModel (const size_type nCols) :
68  GenericTransformationModel<false>(nCols), joint1() {}
69  };
71 
74 
110  template <int _Options>
111  class HPP_CONSTRAINTS_DLLAPI GenericTransformation :
112  public DifferentiableFunction
113  {
114  public:
115  typedef shared_ptr <GenericTransformation> Ptr_t;
116  typedef weak_ptr <GenericTransformation> WkPtr_t;
117 #if __cplusplus >= 201103L
118  static constexpr bool
119  IsRelative = _Options & RelativeBit,
120  ComputeOrientation = _Options & OrientationBit,
121  ComputePosition = _Options & PositionBit,
122  OutputR3xSO3 = _Options & OutputR3xSO3Bit,
123  IsPosition = ComputePosition && !ComputeOrientation,
124  IsOrientation = !ComputePosition && ComputeOrientation,
125  IsTransform = ComputePosition && ComputeOrientation;
126  static constexpr int
127  ValueSize = (ComputePosition?3:0) +
128  (ComputeOrientation?(OutputR3xSO3?4:3):0),
129  DerSize = (ComputePosition?3:0) + (ComputeOrientation ?3:0);
130 #else
131  enum {
132  IsRelative = _Options & RelativeBit,
133  ComputeOrientation = _Options & OrientationBit,
134  ComputePosition = _Options & PositionBit,
135  OutputR3xSO3 = _Options & OutputR3xSO3Bit,
136  IsPosition = ComputePosition && !ComputeOrientation,
137  IsOrientation = !ComputePosition && ComputeOrientation,
138  IsTransform = ComputePosition && ComputeOrientation,
139  ValueSize = (ComputePosition?3:0) +
140  (ComputeOrientation?(OutputR3xSO3?4:3):0),
141  DerSize = (ComputePosition?3:0) + (ComputeOrientation ?3:0)
142  };
143 #endif
144 
146  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
148 
159  static Ptr_t create (const std::string& name, const DevicePtr_t& robot,
160  const JointConstPtr_t& joint2, const Transform3f& reference,
161  std::vector <bool> mask = std::vector<bool>(DerSize,true));
162 
177  static Ptr_t create (const std::string& name, const DevicePtr_t& robot,
178  /* World frame */ const JointConstPtr_t& joint2,
179  const Transform3f& frame2, const Transform3f& frame1,
180  std::vector <bool> mask = std::vector<bool>(DerSize,true));
181 
194  static Ptr_t create (const std::string& name, const DevicePtr_t& robot,
195  const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
196  const Transform3f& reference,
197  std::vector <bool> mask = std::vector<bool>(DerSize,true));
198 
217  static Ptr_t create (const std::string& name, const DevicePtr_t& robot,
218  const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
219  const Transform3f& frame1, const Transform3f& frame2,
220  std::vector <bool> mask = std::vector<bool>(DerSize,true));
221 
223 
226  inline void reference (const Transform3f& reference)
227  {
228  m_.F1inJ1 = reference;
229  m_.checkIsIdentity1();
230  m_.F2inJ2.setIdentity ();
231  m_.checkIsIdentity2();
232  }
233 
235  inline Transform3f reference () const
236  {
237  return m_.F1inJ1.actInv(m_.F2inJ2);
238  }
239 
241  inline void joint1 (const JointConstPtr_t& joint) {
242  // static_assert(IsRelative);
243  m_.setJoint1(joint);
244  computeActiveParams();
245  assert (!joint || joint->robot () == robot_);
246  }
247 
249  inline JointConstPtr_t joint1 () const {
250  return m_.getJoint1();
251  }
252 
254  inline void joint2 (const JointConstPtr_t& joint) {
255  m_.joint2 = joint;
256  computeActiveParams();
257  assert (!joint || (joint->index() > 0 && joint->robot () == robot_));
258  }
259 
261  inline JointConstPtr_t joint2 () const {
262  return m_.joint2;
263  }
264 
266  inline void frame1InJoint1 (const Transform3f& M) {
267  m_.F1inJ1 = M;
268  m_.checkIsIdentity1();
269  }
271  inline const Transform3f& frame1InJoint1 () const {
272  return m_.F1inJ1;
273  }
275  inline void frame2InJoint2 (const Transform3f& M) {
276  m_.F2inJ2 = M;
277  m_.checkIsIdentity2();
278  }
280  inline const Transform3f& frame2InJoint2 () const {
281  return m_.F2inJ2;
282  }
283 
284  virtual std::ostream& print (std::ostream& o) const;
285 
293  GenericTransformation (const std::string& name,
294  const DevicePtr_t& robot,
295  std::vector <bool> mask);
296 
297  protected:
298  void init (const WkPtr_t& self)
299  {
300  self_ = self;
301  computeActiveParams();
302  }
303 
308  virtual void impl_compute (LiegroupElementRef result,
309  ConfigurationIn_t argument) const;
310  virtual void impl_jacobian (matrixOut_t jacobian,
311  ConfigurationIn_t arg) const;
312  private:
313  void computeActiveParams ();
314  DevicePtr_t robot_;
315  GenericTransformationModel<IsRelative> m_;
316  Eigen::RowBlockIndices Vindices_;
317  const std::vector <bool> mask_;
318  WkPtr_t self_;
319 
320  GenericTransformation() : m_ (0) {}
321  HPP_SERIALIZABLE();
322  }; // class GenericTransformation
324  } // namespace constraints
325 } // namespace hpp
326 #endif // HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH
void joint2(const JointConstPtr_t &joint)
Set joint 2.
Definition: generic-transformation.hh:254
Definition: active-set-differentiable-function.hh:24
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:97
void init(const WkPtr_t &self)
Definition: generic-transformation.hh:298
weak_ptr< GenericTransformation > WkPtr_t
Definition: generic-transformation.hh:116
void frame2InJoint2(const Transform3f &M)
Set position of frame 2 in joint 2.
Definition: generic-transformation.hh:275
const Transform3f & frame1InJoint1() const
Get position of frame 1 in joint 1.
Definition: generic-transformation.hh:271
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:94
assert(d.lhs()._blocks()==d.rhs()._blocks())
void reference(const Transform3f &reference)
Definition: generic-transformation.hh:226
pinocchio::JointConstPtr_t JointConstPtr_t
Definition: fwd.hh:39
void joint1(const JointConstPtr_t &joint)
Set joint 1.
Definition: generic-transformation.hh:241
const Transform3f & frame2InJoint2() const
Get position of frame 2 in joint 2.
Definition: generic-transformation.hh:280
JointConstPtr_t joint1() const
Get joint 1.
Definition: generic-transformation.hh:249
JointConstPtr_t joint2() const
Get joint 2.
Definition: generic-transformation.hh:261
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:54
virtual ~GenericTransformation()
Definition: generic-transformation.hh:222
pinocchio::size_type size_type
Definition: fwd.hh:36
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
shared_ptr< GenericTransformation > Ptr_t
Definition: generic-transformation.hh:115
Transform3f reference() const
Get desired relative orientation.
Definition: generic-transformation.hh:235
void frame1InJoint1(const Transform3f &M)
Set position of frame 1 in joint 1.
Definition: generic-transformation.hh:266
pinocchio::Transform3f Transform3f
Definition: fwd.hh:52