eigen_kdl.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Willow Garage, Inc.
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  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
31 
32 namespace tf {
33 
34 void quaternionKDLToEigen(const KDL::Rotation &k, Eigen::Quaterniond &e)
35 {
36  // // is it faster to do this?
37  k.GetQuaternion(e.x(), e.y(), e.z(), e.w());
38 
39  // or this?
40  //double x, y, z, w;
41  //k.GetQuaternion(x, y, z, w);
42  //e = Eigen::Quaterniond(w, x, y, z);
43 }
44 
45 void quaternionEigenToKDL(const Eigen::Quaterniond &e, KDL::Rotation &k)
46 {
47  k = KDL::Rotation::Quaternion(e.x(), e.y(), e.z(), e.w());
48 }
49 
50 namespace {
51  template<typename T>
52  void transformKDLToEigenImpl(const KDL::Frame &k, T &e)
53  {
54  // translation
55  for (unsigned int i = 0; i < 3; ++i)
56  e(i, 3) = k.p[i];
57 
58  // rotation matrix
59  for (unsigned int i = 0; i < 9; ++i)
60  e(i/3, i%3) = k.M.data[i];
61 
62  // "identity" row
63  e(3,0) = 0.0;
64  e(3,1) = 0.0;
65  e(3,2) = 0.0;
66  e(3,3) = 1.0;
67  }
68 
69  template<typename T>
70  void transformEigenToKDLImpl(const T &e, KDL::Frame &k)
71  {
72  for (unsigned int i = 0; i < 3; ++i)
73  k.p[i] = e(i, 3);
74  for (unsigned int i = 0; i < 9; ++i)
75  k.M.data[i] = e(i/3, i%3);
76  }
77 
78 }
79 
80 void transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e)
81 {
82  transformKDLToEigenImpl(k, e);
83 }
84 
85 void transformKDLToEigen(const KDL::Frame &k, Eigen::Isometry3d &e)
86 {
87  transformKDLToEigenImpl(k, e);
88 }
89 
90 void transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k)
91 {
92  transformEigenToKDLImpl(e, k);
93 }
94 
95 void transformEigenToKDL(const Eigen::Isometry3d &e, KDL::Frame &k)
96 {
97  transformEigenToKDLImpl(e, k);
98 }
99 
100 void twistEigenToKDL(const Eigen::Matrix<double, 6, 1> &e, KDL::Twist &k)
101 {
102  for(int i = 0; i < 6; ++i)
103  k[i] = e[i];
104 }
105 
106 void twistKDLToEigen(const KDL::Twist &k, Eigen::Matrix<double, 6, 1> &e)
107 {
108  for(int i = 0; i < 6; ++i)
109  e[i] = k[i];
110 }
111 
112 void vectorEigenToKDL(const Eigen::Matrix<double, 3, 1> &e, KDL::Vector &k)
113 {
114  for(int i = 0; i < 3; ++i)
115  k[i] = e[i];
116 }
117 void vectorKDLToEigen(const KDL::Vector &k, Eigen::Matrix<double, 3, 1> &e)
118 {
119  for(int i = 0; i < 3; ++i)
120  e[i] = k[i];
121 }
122 
123 void wrenchKDLToEigen(const KDL::Wrench &k, Eigen::Matrix<double, 6, 1> &e)
124 {
125  for(int i = 0; i < 6; ++i)
126  e[i] = k[i];
127 }
128 
129 void wrenchEigenToKDL(const Eigen::Matrix<double, 6, 1> &e, KDL::Wrench &k)
130 {
131  for(int i = 0; i < 6; ++i)
132  k[i] = e[i];
133 }
134 
135 
136 } // namespace
static Rotation Quaternion(double x, double y, double z, double w)
void quaternionKDLToEigen(const KDL::Rotation &k, Eigen::Quaterniond &e)
Converts a KDL rotation into an Eigen quaternion.
Definition: eigen_kdl.cpp:34
Definition: eigen_kdl.h:42
void wrenchKDLToEigen(const KDL::Wrench &k, Eigen::Matrix< double, 6, 1 > &e)
Converts a KDL wrench into an Eigen matrix.
Definition: eigen_kdl.cpp:123
void transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k)
Converts an Eigen Affine3d into a KDL frame.
Definition: eigen_kdl.cpp:90
Rotation M
void GetQuaternion(double &x, double &y, double &z, double &w) const
void twistKDLToEigen(const KDL::Twist &k, Eigen::Matrix< double, 6, 1 > &e)
Converts a KDL twist into an Eigen matrix.
Definition: eigen_kdl.cpp:106
void quaternionEigenToKDL(const Eigen::Quaterniond &e, KDL::Rotation &k)
Converts an Eigen quaternion into a KDL rotation.
Definition: eigen_kdl.cpp:45
void vectorEigenToKDL(const Eigen::Matrix< double, 3, 1 > &e, KDL::Vector &k)
Converts an Eigen matrix into a KDL vector.
Definition: eigen_kdl.cpp:112
void vectorKDLToEigen(const KDL::Vector &k, Eigen::Matrix< double, 3, 1 > &e)
Converts a KDL vector into an Eigen matrix.
Definition: eigen_kdl.cpp:117
void twistEigenToKDL(const Eigen::Matrix< double, 6, 1 > &e, KDL::Twist &k)
Converts an Eigen matrix into a KDL Twist.
Definition: eigen_kdl.cpp:100
void transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e)
Converts a KDL frame into an Eigen Affine3d.
Definition: eigen_kdl.cpp:80
void wrenchEigenToKDL(const Eigen::Matrix< double, 6, 1 > &e, KDL::Wrench &k)
Converts an Eigen matrix into a KDL wrench.
Definition: eigen_kdl.cpp:129
double data[9]


eigen_conversions
Author(s): Stuart Glaser, Adam Leeper
autogenerated on Mon Jun 10 2019 12:25:24