diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2943612..b3e3107 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -29,6 +29,9 @@ add_library(${PROJECT_NAME}
 
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${orocos_kdl_LIBRARIES})
 
+catkin_add_gtest(${PROJECT_NAME}_test_kdl test/kdl.cpp)
+target_link_libraries(${PROJECT_NAME}_test_kdl ${PROJECT_NAME} ${catkin_LIBRARIES})
+
 install(TARGETS ${PROJECT_NAME}
 	ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
diff --git a/test/kdl.cpp b/test/kdl.cpp
new file mode 100644
index 0000000..2538d99
--- /dev/null
+++ b/test/kdl.cpp
@@ -0,0 +1,38 @@
+#include "dr_kdl.hpp"
+
+#include <dr_eigen/eigen.hpp>
+#include <dr_util/geometry.hpp>
+
+#include <gtest/gtest.h>
+
+
+int main(int argc, char * * argv) {
+	testing::InitGoogleTest(&argc, argv);
+	return RUN_ALL_TESTS();
+}
+
+namespace dr {
+
+TEST(KdlTree, pose) {
+	KdlTree kdl;
+	kdl.addSegment(KDL::Segment("arm", KDL::Joint("joint", KDL::Joint::RotZ), KDL::Frame(KDL::Rotation::Identity(), KDL::Vector(1, 0, 0))), "root");
+
+	ASSERT_TRUE(kdl.pose("root", "arm", {{"joint", 0.0 * pi()}}).isApprox(translate(1, 0, 0)  * rotateZ(0.0 * pi())));
+	ASSERT_TRUE(kdl.pose("root", "arm", {{"joint", 0.5 * pi()}}).isApprox(translate(0, 1, 0)  * rotateZ(0.5 * pi())));
+	ASSERT_TRUE(kdl.pose("root", "arm", {{"joint", 1.0 * pi()}}).isApprox(translate(-1, 0, 0) * rotateZ(1.0 * pi())));
+	ASSERT_TRUE(kdl.pose("root", "arm", {{"joint", 1.5 * pi()}}).isApprox(translate(0, -1, 0) * rotateZ(1.5 * pi())));
+	ASSERT_TRUE(kdl.pose("root", "arm", {{"joint", 2.0 * pi()}}).isApprox(translate(1, 0, 0)  * rotateZ(0.0 * pi())));
+}
+
+TEST(KdlTree, transform) {
+	KdlTree kdl;
+	kdl.addSegment(KDL::Segment("arm", KDL::Joint("joint", KDL::Joint::RotZ), KDL::Frame(KDL::Rotation::Identity(), KDL::Vector(1, 0, 0))), "root");
+
+	ASSERT_TRUE(kdl.transform("arm", "root", {{"joint", 0.0 * pi()}}).isApprox(translate(1, 0, 0)  * rotateZ(0.0 * pi())));
+	ASSERT_TRUE(kdl.transform("arm", "root", {{"joint", 0.5 * pi()}}).isApprox(translate(0, 1, 0)  * rotateZ(0.5 * pi())));
+	ASSERT_TRUE(kdl.transform("arm", "root", {{"joint", 1.0 * pi()}}).isApprox(translate(-1, 0, 0) * rotateZ(1.0 * pi())));
+	ASSERT_TRUE(kdl.transform("arm", "root", {{"joint", 1.5 * pi()}}).isApprox(translate(0, -1, 0) * rotateZ(1.5 * pi())));
+	ASSERT_TRUE(kdl.transform("arm", "root", {{"joint", 2.0 * pi()}}).isApprox(translate(1, 0, 0)  * rotateZ(0.0 * pi())));
+}
+
+}