Add KDL tree wrapper.

CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(dr_kdl)
+
+find_package(catkin REQUIRED COMPONENTS dr_util kdl_parser)
+find_package(Eigen REQUIRED)
+
+catkin_package(
+	INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS}
+	LIBRARIES ${PROJECT_NAME}
+)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -std=c++11 -Wall -Wextra")
+
+include_directories(
+	include/${PROJECT_NAME}
+	${catkin_INCLUDE_DIRS}
+	${Eigen_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME}
+	src/dr_kdl.cpp
+)
+
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen_LIBRARIES})
+
+install(TARGETS ${PROJECT_NAME}
+	ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+	RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+
+install(DIRECTORY include/${PROJECT_NAME}/
+	DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+	FILES_MATCHING PATTERN "*.hpp"
+)

include/dr_kdl/dr_kdl.hpp

@@ -0,0 +1,62 @@
+#pragma once
+
+#include <utility>
+
+#include <kdl/tree.hpp>
+#include <dr_util/eigen.hpp>
+
+namespace dr {
+
+/// Create an Eigen transform from a KDL Frame.
+inline Eigen::Isometry3d toEigen(KDL::Frame const & frame) {
+	Eigen::Isometry3d result;
+
+	// Rotation.
+	for (int i = 0; i < 3; ++i) {
+		for (int j = 0; j < 3; ++j) {
+			result(i, j) = frame.M(i, j);
+		}
+	}
+
+	// Translation.
+	for (int i = 0; i < 3; ++i) {
+		result(i, 3) = frame.p(i);
+	}
+
+	// Homogenous bit.
+	for (int j = 0; j < 4; ++j) {
+		result(3, j) = j == 3;
+	}
+
+	return result;
+}
+
+/// Get a the transform from the base to the end of a chain.
+/**
+ * Throws if a non-fixed joint is encountered in the chain.
+ * \return The transform from the base of the chain to the end.
+ */
+Eigen::Isometry3d getTransform(
+	KDL::Chain const & chain ///< The chain.
+);
+
+
+/// KDL tree wrapper.
+class KdlTree : public KDL::Tree {
+public:
+	KdlTree() {}
+	KdlTree(KDL::Tree const &  tree) : KDL::Tree{tree} {}
+	KdlTree(KDL::Tree const && tree) : KDL::Tree{std::move(tree)} {}
+
+	static KdlTree fromString(std::string const & urdf);
+	static KdlTree fromParameter(std::string const & parameter);
+	static KdlTree fromFile(std::string const & filename);
+
+	/// Get a transform from one frame to another.
+	/**
+	 * Throws if there is no chain between the frames or the chain contains a non-fixed joint.
+	 */
+	Eigen::Isometry3d transform(std::string const & source, std::string const & target) const;
+};
+
+}

package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package>
+  <name>dr_kdl</name>
+  <version>1.0.0</version>
+  <description>Delft Robotics KDL functions.</description>
+
+  <maintainer email="[email protected]">Maarten de Vries</maintainer>
+
+  <license>GPLv3</license>
+
+  <author email="[email protected]">Maarten de Vries</author>
+  <author email="[email protected]">Hans Gaiser</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>dr_util</build_depend>
+  <build_depend>kdl_parser</build_depend>
+  <run_depend>kdl_parser</run_depend>
+
+  <export>
+  </export>
+</package>

src/dr_kdl.cpp

@@ -0,0 +1,49 @@
+#include <kdl_parser/kdl_parser.hpp>
+#include <kdl/frames_io.hpp>
+
+#include "dr_kdl.hpp"
+
+namespace dr {
+
+/// Get a the transform from the base to the tip of a chain.
+Eigen::Isometry3d getTransform(KDL::Chain const & chain) {
+	KDL::Frame transform = KDL::Frame::Identity();
+	for (auto const & segment : chain.segments) {
+		// Make sure we're only dealing with fixed joints.
+		if (segment.getJoint().getType() != KDL::Joint::JointType::None) {
+			throw std::runtime_error("Non-fixed joint `" + segment.getName() + "' found in chain, but no joint positions are given.");
+		}
+		transform = transform * segment.pose(0);
+	}
+	return toEigen(transform);
+}
+
+
+Eigen::Isometry3d KdlTree::transform(std::string const & source, std::string const & target) const {
+	KDL::Chain chain;
+	if (getChain(source, target, chain)) {
+		return getTransform(chain);
+	} else {
+		throw std::runtime_error("No chain found from frame `" + source + "' to frame `" + target + "'.");
+	}
+}
+
+KdlTree KdlTree::fromParameter(std::string const & parameter) {
+	KDL::Tree kdl;
+	kdl_parser::treeFromParam(parameter, kdl);
+	return kdl;
+}
+
+KdlTree KdlTree::fromString(std::string const & urdf) {
+	KDL::Tree kdl;
+	kdl_parser::treeFromString(urdf, kdl);
+	return kdl;
+}
+
+KdlTree KdlTree::fromFile(std::string const & filename) {
+	KDL::Tree kdl;
+	kdl_parser::treeFromFile(filename, kdl);
+	return kdl;
+}
+
+}