From 805feb46d5fac8054e901580c0101153045dda66 Mon Sep 17 00:00:00 2001
From: bcalli <b.calli@tudelft.nl>
Date: Wed, 23 Jul 2014 19:35:02 +0200
Subject: [PATCH] Add a transform function that accepts urpp/joint_position

---
 CMakeLists.txt            |  2 +-
 include/dr_kdl/dr_kdl.hpp | 16 ++++++++++++++++
 package.xml               |  1 +
 src/dr_kdl.cpp            | 26 ++++++++++++++++++++++++++
 4 files changed, 44 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0c38e50..246ca8c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(dr_kdl)
 
-find_package(catkin REQUIRED COMPONENTS dr_util kdl_parser sensor_msgs cmake_modules)
+find_package(catkin REQUIRED COMPONENTS dr_util kdl_parser sensor_msgs cmake_modules urpp)
 find_package(orocos_kdl REQUIRED)
 find_package(Eigen REQUIRED)
 
diff --git a/include/dr_kdl/dr_kdl.hpp b/include/dr_kdl/dr_kdl.hpp
index 9e9a267..90577f4 100644
--- a/include/dr_kdl/dr_kdl.hpp
+++ b/include/dr_kdl/dr_kdl.hpp
@@ -5,6 +5,7 @@
 #include <sensor_msgs/JointState.h>
 #include <kdl/tree.hpp>
 #include <Eigen/Geometry>
+#include "urpp/kinematics.hpp"
 
 namespace dr {
 
@@ -43,6 +44,12 @@ Eigen::Isometry3d getTransform(
 	std::vector<double> const & joint_positions   ///< The positions of the joints in the same order as the joint name vector.
 );
 
+Eigen::Isometry3d getTransform(
+	KDL::Chain const & chain, 					  ///< The chain
+	urpp::joint_position const & joint_positions, ///< The joint positions
+	const std::string & joint_name_prefix = "ur_" ///< Prefix for ur arm joint names.
+);
+
 
 /// KDL tree wrapper.
 class KdlTree : public KDL::Tree {
@@ -109,6 +116,15 @@ class KdlTree : public KDL::Tree {
 		return getTransform(getChain(source, target), joints.name, joints.position);
 	}
 
+	Eigen::Isometry3d transform(
+		std::string const & source,                   ///< The source frame.
+		std::string const & target,                   ///< The target frame.
+		urpp::joint_position const & joint_positions, ///< The joint positions.
+		std::string const & joint_name_prefix = "ur_" ///< joint name prefix for ur arm.
+	) const {
+		return getTransform(getChain(source, target), joint_positions, joint_name_prefix);
+	}
+
 
 };
 
diff --git a/package.xml b/package.xml
index 3fc6917..9052fb1 100644
--- a/package.xml
+++ b/package.xml
@@ -15,6 +15,7 @@
   <build_depend>dr_util</build_depend>
   <build_depend>kdl_parser</build_depend>
   <build_depend>cmake_modules</build_depend>
+  <build_depend>urpp</build_depend>
   <run_depend>kdl_parser</run_depend>
 
   <export>
diff --git a/src/dr_kdl.cpp b/src/dr_kdl.cpp
index e218a69..9ca1f38 100644
--- a/src/dr_kdl.cpp
+++ b/src/dr_kdl.cpp
@@ -87,6 +87,32 @@ Eigen::Isometry3d getTransform(KDL::Chain const & chain, std::vector<std::string
 	return toEigen(transform);
 }
 
+Eigen::Isometry3d getTransform(KDL::Chain const & chain, urpp::joint_position const & joint_positions, const std::string & joint_name_prefix) {
+	
+	std::vector<std::string> joint_names = {joint_name_prefix+"shoulder_pan_joint",
+											joint_name_prefix+"shoulder_lift_joint",
+											joint_name_prefix+"elbow_joint",
+											joint_name_prefix+"wrist_1_joint",
+											joint_name_prefix+"wrist_2_joint",
+											joint_name_prefix+"wrist_3_joint"};
+
+	KDL::Frame transform = KDL::Frame::Identity();
+
+	for (auto const & segment : chain.segments) {
+		// Fixed joints.
+		if (segment.getJoint().getType() == KDL::Joint::JointType::None) {
+			transform = transform * segment.pose(0);
+		
+		// Look up non-fixed joints in map.
+		} else {
+			auto index = std::find(joint_names.begin(), joint_names.end(), segment.getJoint().getName());
+			if (index == joint_names.end()) throw std::runtime_error("Joint `" + segment.getName() + "' not found in joint list.");
+			transform = transform * segment.pose(joint_positions[index - joint_names.begin()]);
+		}
+	}
+	return toEigen(transform);
+}
+
 KDL::Chain KdlTree::getChain(std::string const & start, std::string const & end) const {
 	KDL::Chain chain;
 	if (KDL::Tree::getChain(start, end, chain)) {