tf_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 
30 #include "tf_conversions/tf_kdl.h"
32 
33 namespace tf {
34 
35  void poseTFToKDL(const tf::Pose& t, KDL::Frame& k)
36  {
37  for (unsigned int i = 0; i < 3; ++i)
38  k.p[i] = t.getOrigin()[i];
39  for (unsigned int i = 0; i < 9; ++i)
40  k.M.data[i] = t.getBasis()[i/3][i%3];
41  }
42 
43  void poseKDLToTF(const KDL::Frame& k, tf::Pose& t)
44  {
45  t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
46  t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
47  k.M.data[3], k.M.data[4], k.M.data[5],
48  k.M.data[6], k.M.data[7], k.M.data[8]));
49  }
50 
52  {
53  k = KDL::Rotation::Quaternion(t[0], t[1], t[2], t[3]);
54  }
55 
57  {
58  double x, y, z, w;
59  k.GetQuaternion(x, y, z, w);
60  t = tf::Quaternion(x, y, z, w);
61  }
62 
64  {
65  for (unsigned int i = 0; i < 3; ++i)
66  k.p[i] = t.getOrigin()[i];
67  for (unsigned int i = 0; i < 9; ++i)
68  k.M.data[i] = t.getBasis()[i/3][i%3];
69  }
70 
72  {
73  t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
74  t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
75  k.M.data[3], k.M.data[4], k.M.data[5],
76  k.M.data[6], k.M.data[7], k.M.data[8]));
77  }
78 
80  {
81  k[0] = t[0];
82  k[1] = t[1];
83  k[2] = t[2];
84  }
85 
87  {
88  t[0] = k[0];
89  t[1] = k[1];
90  t[2] = k[2];
91  }
92 
93 
94  /* DEPRECATED FUNCTIONS */
95  geometry_msgs::Pose addDelta(const geometry_msgs::Pose &pose, const geometry_msgs::Twist &twist, const double &t)
96  {
97  geometry_msgs::Pose result;
98  KDL::Twist kdl_twist;
99  KDL::Frame kdl_pose_id, kdl_pose;
100 
101  poseMsgToKDL(pose,kdl_pose);
102  twistMsgToKDL(twist,kdl_twist);
103  kdl_pose = KDL::addDelta(kdl_pose_id,kdl_twist,t)*kdl_pose;
104  poseKDLToMsg(kdl_pose,result);
105  return result;
106  }
107 
108 } // namespace tf
109 
void vectorKDLToTF(const KDL::Vector &k, tf::Vector3 &t)
Converts a tf Vector3 into a KDL Vector.
Definition: tf_kdl.cpp:86
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
geometry_msgs::Pose addDelta(const geometry_msgs::Pose &pose, const geometry_msgs::Twist &twist, const double &t) __attribute__((deprecated))
Starting from a Pose from A to B, apply a Twist with reference frame A and reference point B...
Definition: tf_kdl.cpp:95
void transformKDLToTF(const KDL::Frame &k, tf::Transform &t)
Converts a KDL Frame into a tf Transform.
Definition: tf_kdl.cpp:71
static Rotation Quaternion(double x, double y, double z, double w)
void twistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k)
TFSIMD_FORCE_INLINE const tfScalar & y() const
void quaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t)
Converts a tf Quaternion into a KDL Rotation.
Definition: tf_kdl.cpp:56
Rotation M
void GetQuaternion(double &x, double &y, double &z, double &w) const
void poseTFToKDL(const tf::Pose &t, KDL::Frame &k)
Converts a tf Pose into a KDL Frame.
Definition: tf_kdl.cpp:35
void poseKDLToTF(const KDL::Frame &k, tf::Pose &t)
Converts a KDL Frame into a tf Pose.
Definition: tf_kdl.cpp:43
void transformTFToKDL(const tf::Transform &t, KDL::Frame &k)
Converts a tf Transform into a KDL Frame.
Definition: tf_kdl.cpp:63
void vectorTFToKDL(const tf::Vector3 &t, KDL::Vector &k)
Converts a tf Vector3 into a KDL Vector.
Definition: tf_kdl.cpp:79
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
TFSIMD_FORCE_INLINE const tfScalar & x() const
void quaternionTFToKDL(const tf::Quaternion &t, KDL::Rotation &k)
Converts a tf Quaternion into a KDL Rotation.
Definition: tf_kdl.cpp:51
TFSIMD_FORCE_INLINE void setBasis(const Matrix3x3 &basis)
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
TFSIMD_FORCE_INLINE const tfScalar & z() const
TFSIMD_FORCE_INLINE const tfScalar & w() const
IMETHOD doubleVel addDelta(const doubleVel &a, const doubleVel &da, double dt=1.0)
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
double data[9]


tf_conversions
Author(s): Tully Foote
autogenerated on Mon Jun 10 2019 12:25:33