hpp-constraints  4.9.1
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 // This file is part of hpp-constraints.
5 // hpp-constraints is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-constraints is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-constraints. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_CONSTRAINTS_EXPLICIT_RELATIVE_TRANSFORMATION_HH
18 # define HPP_CONSTRAINTS_EXPLICIT_RELATIVE_TRANSFORMATION_HH
19 
23 
24 namespace hpp {
25  namespace constraints {
28 
29  namespace explicit_ {
32 
45  class HPP_CONSTRAINTS_DLLAPI RelativeTransformation :
47  {
48  public:
59  (const std::string& name , const DevicePtr_t& robot,
60  const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
61  const Transform3f& frame1 , const Transform3f& frame2);
62 
64  const JointConstPtr_t& joint1 () const
65  {
66  return joint1_;
67  }
68 
70  const JointConstPtr_t& joint2 () const
71  {
72  return joint2_;
73  }
74 
75  protected:
79 
81  const std::string& name , const DevicePtr_t& robot,
82  const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
83  const Transform3f& frame1 , const Transform3f& frame2,
84  const segments_t inConf , const segments_t outConf,
85  const segments_t inVel , const segments_t outVel ,
86  std::vector <bool> mask = std::vector<bool>(6,true));
87 
89  (const RelativeTransformation& other) :
90  DifferentiableFunction (other),
91  robot_ (other.robot_),
92  parentJoint_ (other.parentJoint_),
93  inConf_ (other.inConf_), inVel_ (other.inVel_),
94  outConf_ (other.outConf_), outVel_ (other.outVel_),
95  F1inJ1_invF2inJ2_ (other.F1inJ1_invF2inJ2_)
96  {
97  }
98 
99  // Store weak pointer to itself
100  void init (const RelativeTransformationWkPtr_t& weak)
101  {
102  weak_ = weak;
103  }
104 
111  void impl_compute (LiegroupElementRef result, vectorIn_t argument) const;
112 
113  void impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const;
114 
115  private:
116  void forwardKinematics (vectorIn_t arg) const;
117 
118  DevicePtr_t robot_;
119  // Parent of the R3 joint.
120  JointConstPtr_t parentJoint_;
121  JointConstPtr_t joint1_, joint2_;
122  Transform3f frame1_, frame2_;
123  RowBlockIndices inConf_;
124  ColBlockIndices inVel_;
125  RowBlockIndices outConf_ , outVel_;
126  Transform3f F1inJ1_invF2inJ2_;
127 
128  RelativeTransformationWkPtr_t weak_;
129 
130  // Tmp variables
131  mutable vector_t qsmall_, q_;
132  mutable matrix_t tmpJac_, J2_parent_minus_J1_;
133  }; // class RelativeTransformation
135 
136  } // namespace explicit_
137  } // namespace constraints
138 } // namespace hpp
139 
140 #endif // HPP_CONSTRAINTS_EXPLICIT_RELATIVE_TRANSFORMATION_HH
Eigen::BlockIndex BlockIndex
Definition: relative-transformation.hh:76
pinocchio::vector_t vector_t
Definition: fwd.hh:45
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:46
Definition: matrix-view.hh:37
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:91
const JointConstPtr_t & joint1() const
Get joint 1.
Definition: relative-transformation.hh:64
GenericTransformation< RelativeBit|PositionBit|OrientationBit > RelativeTransformation
Definition: fwd.hh:138
ObjectFactory * create(ObjectFactory *parent=NULL, const XMLElement *element=NULL)
std::vector< segment_t > segments_t
Definition: fwd.hh:69
Eigen::RowBlockIndices RowBlockIndices
Definition: relative-transformation.hh:77
pinocchio::matrix_t matrix_t
Definition: fwd.hh:42
Definition: relative-transformation.hh:45
pinocchio::JointConstPtr_t JointConstPtr_t
Definition: fwd.hh:38
Definition: differentiable-function.hh:50
const JointConstPtr_t & joint2() const
Get joint 2.
Definition: relative-transformation.hh:70
boost::shared_ptr< RelativeTransformation > RelativeTransformationPtr_t
Definition: fwd.hh:194
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:44
void init(const RelativeTransformationWkPtr_t &weak)
Definition: relative-transformation.hh:100
Eigen::ColBlockIndices ColBlockIndices
Definition: relative-transformation.hh:78
boost::shared_ptr< RelativeTransformation > RelativeTransformationPtr_t
Definition: fwd.hh:152
pinocchio::Transform3f Transform3f
Definition: fwd.hh:50