Kindr
Kinematics and Dynamics for Robotics
RotationDiff.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 
33 
34 namespace kindr {
35 
36 
37 template<typename PrimType_>
39 
40 template<typename PrimType_>
42 
43 template<typename PrimType_>
45 
46 template<typename PrimType_>
48 
49 template<typename PrimType_>
50 class EulerAnglesZyxDiff;
51 
52 template<typename PrimType_>
53 class EulerAnglesXyzDiff;
54 
55 
61 template<typename PrimType_>
62 inline static Eigen::Matrix<PrimType_, 3, 3> getJacobianOfExponentialMap(const Eigen::Matrix<PrimType_, 3, 1>& vector) {
63  const PrimType_ norm = vector.norm();
64  const Eigen::Matrix<PrimType_, 3, 3> skewMatrix = getSkewMatrixFromVector(vector);
65  if (norm < 1.0e-4) {
66  return Eigen::Matrix<PrimType_, 3, 3>::Identity() + 0.5*skewMatrix;
67  }
68  return Eigen::Matrix<PrimType_, 3, 3>::Identity() + (PrimType_(1.0) - cos(norm))/(norm*norm)*skewMatrix + (norm - sin(norm))/(norm*norm*norm)*(skewMatrix*skewMatrix);
69 }
70 
71 
72 } // namespace kindr
73 
74 
81 
82 
static Eigen::Matrix< PrimType_, 3, 3 > getJacobianOfExponentialMap(const Eigen::Matrix< PrimType_, 3, 1 > &vector)
Gets the 3x3 Jacobian of the exponential map.
Implementation of time derivatives of Euler angles (Z,Y&#39;,X&#39;&#39; / yaw,pitch,roll) based on Eigen::Matrix...
Time derivative of a rotation matrix.
Implementation of time derivatives of Euler angles (X,Y&#39;,Z&#39;&#39; / roll,pitch,yaw) based on Eigen::Matrix...
static Eigen::Matrix< PrimType_, 3, 3 > getSkewMatrixFromVector(const Eigen::Matrix< PrimType_, 3, 1 > &vec)
Gets a skew-symmetric matrix from a (column) vector.
Time derivative of a rotation quaternion.