From 3438a44a14429fb8d600fc8fd050cbcec4133e52 Mon Sep 17 00:00:00 2001
From: wim <devnull@localhost>
Date: Tue, 13 Jul 2010 00:23:37 +0000
Subject: [PATCH] construct kdl object directly from quaternion

---
 kdl_parser/src/kdl_parser.cpp | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/kdl_parser/src/kdl_parser.cpp b/kdl_parser/src/kdl_parser.cpp
index f5abc69..114aa40 100644
--- a/kdl_parser/src/kdl_parser.cpp
+++ b/kdl_parser/src/kdl_parser.cpp
@@ -53,9 +53,7 @@ Vector toKdl(urdf::Vector3 v)
 // construct rotation
 Rotation toKdl(urdf::Rotation r)
 {
-  double roll, pitch, yaw;
-  r.getRPY(roll, pitch, yaw);
-  return Rotation::RPY(roll, pitch, yaw);
+  return Rotation::Quaternion(r.x, r.y, r.z, r.w);
 }
 
 // construct pose