Kindr
Kinematics and Dynamics for Robotics
HomogeneousTransformation.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
20  * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
21  * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
22  * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27 */
28 
29 #pragma once
30 
31 #include "kindr/common/common.hpp"
35 #include "kindr/poses/PoseBase.hpp"
36 
37 
38 namespace kindr {
39 
40 
41 template<typename PrimType_, typename Position_, typename Rotation_>
42 class HomogeneousTransformation : public PoseBase<HomogeneousTransformation<PrimType_, Position_, Rotation_> > {
43  protected:
44  Position_ position_;
45  Rotation_ rotation_;
46  public:
47 
48  typedef PrimType_ Scalar;
49  typedef Position_ Position;
50  typedef Rotation_ Rotation;
51  typedef Eigen::Matrix<PrimType_, 4, 4> TransformationMatrix;
52 
53 
54  explicit HomogeneousTransformation(): position_(), rotation_() {
55 
56  }
57 
58  inline explicit HomogeneousTransformation(const Position& position, const Rotation& rotation) :
59  position_(position),rotation_(rotation) {
60  }
61 
65  template<typename OtherDerived_>
66  inline explicit HomogeneousTransformation(const PoseBase<OtherDerived_>& other)
67  : position_(Position(other.derived().getPosition())), rotation_(Rotation(other.derived().getRotation()))
68  {
69  // todo: use conversion trait
70  }
71 
77  position_ = Position(other.getPosition());
78  rotation_ = Rotation(other.getRotation());
79  return *this;
80  }
81 
82  inline Position_ & getPosition() {
83  return position_;
84  }
85 
86  inline const Position_ & getPosition() const {
87  return position_;
88  }
89 
90  inline Rotation_ & getRotation() {
91  return rotation_;
92  }
93 
94  inline const Rotation_ & getRotation() const {
95  return rotation_;
96  }
97 
103 
104  inline TransformationMatrix getTransformationMatrix() const {
105  TransformationMatrix mat = TransformationMatrix::Zero();
106  mat.template topLeftCorner<3,3>() = RotationMatrix<Scalar>(getRotation()).toImplementation();
107  mat.template topRightCorner<3,1>() = getPosition().toImplementation();
108  mat(3,3) = Scalar(1);
109  return mat;
110  }
111 
115  friend std::ostream & operator << (std::ostream & out, const HomogeneousTransformation & pose) {
116  out << pose.getTransformationMatrix();
117  return out;
118  }
119 
124  position_.setZero();
125  rotation_.setIdentity();
126  return *this;
127  }
128 };
129 
130 template <typename PrimType_>
134 
135 // For backwards comp.
136 template <typename PrimType_>
140 
141 template <typename PrimType_>
145 
146 
147 namespace internal {
148 
149 template<typename PrimType_, typename Position_, typename Rotation_>
150 class get_position<HomogeneousTransformation<PrimType_, Position_, Rotation_>> {
151  public:
153  typedef Position_ Position;
154 };
155 
156 template<typename PrimType_, typename Position_, typename Rotation_>
157 class TransformationTraits<HomogeneousTransformation<PrimType_, Position_, Rotation_>> {
158  private:
160  typedef typename get_position<Pose>::Position Translation;
161  public:
162  inline static Translation transform(const Pose & pose, const Translation & position){
163  return pose.getRotation().rotate(position) + pose.getPosition();
164  }
165  inline static Translation inverseTransform(const Pose & pose, const Translation & position){
166  return pose.getRotation().inverseRotate((position-pose.getPosition()));
167  }
168 };
169 
170 
173 template<typename PrimType_, typename Position_, typename Rotation_>
174 class MultiplicationTraits<PoseBase<HomogeneousTransformation<PrimType_, Position_, Rotation_>>, PoseBase<HomogeneousTransformation<PrimType_, Position_, Rotation_>> > {
175  public:
181  }
182 };
183 
184 
185 
186 } // namespace internal
187 } // namespace kindr
188 
HomTransformMatrix< double > HomTransformMatrixD
internal::get_position< HomogeneousTransformation< PrimType_, Position_, Rotation_ > >::Position inverseTransform(const typename internal::get_position< HomogeneousTransformation< PrimType_, Position_, Rotation_ > >::Position &position) const
Transforms a position in reverse.
Definition: PoseBase.hpp:130
HomogeneousTransformationPosition3RotationQuaternion< float > HomogeneousTransformationPosition3RotationQuaternionF
HomTransformQuat< float > HomTransformQuatF
Eigen::Matrix< PrimType_, 4, 4 > TransformationMatrix
internal::get_position< HomogeneousTransformation< PrimType_, Position_, Rotation_ > >::Position transform(const typename internal::get_position< HomogeneousTransformation< PrimType_, Position_, Rotation_ > >::Position &position) const
Transforms a position.
Definition: PoseBase.hpp:123
HomogeneousTransformation(const PoseBase< OtherDerived_ > &other)
Constructor using another rotation.
const HomogeneousTransformation< PrimType_, Position_, Rotation_ > & derived() const
Gets the derived pose.
Definition: PoseBase.hpp:115
HomTransformMatrix< float > HomTransformMatrixF
HomogeneousTransformation & setIdentity()
Sets the pose to identity.
friend std::ostream & operator<<(std::ostream &out, const HomogeneousTransformation &pose)
Used for printing the object with std::cout.
HomTransformQuat< double > HomTransformQuatD
TransformationMatrix getTransformationMatrix() const
Implementation of matrix rotation based on Eigen::Matrix<Scalar, 3, 3>
Definition: Rotation.hpp:49
HomogeneousTransformationPosition3RotationQuaternion< double > HomogeneousTransformationPosition3RotationQuaternionD
Base class that defines the interface of a pose of a rigid body.
Definition: PoseBase.hpp:79
HomogeneousTransformation(const Position &position, const Rotation &rotation)
HomogeneousTransformation & operator=(const HomogeneousTransformation &other)
Assignment operator.
Implementation of quaternion rotation based on Eigen::Quaternion.
Definition: Rotation.hpp:46