50 k.
p[0] = m.position.
x;
51 k.
p[1] = m.position.
y;
52 k.
p[2] = m.position.
z;
59 m.position.x = k.
p[0];
60 m.position.
y = k.
p[1];
61 m.position.
z = k.
p[2];
63 k.
M.
GetQuaternion(m.orientation.x, m.orientation.y, m.orientation.z, m.orientation.w);
78 k.
p[0] = m.translation.
x;
79 k.
p[1] = m.translation.
y;
80 k.
p[2] = m.translation.
z;
87 m.translation.x = k.
p[0];
88 m.translation.
y = k.
p[1];
89 m.translation.
z = k.
p[2];
91 k.
M.
GetQuaternion(m.rotation.x, m.rotation.y, m.rotation.z, m.rotation.w);
96 m.linear.x = t.
vel[0];
97 m.linear.
y = t.
vel[1];
98 m.linear.
z = t.
vel[2];
99 m.angular.
x = t.
rot[0];
100 m.angular.
y = t.
rot[1];
101 m.angular.
z = t.
rot[2];
106 t.
vel[0] = m.linear.
x;
107 t.
vel[1] = m.linear.
y;
108 t.
vel[2] = m.linear.
z;
109 t.
rot[0] = m.angular.
x;
110 t.
rot[1] = m.angular.
y;
111 t.
rot[2] = m.angular.
z;
void vectorKDLToMsg(const KDL::Vector &k, geometry_msgs::Vector3 &m)
Converts a KDL Vector into a Vector3 message.
void transformKDLToMsg(const KDL::Frame &k, geometry_msgs::Transform &m)
Converts a KDL Frame into a Transform message.
static Rotation Quaternion(double x, double y, double z, double w)
void twistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k)
Converts a Twist message into a KDL Twist.
void GetQuaternion(double &x, double &y, double &z, double &w) const
void TwistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k) __attribute__((deprecated))
Converts a Twist message into a KDL Twist.
void quaternionMsgToKDL(const geometry_msgs::Quaternion &m, KDL::Rotation &k)
Converts a quaternion message into a KDL Rotation.
void transformMsgToKDL(const geometry_msgs::Transform &m, KDL::Frame &k)
Converts a Transform message into a KDL Frame.
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
Converts a Pose message into a KDL Frame.
void twistKDLToMsg(const KDL::Twist &k, geometry_msgs::Twist &m)
Converts a KDL Twist into a Twist message.
void PoseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m) __attribute__((deprecated))
Converts a KDL Frame into a Pose message.
void wrenchMsgToKDL(const geometry_msgs::Wrench &m, KDL::Wrench &k)
Converts a Wrench message into a KDL Wrench.
void quaternionKDLToMsg(const KDL::Rotation &k, geometry_msgs::Quaternion &m)
Converts a KDL Rotation into a message quaternion.
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
Converts a KDL Frame into a Pose message.
void pointKDLToMsg(const KDL::Vector &k, geometry_msgs::Point &m)
Converts a KDL Vector into a geometry_msgs Vector3.
void pointMsgToKDL(const geometry_msgs::Point &m, KDL::Vector &k)
Conversion functions from/to the corresponding KDL and geometry_msgs types.
void PoseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k) __attribute__((deprecated))
Converts a Pose message into a KDL Frame.
void wrenchKDLToMsg(const KDL::Wrench &k, geometry_msgs::Wrench &m)
Converts a KDL Wrench into a Wrench message.
void TwistKDLToMsg(const KDL::Twist &k, geometry_msgs::Twist &m) __attribute__((deprecated))
Converts a KDL Twist into a Twist message.
void vectorMsgToKDL(const geometry_msgs::Vector3 &m, KDL::Vector &k)
Converts a Vector3 message into a KDL Vector.