hpp-constraints  4.10.1
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 boost::shared_ptr <GenericTransformation> Ptr_t;
116  typedef boost::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  OutputSE3 = _Options & OutputSE3Bit,
123  IsPosition = ComputePosition && !ComputeOrientation,
124  IsOrientation = !ComputePosition && ComputeOrientation,
125  IsTransform = ComputePosition && ComputeOrientation;
126  static constexpr int
127  ValueSize = (ComputePosition?3:0) + (ComputeOrientation?(OutputSE3?4:3):0),
128  DerSize = (ComputePosition?3:0) + (ComputeOrientation ?3:0);
129 #else
130  enum {
131  IsRelative = _Options & RelativeBit,
132  ComputeOrientation = _Options & OrientationBit,
133  ComputePosition = _Options & PositionBit,
134  OutputSE3 = _Options & OutputSE3Bit,
135  IsPosition = ComputePosition && !ComputeOrientation,
136  IsOrientation = !ComputePosition && ComputeOrientation,
137  IsTransform = ComputePosition && ComputeOrientation,
138  ValueSize = (ComputePosition?3:0) + (ComputeOrientation?(OutputSE3?4:3):0),
139  DerSize = (ComputePosition?3:0) + (ComputeOrientation ?3:0)
140  };
141 #endif
142 
144  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
146 
157  static Ptr_t create (const std::string& name, const DevicePtr_t& robot,
158  const JointConstPtr_t& joint2, const Transform3f& reference,
159  std::vector <bool> mask = std::vector<bool>(DerSize,true));
160 
175  static Ptr_t create (const std::string& name, const DevicePtr_t& robot,
176  /* World frame */ const JointConstPtr_t& joint2,
177  const Transform3f& frame2, const Transform3f& frame1,
178  std::vector <bool> mask = std::vector<bool>(DerSize,true));
179 
192  static Ptr_t create (const std::string& name, const DevicePtr_t& robot,
193  const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
194  const Transform3f& reference,
195  std::vector <bool> mask = std::vector<bool>(DerSize,true));
196 
215  static Ptr_t create (const std::string& name, const DevicePtr_t& robot,
216  const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
217  const Transform3f& frame1, const Transform3f& frame2,
218  std::vector <bool> mask = std::vector<bool>(DerSize,true));
219 
221 
224  inline void reference (const Transform3f& reference)
225  {
226  m_.F1inJ1 = reference;
227  m_.checkIsIdentity1();
228  m_.F2inJ2.setIdentity ();
229  m_.checkIsIdentity2();
230  }
231 
233  inline Transform3f reference () const
234  {
235  return m_.F1inJ1.actInv(m_.F2inJ2);
236  }
237 
239  inline void joint1 (const JointConstPtr_t& joint) {
240  // static_assert(IsRelative);
241  m_.setJoint1(joint);
242  computeActiveParams();
243  assert (!joint || joint->robot () == robot_);
244  }
245 
247  inline JointConstPtr_t joint1 () const {
248  return m_.getJoint1();
249  }
250 
252  inline void joint2 (const JointConstPtr_t& joint) {
253  m_.joint2 = joint;
254  computeActiveParams();
255  assert (!joint || (joint->index() > 0 && joint->robot () == robot_));
256  }
257 
259  inline JointConstPtr_t joint2 () const {
260  return m_.joint2;
261  }
262 
264  inline void frame1InJoint1 (const Transform3f& M) {
265  m_.F1inJ1 = M;
266  m_.checkIsIdentity1();
267  }
269  inline const Transform3f& frame1InJoint1 () const {
270  return m_.F1inJ1;
271  }
273  inline void frame2InJoint2 (const Transform3f& M) {
274  m_.F2inJ2 = M;
275  m_.checkIsIdentity2();
276  }
278  inline const Transform3f& frame2InJoint2 () const {
279  return m_.F2inJ2;
280  }
281 
282  virtual std::ostream& print (std::ostream& o) const;
283 
291  GenericTransformation (const std::string& name,
292  const DevicePtr_t& robot,
293  std::vector <bool> mask);
294 
295  protected:
296  void init (const WkPtr_t& self)
297  {
298  self_ = self;
299  computeActiveParams();
300  }
301 
306  virtual void impl_compute (LiegroupElementRef result,
307  ConfigurationIn_t argument) const;
308  virtual void impl_jacobian (matrixOut_t jacobian,
309  ConfigurationIn_t arg) const;
310  private:
311  void computeActiveParams ();
312  DevicePtr_t robot_;
313  GenericTransformationModel<IsRelative> m_;
314  Eigen::RowBlockIndices Vindices_;
315  const std::vector <bool> mask_;
316  WkPtr_t self_;
317 
318  GenericTransformation() : m_ (0) {}
319  HPP_SERIALIZABLE();
320  }; // class GenericTransformation
322  } // namespace constraints
323 } // namespace hpp
324 #endif // HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH
void joint2(const JointConstPtr_t &joint)
Set joint 2.
Definition: generic-transformation.hh:252
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:296
void frame2InJoint2(const Transform3f &M)
Set position of frame 2 in joint 2.
Definition: generic-transformation.hh:273
const Transform3f & frame1InJoint1() const
Get position of frame 1 in joint 1.
Definition: generic-transformation.hh:269
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:224
pinocchio::JointConstPtr_t JointConstPtr_t
Definition: fwd.hh:39
void joint1(const JointConstPtr_t &joint)
Set joint 1.
Definition: generic-transformation.hh:239
const Transform3f & frame2InJoint2() const
Get position of frame 2 in joint 2.
Definition: generic-transformation.hh:278
JointConstPtr_t joint1() const
Get joint 1.
Definition: generic-transformation.hh:247
boost::shared_ptr< GenericTransformation > Ptr_t
Definition: generic-transformation.hh:115
boost::weak_ptr< GenericTransformation > WkPtr_t
Definition: generic-transformation.hh:116
JointConstPtr_t joint2() const
Get joint 2.
Definition: generic-transformation.hh:259
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:54
virtual ~GenericTransformation()
Definition: generic-transformation.hh:220
pinocchio::size_type size_type
Definition: fwd.hh:36
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
Transform3f reference() const
Get desired relative orientation.
Definition: generic-transformation.hh:233
void frame1InJoint1(const Transform3f &M)
Set position of frame 1 in joint 1.
Definition: generic-transformation.hh:264
pinocchio::Transform3f Transform3f
Definition: fwd.hh:52