Add a transform function that accepts urpp/joint_position

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)
 

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);
+	}
+
 
 };
 

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>

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)) {