hpp-constraints  4.13.0
Definition of basic geometric constraints for motion planning
relative-transformation.hh
Go to the documentation of this file.
1 // Copyright (c) 2015 - 2018, CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr), Florent Lamiraux
3 //
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28 
29 #ifndef HPP_CONSTRAINTS_EXPLICIT_RELATIVE_TRANSFORMATION_HH
30 #define HPP_CONSTRAINTS_EXPLICIT_RELATIVE_TRANSFORMATION_HH
31 
35 
36 namespace hpp {
37 namespace constraints {
40 
41 namespace explicit_ {
44 
58  : public DifferentiableFunction {
59  public:
69  static RelativeTransformationPtr_t create(const std::string& name,
70  const DevicePtr_t& robot,
71  const JointConstPtr_t& joint1,
72  const JointConstPtr_t& joint2,
73  const Transform3f& frame1,
74  const Transform3f& frame2);
75 
77  const JointConstPtr_t& joint1() const { return joint1_; }
78 
80  const JointConstPtr_t& joint2() const { return joint2_; }
81 
82  protected:
86 
87  RelativeTransformation(const std::string& name, const DevicePtr_t& robot,
88  const JointConstPtr_t& joint1,
89  const JointConstPtr_t& joint2,
90  const Transform3f& frame1, const Transform3f& frame2,
91  const segments_t inConf, const segments_t outConf,
92  const segments_t inVel, const segments_t outVel,
93  std::vector<bool> mask = std::vector<bool>(6, true));
94 
96  : DifferentiableFunction(other),
97  robot_(other.robot_),
98  parentJoint_(other.parentJoint_),
99  inConf_(other.inConf_),
100  inVel_(other.inVel_),
101  outConf_(other.outConf_),
102  outVel_(other.outVel_),
103  F1inJ1_invF2inJ2_(other.F1inJ1_invF2inJ2_) {}
104 
105  // Store weak pointer to itself
106  void init(const RelativeTransformationWkPtr_t& weak) { weak_ = weak; }
107 
114  void impl_compute(LiegroupElementRef result, vectorIn_t argument) const;
115 
116  void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const;
117 
118  bool isEqual(const DifferentiableFunction& other) const {
119  const RelativeTransformation& castother =
120  dynamic_cast<const RelativeTransformation&>(other);
121  if (!DifferentiableFunction::isEqual(other)) return false;
122 
123  if (robot_ != castother.robot_) return false;
124  if (parentJoint_ != castother.parentJoint_) return false;
125  if (joint1_ != castother.joint1_) return false;
126  if (joint2_ != castother.joint2_) return false;
127  if (frame1_ != castother.frame1_) return false;
128  if (frame2_ != castother.frame2_) return false;
129  if (inConf_.rows() != castother.inConf_.rows()) return false;
130  if (inVel_.cols() != castother.inVel_.cols()) return false;
131  if (outConf_.rows() != castother.outConf_.rows()) return false;
132  if (outVel_.rows() != castother.outVel_.rows()) return false;
133  if (F1inJ1_invF2inJ2_ != castother.F1inJ1_invF2inJ2_) return false;
134 
135  return true;
136  }
137 
138  private:
139  void forwardKinematics(vectorIn_t arg) const;
140 
141  DevicePtr_t robot_;
142  // Parent of the R3 joint.
143  JointConstPtr_t parentJoint_;
144  JointConstPtr_t joint1_, joint2_;
145  Transform3f frame1_, frame2_;
146  RowBlockIndices inConf_;
147  ColBlockIndices inVel_;
148  RowBlockIndices outConf_, outVel_;
149  Transform3f F1inJ1_invF2inJ2_;
150 
151  RelativeTransformationWkPtr_t weak_;
152 
153  // Tmp variables
154  mutable vector_t qsmall_, q_;
155  mutable matrix_t tmpJac_, J2_parent_minus_J1_;
156 
158  HPP_SERIALIZABLE();
159 }; // class RelativeTransformation
161 
162 } // namespace explicit_
163 } // namespace constraints
164 } // namespace hpp
165 
167 
168 #endif // HPP_CONSTRAINTS_EXPLICIT_RELATIVE_TRANSFORMATION_HH
std::vector< segment_t > segments_t
Definition: fwd.hh:83
Eigen::BlockIndex BlockIndex
Definition: relative-transformation.hh:83
pinocchio::vector_t vector_t
Definition: fwd.hh:58
GenericTransformation< RelativeBit|PositionBit|OrientationBit > RelativeTransformation
Definition: fwd.hh:151
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:59
RelativeTransformation(const RelativeTransformation &other)
Definition: relative-transformation.hh:95
Definition: matrix-view.hh:49
Definition: active-set-differentiable-function.hh:36
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:108
const JointConstPtr_t & joint1() const
Get joint 1.
Definition: relative-transformation.hh:77
shared_ptr< RelativeTransformation > RelativeTransformationPtr_t
Definition: fwd.hh:168
Eigen::RowBlockIndices RowBlockIndices
Definition: relative-transformation.hh:84
pinocchio::matrix_t matrix_t
Definition: fwd.hh:55
Definition: relative-transformation.hh:57
virtual bool isEqual(const DifferentiableFunction &other) const
Definition: differentiable-function.hh:190
Eigen::MatrixBlocks< false, true > RowBlockIndices
Definition: matrix-view.hh:799
pinocchio::JointConstPtr_t JointConstPtr_t
Definition: fwd.hh:50
Definition: differentiable-function.hh:63
const JointConstPtr_t & joint2() const
Get joint 2.
Definition: relative-transformation.hh:80
const ColIndices_t & cols() const
Definition: matrix-view.hh:677
Eigen::MatrixBlocks< true, false > ColBlockIndices
Definition: matrix-view.hh:800
shared_ptr< RelativeTransformation > RelativeTransformationPtr_t
Definition: fwd.hh:197
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:65
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:57
void init(const RelativeTransformationWkPtr_t &weak)
Definition: relative-transformation.hh:106
Eigen::ColBlockIndices ColBlockIndices
Definition: relative-transformation.hh:85
const RowIndices_t & rows() const
Definition: matrix-view.hh:673
bool isEqual(const DifferentiableFunction &other) const
Definition: relative-transformation.hh:118
pinocchio::Transform3f Transform3f
Definition: fwd.hh:63