Kindr
Kinematics and Dynamics for Robotics
RotationConversion.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 /*
30  * convention.hpp
31  *
32  * Created on: Jun 16, 2016
33  * Author: Christian Gehring
34  */
35 
36 #pragma once
37 
38 #include <kindr/Core>
39 
40 namespace kindr {
41 
45 template<typename OtherRotation_, typename OtherVelocity_, typename PrimType_>
47  public:
48  inline static void convertToOtherRotation(OtherRotation_& otherRotation, const kindr::RotationQuaternion<PrimType_>& quaternionIn) {
49  // Implement rotation = f(quaternionIn);
50  static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method convertToOtherRotation!");
51  }
52 
53  inline static void convertToKindr(kindr::RotationQuaternion<PrimType_>& quaternion, const OtherRotation_& otherRotation) {
54  static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method convertToKindr!");
55  }
56 
57  inline static void convertToOtherVelocityVector(OtherVelocity_& otherVelocity, const OtherRotation_& rotation, const Eigen::Matrix<PrimType_,3,1>& velocityIn) {
58  // Implement otherVelocity = g(velocityIn, rotation);
59  static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method convertToOtherVelocityVector!");
60  }
61 
62  inline static void getRotationMatrixFromRotation(Eigen::Matrix<PrimType_,3,3>& rotationMatrix, const OtherRotation_& rotation) {
63  // Implement rotationMatrix = C(rotation);
64  static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method getRotationMatrixFromRotation!");
65  }
66 
67  inline static void concatenate(OtherRotation_& result, const OtherRotation_& rot1, OtherRotation_& rot2) {
68  static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method concatenate!");
69  }
70 
71  inline static void rotateVector(Eigen::Matrix<PrimType_,3,1>& A_r, const OtherRotation_& rotationBToA, const Eigen::Matrix<PrimType_,3,1>& B_r) {
72  // Implement A_r = rotationBtoA.rotate(A_r);
73  static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method rotateVector!");
74  }
75 
76  inline static void boxPlus(OtherRotation_& result, const OtherRotation_& rotation, const OtherVelocity_& velocity) {
77  // Implement result = rotation.boxPlus(vector);
78  static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method boxPlus!");
79  }
80 
81  inline static bool testRotation(const OtherRotation_& expected, const OtherRotation_& actual) {
82  // Implement EXPECT_NEAR(expected, actual, 1.0e-6);
83  static_assert(sizeof(OtherRotation_) == -1, "You need to implement the method testRotation!");
84  return false;
85  }
86 };
87 
88 } // namespace
static void convertToOtherVelocityVector(OtherVelocity_ &otherVelocity, const OtherRotation_ &rotation, const Eigen::Matrix< PrimType_, 3, 1 > &velocityIn)
static bool testRotation(const OtherRotation_ &expected, const OtherRotation_ &actual)
static void concatenate(OtherRotation_ &result, const OtherRotation_ &rot1, OtherRotation_ &rot2)
static void getRotationMatrixFromRotation(Eigen::Matrix< PrimType_, 3, 3 > &rotationMatrix, const OtherRotation_ &rotation)
static void convertToKindr(kindr::RotationQuaternion< PrimType_ > &quaternion, const OtherRotation_ &otherRotation)
static void rotateVector(Eigen::Matrix< PrimType_, 3, 1 > &A_r, const OtherRotation_ &rotationBToA, const Eigen::Matrix< PrimType_, 3, 1 > &B_r)
static void convertToOtherRotation(OtherRotation_ &otherRotation, const kindr::RotationQuaternion< PrimType_ > &quaternionIn)
static void boxPlus(OtherRotation_ &result, const OtherRotation_ &rotation, const OtherVelocity_ &velocity)
Implementation of quaternion rotation based on Eigen::Quaternion.
Definition: Rotation.hpp:46