Kindr
Kinematics and Dynamics for Robotics
Pose.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 
32 
33 namespace kindr {
34 
35 template<typename PrimType_, typename Position_, typename Rotation_>
36 class HomogeneousTransformation;
37 
38 
39 namespace internal {
40 
43 template<typename Left_, typename Right_>
44 class MultiplicationTraits<PoseBase<Left_>, PoseBase<Right_> > {
45  public:
46  typedef typename Left_::Position Position;
47  typedef typename Left_::Rotation Rotation;
48  typedef typename Left_::Scalar Scalar;
49  typedef HomogeneousTransformation<Scalar, Position, Rotation> HomTrans;
50  public:
52  inline static Left_ mult(const PoseBase<Left_>& lhs, const PoseBase<Right_>& rhs) {
53  const Position position = lhs.derived().getPosition()+lhs.derived().getRotation().rotate(rhs.derived().getPosition());
54  const Rotation rotation = lhs.derived().getRotation()*rhs.derived().getRotation();
55  return Left_(HomTrans(position, rotation));
56  }
57 };
58 
60 // */
61 //template<typename PrimType_, typename Position_, typename Rotation_>
62 //class MultiplicationTraits<HomogeneousTransformation<PrimType_, Position_, Rotation_>, HomogeneousTransformation<PrimType_, Position_, Rotation_> > {
63 // public:
64 // //! Default multiplication of rotations converts the representations of the rotations to rotation quaternions and multiplies them
65 // inline static HomogeneousTransformation<PrimType_, Position_, Rotation_> mult(const HomogeneousTransformation<PrimType_, Position_, Rotation_>& lhs, const HomogeneousTransformation<PrimType_, Position_, Rotation_>& rhs) {
66 // const typename HomogeneousTransformation<PrimType_, Position_, Rotation_>::Position position = lhs.getPosition()+lhs.getRotation().rotate(rhs.getPosition());
67 // const typename HomogeneousTransformation<PrimType_, Position_, Rotation_>::Rotation rotation = lhs.getRotation()*rhs.getRotation();
68 // return HomogeneousTransformation<PrimType_, Position_, Rotation_>(position, rotation);
69 // }
70 //};
71 
73 // */
74 //template<typename LeftAndRight_>
75 //class MultiplicationTraits<RotationBase<LeftAndRight_>, RotationBase<LeftAndRight_> > {
76 // public:
77 // //! Default multiplication of rotations converts the representations of the rotations to rotation quaternions and multiplies them
78 // inline static LeftAndRight_ mult(const RotationBase<LeftAndRight_>& lhs, const RotationBase<LeftAndRight_>& rhs) {
79 // return LeftAndRight_(RotationQuaternion<typename LeftAndRight_::Scalar>(
80 // (RotationQuaternion<typename LeftAndRight_::Scalar>(lhs.derived())).toImplementation() *
81 // (RotationQuaternion<typename LeftAndRight_::Scalar>(rhs.derived())).toImplementation()
82 // ));
83 //
84 // }
85 //};
86 
89 template<typename Left_, typename Right_>
90 class ComparisonTraits<PoseBase<Left_>, PoseBase<Right_>>{
91  public:
92  inline static bool isEqual(const PoseBase<Left_>& lhs, const PoseBase<Right_>& rhs) {
93  return lhs.derived().getPosition() == rhs.derived().getPosition() &&
94  lhs.derived().getRotation() == rhs.derived().getRotation();
95  }
96 };
97 
98 
99 } // namespace internal
100 } // namespace kindr