diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
index fac326a8d..21996d174 100644
--- a/.github/workflows/main.yml
+++ b/.github/workflows/main.yml
@@ -24,6 +24,7 @@ jobs:
       - name: Install
         run: |
           sudo apt-get install libeigen3-dev libcppunit-dev python-sip-dev
+          sudo apt-get install python-psutil python-future python3-psutil
       - name: Build orocos_kdl
         run: |
           cd orocos_kdl
@@ -41,6 +42,15 @@ jobs:
           cmake -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} ./..
           make
           sudo make install
+      - name: Build PyKDL Python 3
+        run: |
+          cd python_orocos_kdl
+          mkdir build3
+          cd build3
+          export ROS_PYTHON_VERSION=3
+          cmake -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} ./..
+          make
+          sudo make install
       - name: Update LD_LIBRARY_PATH
         run: |
           export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib
@@ -53,6 +63,10 @@ jobs:
         run: |
           cd python_orocos_kdl
           python2 tests/PyKDLtest.py
+      - name: Test PyKDL Python 3
+        run: |
+          cd python_orocos_kdl
+          python3 tests/PyKDLtest.py
 
   industrial_ci:
     name: Industrial CI - ${{ matrix.env.ROS_DISTRO }}
diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 000000000..2c9699f57
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,4 @@
+[submodule "python_orocos_kdl/pybind11"]
+    path = python_orocos_kdl/pybind11
+    url = https://github.com/pybind/pybind11.git
+    branch = v2.6.0
diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt
index c323e145a..2bbc61436 100644
--- a/orocos_kdl/CMakeLists.txt
+++ b/orocos_kdl/CMakeLists.txt
@@ -2,6 +2,9 @@
 # Test CMake version
 #
 CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+IF(POLICY CMP0048)
+  CMAKE_POLICY(SET CMP0048 NEW)
+ENDIF()
 #MARK_AS_ADVANCED( FORCE CMAKE_BACKWARDS_COMPATIBILITY )
 
 
@@ -44,14 +47,14 @@ SET( KDL_CFLAGS "")
 
 find_package(Eigen 3 QUIET)
 if(NOT Eigen_FOUND)
-  include(${PROJ_SOURCE_DIR}/config/FindEigen3.cmake)
+  include(${PROJ_SOURCE_DIR}/cmake/FindEigen3.cmake)
   set(Eigen_INCLUDE_DIR "${EIGEN3_INCLUDE_DIR}")
 endif()
 include_directories(${Eigen_INCLUDE_DIR})
 SET(KDL_CFLAGS "${KDL_CFLAGS} -I${Eigen_INCLUDE_DIR}")
 
 # Check the platform STL containers capabilities
-include(config/CheckSTLContainers.cmake)
+include(cmake/CheckSTLContainers.cmake)
 CHECK_STL_CONTAINERS()
 
 # Set the default option appropriately
@@ -64,6 +67,12 @@ endif(HAVE_STL_CONTAINER_INCOMPLETE_TYPES)
 # Allow the user to select the Tree API version to use
 set(KDL_USE_NEW_TREE_INTERFACE ${KDL_USE_NEW_TREE_INTERFACE_DEFAULT} CACHE BOOL "Use the new KDL Tree interface")
 
+#Sanity check, inform the user
+if(NOT HAVE_STL_CONTAINER_INCOMPLETE_TYPES AND NOT KDL_USE_NEW_TREE_INTERFACE)
+    message(WARNING "You have chosen to use the current Tree Interface, but your platform doesn't support containers of "
+        "incomplete types, this configuration is likely invalid")
+endif()
+
 # The new interface requires the use of shared pointers
 if(KDL_USE_NEW_TREE_INTERFACE)
     # We need shared_ptr from boost since not all compilers are c++11 capable
@@ -72,8 +81,6 @@ if(KDL_USE_NEW_TREE_INTERFACE)
     set(KDL_INCLUDE_DIRS ${Boost_INCLUDE_DIRS})
 endif(KDL_USE_NEW_TREE_INTERFACE)
 
-INCLUDE (${PROJ_SOURCE_DIR}/config/DependentOption.cmake)
-
 OPTION(ENABLE_TESTS OFF "Enable building of tests")
 IF( ENABLE_TESTS )
   # If not in standard paths, set CMAKE_xxx_PATH's in environment, eg.
diff --git a/orocos_kdl/config/CheckSTLContainers.cmake b/orocos_kdl/cmake/CheckSTLContainers.cmake
similarity index 100%
rename from orocos_kdl/config/CheckSTLContainers.cmake
rename to orocos_kdl/cmake/CheckSTLContainers.cmake
diff --git a/orocos_kdl/config/FindEigen3.cmake b/orocos_kdl/cmake/FindEigen3.cmake
similarity index 100%
rename from orocos_kdl/config/FindEigen3.cmake
rename to orocos_kdl/cmake/FindEigen3.cmake
diff --git a/orocos_kdl/config/FindPkgConfig.cmake b/orocos_kdl/cmake/FindPkgConfig.cmake
similarity index 100%
rename from orocos_kdl/config/FindPkgConfig.cmake
rename to orocos_kdl/cmake/FindPkgConfig.cmake
diff --git a/orocos_kdl/config/DependentOption.cmake b/orocos_kdl/config/DependentOption.cmake
deleted file mode 100644
index 4ec4a6e1b..000000000
--- a/orocos_kdl/config/DependentOption.cmake
+++ /dev/null
@@ -1,37 +0,0 @@
-# - Macro to provide an option dependent on other options.
-# This macro presents an option to the user only if a set of other
-# conditions are true.  When the option is not presented a default
-# value is used, but any value set by the user is preserved for when
-# the option is presented again.
-# Example invocation:
-#  DEPENDENT_OPTION(USE_FOO "Use Foo" ON
-#                         "USE_BAR;NOT USE_ZOT" OFF)
-# If USE_BAR is true and USE_ZOT is false, this provides an option called
-# USE_FOO that defaults to ON.  Otherwise, it sets USE_FOO to OFF.  If
-# the status of USE_BAR or USE_ZOT ever changes, any value for the
-# USE_FOO option is saved so that when the option is re-enabled it
-# retains its old value.
-MACRO(DEPENDENT_OPTION option doc default depends force)
-  IF(${option}_ISSET MATCHES "^${option}_ISSET$")
-    SET(${option}_AVAILABLE 1)
-    FOREACH(d ${depends})
-      STRING(REGEX REPLACE " +" ";" CMAKE_DEPENDENT_OPTION_DEP "${d}")
-      IF(${CMAKE_DEPENDENT_OPTION_DEP})
-      ELSE(${CMAKE_DEPENDENT_OPTION_DEP})
-        SET(${option}_AVAILABLE 0)
-      ENDIF(${CMAKE_DEPENDENT_OPTION_DEP})
-    ENDFOREACH(d)
-    IF(${option}_AVAILABLE)
-      OPTION(${option} "${doc}" "${default}")
-      SET(${option} "${${option}}" CACHE BOOL "${doc}" FORCE)
-    ELSE(${option}_AVAILABLE)
-      IF(${option} MATCHES "^${option}$")
-      ELSE(${option} MATCHES "^${option}$")
-        SET(${option} "${${option}}" CACHE INTERNAL "${doc}")
-      ENDIF(${option} MATCHES "^${option}$")
-      SET(${option} ${force})
-    ENDIF(${option}_AVAILABLE)
-  ELSE(${option}_ISSET MATCHES "^${option}_ISSET$")
-    SET(${option} "${${option}_ISSET}")
-  ENDIF(${option}_ISSET MATCHES "^${option}_ISSET$")
-ENDMACRO(DEPENDENT_OPTION)
diff --git a/orocos_kdl/doc/tex/UserManual.tex b/orocos_kdl/doc/tex/UserManual.tex
index 784b0ad1a..4accacc2a 100644
--- a/orocos_kdl/doc/tex/UserManual.tex
+++ b/orocos_kdl/doc/tex/UserManual.tex
@@ -120,7 +120,7 @@ \subsection{Vector}
 
 \paragraph{Add and subtract vectors}
 \label{sec:add-subtract-vectors}
-You can add or substract a vector with another vector:
+You can add or subtract a vector with another vector:
 \begin{lstlisting}
   v2+=v1;
   v3-=v1;
diff --git a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp
index f2121d4d3..01f8f5d71 100644
--- a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp
+++ b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp
@@ -54,6 +54,8 @@ estimate of shortest time per invposkin (ms) 0.155544
 #include <models.hpp>
 #include <chainiksolverpos_lma.hpp>
 #include <chainfksolverpos_recursive.hpp>
+#include <utilities/utility.h>
+
 #include <boost/timer.hpp>
 
 /**
@@ -90,9 +92,9 @@ void test_inverseposkin(KDL::Chain& chain) {
 	JntArray q_sol(n);
 	for (int trial=0;trial<num_of_trials;++trial) {
 		q.data.setRandom();
-		q.data *= M_PI;
+		q.data *= PI;
 		q_init.data.setRandom();
-		q_init.data *= M_PI;
+		q_init.data *= PI;
 		Frame pos_goal,pos_reached;
 		fwdkin.JntToCart(q,pos_goal);
 		//solver.compute_fwdpos(q.data);
diff --git a/orocos_kdl/examples/geometry.cpp b/orocos_kdl/examples/geometry.cpp
index b90599f4d..5f9cfc764 100644
--- a/orocos_kdl/examples/geometry.cpp
+++ b/orocos_kdl/examples/geometry.cpp
@@ -1,21 +1,21 @@
 #include <frames.hpp>
 #include <frames_io.hpp>
+#include <utilities/utility.h>
 
 int main()
 {
-
     //Creating Vectors
     KDL::Vector v1;//Default constructor
     KDL::Vector v2(1.0,2.0,3.0);//Most used constructor
     KDL::Vector v3(v2);//Copy constructor
     KDL::Vector v4 = KDL::Vector::Zero();//Static member
-    
+
     //Use operator << to print the values of your vector
     std::cout<<"v1 ="<<v1<<std::endl;
     std::cout<<"v2 = "<<v2<<std::endl;
     std::cout<<"v3 = "<<v3<<std::endl;
     std::cout<<"v4 = "<<v4<<std::endl;
-    
+
     //Get/Set values of a vector
     v1[0]=4.0;
     v1[1]=5.0;
@@ -26,7 +26,7 @@ int main()
     v3.x(10.0);
     v3.y(11.0);
     v3.z(12.0);
-    
+
     std::cout<<"v1: "<<v1[0]<<", "<<v1[1]<<", "<<v1[2]<<std::endl;
     std::cout<<"v2: "<<v2(0)<<", "<<v2(1)<<", "<<v2(2)<<std::endl;
     std::cout<<"v3: "<<v3.x()<<", "<<v3.y()<<", "<<v3.z()<<std::endl;
@@ -35,13 +35,13 @@ int main()
     std::cout<<"2*v2 = "<<2*v2<<std::endl;
     std::cout<<"v1*2 = "<<v1*2<<std::endl;
     std::cout<<"v1/2 = "<<v1/2<<std::endl;
-    
+
     //vector - vector operators
     std::cout<<"v1+v2 = "<<v1+v2<<std::endl;
     std::cout<<"v3-v1 = "<<v3-v1<<std::endl;
 
     v3-=v1;
-    v2+=v1;    
+    v2+=v1;
     std::cout<<"v3-=v1; v3 = "<<v3<<std::endl;
     std::cout<<"v2+=v1; v2 = "<<v2<<std::endl;
 
@@ -54,17 +54,17 @@ int main()
     std::cout<<"v1=-v2; v1="<<v1<<std::endl;
     v1.ReverseSign();
     std::cout<<"v1.ReverseSign(); v1 = "<<v1<<std::endl;
-    
+
     //Equal operators
     std::cout<<"v1==v2 ? "<<(v1==v2)<<std::endl;
     std::cout<<"v1!=v2 ? "<<(v1!=v2)<<std::endl;
     std::cout<<"Equal(v1,v2,1e-6) ? "<<Equal(v1,v2,1e-6)<<std::endl;
-    
+
     //Calculating the norm and normalising your vector
     std::cout<<"norm(v3): "<<v3.Norm()<<std::endl;
     v3.Normalize();
     std::cout<<"Normalize(v3)"<<v3<<std::endl;
-        
+
     //Setting your vector to zero
     SetToZero(v1);
     std::cout<<"SetToZero(v1); v1 = "<<v1<<std::endl;
@@ -81,31 +81,31 @@ int main()
                      KDL::Vector(0,-1,0),
                      KDL::Vector(-1,0,0));
     //Creating a rotation matrix out of 9 values, Be careful, these
-    //values can result in an inconsisten rotation matrix if the
+    //values can result in an inconsistent rotation matrix if the
     //resulting rows/columns are not orthogonal/normalized
     KDL::Rotation r3(0,0,-1,1,0,0,0,-1,0);
     //Creating an Identity rotation matrix
     KDL::Rotation r4=KDL::Rotation::Identity();
     //Creating a Rotation matrix from a rotation around X
-    KDL::Rotation r5=KDL::Rotation::RotX(M_PI/3);
+    KDL::Rotation r5=KDL::Rotation::RotX(KDL::PI/3);
     //Creating a Rotation matrix from a rotation around Y
-    KDL::Rotation r6=KDL::Rotation::RotY(M_PI/3);
+    KDL::Rotation r6=KDL::Rotation::RotY(KDL::PI/3);
     //Creating a Rotation matrix from a rotation around Z
-    KDL::Rotation r7=KDL::Rotation::RotZ(M_PI/3);
+    KDL::Rotation r7=KDL::Rotation::RotZ(KDL::PI/3);
     //Creating a Rotation matrix from a rotation around a arbitrary
     //vector, the vector should not be normalised
-    KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),M_PI/4);
+    KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),KDL::PI_4);
     //Creating a Rotation matrix from a rotation around a arbitrary
     //vector, the vector should be normalised
     KDL::Rotation r9=KDL::Rotation::Rot2(KDL::Vector(0.4472,0.5477,0.7071),
-                                         M_PI/4);
+                                         KDL::PI_4);
     //Creating a Rotation matrix from Euler ZYZ rotation angles
     KDL::Rotation r10=KDL::Rotation::EulerZYZ(1.,2.,3.);
     //Creating a Rotation matrix from Euler ZYX rotation angles
     KDL::Rotation r11=KDL::Rotation::EulerZYX(1.,2.,3.);
     //Creating a Rotation matrix from Roll-Pitch-Yaw rotation angles
     KDL::Rotation r12=KDL::Rotation::RPY(1.,2.,3.);
-    
+
     //Printing the rotations:
     std::cout<<"r1: "<<r1<<std::endl;
     std::cout<<"r2: "<<r2<<std::endl;
@@ -119,7 +119,7 @@ int main()
     std::cout<<"r10: "<<r10<<std::endl;
     std::cout<<"r11: "<<r11<<std::endl;
     std::cout<<"r12: "<<r12<<std::endl;
-        
+
     //Getting information out of the rotation matrix:
     //The individual elements
     std::cout<<"r8(1,2): "<<r8(1,2)<<std::endl;
@@ -147,8 +147,4 @@ int main()
     //The underlying unitvector Z
     r8.UnitZ(v1);//or
     std::cout<<"UnitZ of r8:"<<r8.UnitZ()<<std::endl;
-    
-    
-
-        
-}    
+}
diff --git a/orocos_kdl/examples/trajectory_example.cpp b/orocos_kdl/examples/trajectory_example.cpp
index 99908fe6d..da34357e2 100644
--- a/orocos_kdl/examples/trajectory_example.cpp
+++ b/orocos_kdl/examples/trajectory_example.cpp
@@ -19,6 +19,7 @@
 #include <path_roundedcomposite.hpp>
 #include <rotational_interpolation_sa.hpp>
 #include <utilities/error.h>
+#include <utilities/utility.h>
 #include <trajectory_composite.hpp>
 
 int main(int argc,char* argv[]) {
@@ -35,13 +36,13 @@ int main(int argc,char* argv[]) {
 		// When the routines are parallel, no rounding is needed, and no attempt is made
 		// add constructing a rounding arc.
 		// (It is still not possible when the segments are on top of each other)
-		// Note that you can only rotate in a deterministic way over an angle less then M_PI!
-		// With an angle == M_PI, you cannot predict over which side will be rotated.
-		// With an angle > M_PI, the routine will rotate over 2*M_PI-angle.
+		// Note that you can only rotate in a deterministic way over an angle less then PI!
+		// With an angle == PI, you cannot predict over which side will be rotated.
+		// With an angle > PI, the routine will rotate over 2*PI-angle.
 		// If you need to rotate over a larger angle, you need to introduce intermediate points.
 		// So, there is a common use case for using parallel segments.
-		path->Add(Frame(Rotation::RPY(M_PI,0,0), Vector(-1,0,0)));
-		path->Add(Frame(Rotation::RPY(M_PI/2,0,0), Vector(-0.5,0,0)));
+		path->Add(Frame(Rotation::RPY(PI,0,0), Vector(-1,0,0)));
+		path->Add(Frame(Rotation::RPY(PI_2,0,0), Vector(-0.5,0,0)));
 		path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0)));
 		path->Add(Frame(Rotation::RPY(0.7,0.7,0.7), Vector(1,1,1)));
 		path->Add(Frame(Rotation::RPY(0,0.7,0), Vector(1.5,0.3,0)));
@@ -61,8 +62,6 @@ int main(int argc,char* argv[]) {
 		ctraject->Add(traject);
 		ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0))));
 
-
-
 		// use the trajectory
 		double dt=0.1;
 		std::ofstream of("./trajectory.dat");
@@ -113,5 +112,3 @@ int main(int argc,char* argv[]) {
 	}
 
 }
-
-
diff --git a/orocos_kdl/models/puma560.cpp b/orocos_kdl/models/puma560.cpp
index 607442815..d98a5105c 100644
--- a/orocos_kdl/models/puma560.cpp
+++ b/orocos_kdl/models/puma560.cpp
@@ -21,28 +21,29 @@
 
 #include <chain.hpp>
 #include "models.hpp"
+#include <utilities/utility.h>
 
 namespace KDL{
     Chain Puma560(){
         Chain puma560;
         puma560.addSegment(Segment());
         puma560.addSegment(Segment(Joint(Joint::RotZ),
-                                   Frame::DH(0.0,M_PI_2,0.0,0.0),
+                                   Frame::DH(0.0,PI_2,0.0,0.0),
                                    RigidBodyInertia(0,Vector::Zero(),RotationalInertia(0,0.35,0,0,0,0))));
         puma560.addSegment(Segment(Joint(Joint::RotZ),
                                    Frame::DH(0.4318,0.0,0.0,0.0),
                                    RigidBodyInertia(17.4,Vector(-.3638,.006,.2275),RotationalInertia(0.13,0.524,0.539,0,0,0))));
         puma560.addSegment(Segment());
         puma560.addSegment(Segment(Joint(Joint::RotZ),
-                                   Frame::DH(0.0203,-M_PI_2,0.15005,0.0),
+                                   Frame::DH(0.0203,-PI_2,0.15005,0.0),
                                    RigidBodyInertia(4.8,Vector(-.0203,-.0141,.070),RotationalInertia(0.066,0.086,0.0125,0,0,0))));
         puma560.addSegment(Segment(Joint(Joint::RotZ),
-                                   Frame::DH(0.0,M_PI_2,0.4318,0.0),
+                                   Frame::DH(0.0,PI_2,0.4318,0.0),
                                    RigidBodyInertia(0.82,Vector(0,.019,0),RotationalInertia(1.8e-3,1.3e-3,1.8e-3,0,0,0))));
         puma560.addSegment(Segment());
         puma560.addSegment(Segment());
         puma560.addSegment(Segment(Joint(Joint::RotZ),
-                                   Frame::DH(0.0,-M_PI_2,0.0,0.0),
+                                   Frame::DH(0.0,-PI_2,0.0,0.0),
                                    RigidBodyInertia(0.34,Vector::Zero(),RotationalInertia(.3e-3,.4e-3,.3e-3,0,0,0))));
         puma560.addSegment(Segment(Joint(Joint::RotZ),
                                    Frame::DH(0.0,0.0,0.0,0.0),
diff --git a/orocos_kdl/package.xml b/orocos_kdl/package.xml
index cc8cfbf0b..f858bfd21 100644
--- a/orocos_kdl/package.xml
+++ b/orocos_kdl/package.xml
@@ -1,9 +1,10 @@
-<package>
+<?xml version="1.0"?>
+<package format="3">
   <name>orocos_kdl</name>
   <version>1.4.0</version>
   <description>
     This package contains a recent version of the Kinematics and Dynamics
-    Library (KDL), distributed by the Orocos Project. 
+    Library (KDL), distributed by the Orocos Project.
   </description>
   <maintainer email="ruben@intermodalics.eu">Ruben Smits</maintainer>
   <url>http://wiki.ros.org/orocos_kdl</url>
@@ -12,11 +13,14 @@
   <buildtool_depend>cmake</buildtool_depend>
   <build_depend>eigen</build_depend>
 
-  <run_depend>catkin</run_depend>
-  <run_depend>eigen</run_depend>
-  <run_depend>pkg-config</run_depend>
+  <exec_depend>catkin</exec_depend>
+  <exec_depend>eigen</exec_depend>
+  <exec_depend>pkg-config</exec_depend>
 
   <test_depend>cppunit</test_depend>
+
+  <doc_depend>doxygen</doc_depend>
+
   <export>
     <build_type>cmake</build_type>
   </export>
diff --git a/orocos_kdl/src/CMakeLists.txt b/orocos_kdl/src/CMakeLists.txt
index 079ca8ac1..7557069bb 100644
--- a/orocos_kdl/src/CMakeLists.txt
+++ b/orocos_kdl/src/CMakeLists.txt
@@ -3,52 +3,6 @@ FILE( GLOB KDL_HPPS [^.]*.hpp [^.]*.inl)
 
 FILE( GLOB UTIL_HPPS utilities/[^.]*.h utilities/[^.]*.hpp)
 
-INCLUDE(CheckCXXSourceCompiles)
-SET(CMAKE_REQUIRED_FLAGS)
-CHECK_CXX_SOURCE_COMPILES("
-    #include <string>
-    #include <map>
-    #include <vector.hpp>
-
-    class TreeElement;
-    typedef std::map<std::string, TreeElement> SegmentMap;
-
-    class TreeElement
-    {
-        TreeElement(const std::string& name): number(0) {}
-
-    public:
-        int number;
-        SegmentMap::const_iterator parent;
-        std::vector<SegmentMap::const_iterator> children;
-
-        static TreeElement Root(std::string& name)
-        {
-            return TreeElement(name);
-        }
-    };
-
-    int main()
-    {
-        return 0;
-    }
-    "
-    HAVE_STL_CONTAINER_INCOMPLETE_TYPES)
-
-if(HAVE_STL_CONTAINER_INCOMPLETE_TYPES)
-    SET(KDL_USE_NEW_TREE_INTERFACE_DEFAULT Off)
-ELSE(HAVE_STL_CONTAINER_INCOMPLETE_TYPES)
-    SET(KDL_USE_NEW_TREE_INTERFACE_DEFAULT On)
-ENDIF(HAVE_STL_CONTAINER_INCOMPLETE_TYPES)
-
-SET(KDL_USE_NEW_TREE_INTERFACE ${KDL_USE_NEW_TREE_INTERFACE_DEFAULT} CACHE BOOL "Use the new KDL Tree interface")
-
-#Sanity check, inform the user
-IF(NOT HAVE_STL_CONTAINER_INCOMPLETE_TYPES AND NOT KDL_USE_NEW_TREE_INTERFACE)
-    MESSAGE(WARNING "You have chosen to use the current Tree Interface, but your platform doesn't support containers of "
-        "incomplete types, this configuration is likely invalid")
-ENDIF()
-
 #In Windows (Visual Studio) it is necessary to specify the postfix
 #of the debug library name and no symbols are exported by kdl, 
 #so it is necessary to compile it as a static library
diff --git a/orocos_kdl/src/chain.cpp b/orocos_kdl/src/chain.cpp
index 2b6320b27..016dc4b90 100644
--- a/orocos_kdl/src/chain.cpp
+++ b/orocos_kdl/src/chain.cpp
@@ -1,8 +1,8 @@
-// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 // Version: 1.0
-// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 // URL: http://www.orocos.org/kdl
 
 // This library is free software; you can redistribute it and/or
@@ -55,7 +55,7 @@ namespace KDL {
     {
         segments.push_back(segment);
         nrOfSegments++;
-        if(segment.getJoint().getType()!=Joint::None)
+        if(segment.getJoint().getType()!=Joint::Fixed)
             nrOfJoints++;
     }
 
diff --git a/orocos_kdl/src/chaindynparam.cpp b/orocos_kdl/src/chaindynparam.cpp
index 3773c3b0f..814a94ef9 100644
--- a/orocos_kdl/src/chaindynparam.cpp
+++ b/orocos_kdl/src/chaindynparam.cpp
@@ -1,8 +1,9 @@
-// Copyright  (C)  2009  Dominick Vanthienen <dominick dot vanthienen at mech dot kuleuven dot be>
+// Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 // Version: 1.0
 // Author: Dominick Vanthienen <dominick dot vanthienen at mech dot kuleuven dot be>
-// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 // URL: http://www.orocos.org/kdl
 
 // This library is free software; you can redistribute it and/or
@@ -65,13 +66,13 @@ namespace KDL {
             return (error = E_SIZE_MISMATCH);
         unsigned int k=0;
 	double q_;
-	
+
 	//Sweep from root to leaf
         for(unsigned int i=0;i<ns;i++)
 	{
 	  //Collect RigidBodyInertia
           Ic[i]=chain.getSegment(i).getInertia();
-          if(chain.getSegment(i).getJoint().getType()!=Joint::None)
+          if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed)
 	  {
 	      q_=q(k);
 	      k++;
@@ -81,24 +82,25 @@ namespace KDL {
 	    q_=0.0;
 	  }
 	  X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the frame for transformations from the parent to the current coord frame
-	  S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));  
+	  S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));
         }
 	//Sweep from leaf to root
         int j,l;
 	k=nj-1; //reset k
         for(int i=ns-1;i>=0;i--)
 	{
-	  
+
 	  if(i!=0)
 	    {
 	      //assumption that previous segment is parent
 	      Ic[i-1]=Ic[i-1]+X[i]*Ic[i];
-	    } 
+	    }
 
 	  F=Ic[i]*S[i];
-	  if(chain.getSegment(i).getJoint().getType()!=Joint::None)
+      if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed)
 	  {
-	      H(k,k)=dot(S[i],F);
+          H(k,k)=dot(S[i],F);
+          H(k,k)+=chain.getSegment(i).getJoint().getInertia();  // add joint inertia
 	      j=k; //countervariable for the joints
 	      l=i; //countervariable for the segments
 	      while(l!=0) //go from leaf to root starting at i
@@ -106,14 +108,14 @@ namespace KDL {
 		  //assumption that previous segment is parent
 		  F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l]
 		  l--; //go down a segment
-		  
-		  if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint
-		  {    
+
+          if(chain.getSegment(l).getJoint().getType()!=Joint::Fixed) //if the joint connected to segment is not a fixed joint
+		  {
 		    j--;
-		    H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment 
+		    H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment
 		    H(j,k)=H(k,j);
 		  }
-		} 
+		}
 	      k--; //this if-loop should be repeated nj times (k=nj-1 to k=0)
 	  }
 
@@ -127,10 +129,10 @@ namespace KDL {
     //make a null matrix with the size of q_dotdot and a null wrench
 	SetToZero(jntarraynull);
 
-	
+
 	//the calculation of coriolis matrix C
 	return chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis);
-	
+
     }
 
     //calculate gravity matrix G
@@ -138,7 +140,7 @@ namespace KDL {
     {
 
 	//make a null matrix with the size of q_dotdot and a null wrench
-	
+
 	SetToZero(jntarraynull);
 	//the calculation of coriolis matrix C
 	return chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity);
diff --git a/orocos_kdl/src/chaindynparam.hpp b/orocos_kdl/src/chaindynparam.hpp
index 964c213ae..2641ab1af 100644
--- a/orocos_kdl/src/chaindynparam.hpp
+++ b/orocos_kdl/src/chaindynparam.hpp
@@ -44,7 +44,7 @@ namespace KDL {
      * (expressed in the segments reference frame) and the dynamical
      * parameters of the segments.
      */
-    class ChainDynParam : SolverI
+    class ChainDynParam : public SolverI
     {
     public:
         ChainDynParam(const Chain& chain, Vector _grav);
diff --git a/orocos_kdl/src/chainfdsolver.hpp b/orocos_kdl/src/chainfdsolver.hpp
new file mode 100644
index 000000000..b542bb794
--- /dev/null
+++ b/orocos_kdl/src/chainfdsolver.hpp
@@ -0,0 +1,61 @@
+// Copyright  (C)  2018  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits, Craig Carignan
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_CHAIN_FDSOLVER_HPP
+#define KDL_CHAIN_FDSOLVER_HPP
+
+#include "chain.hpp"
+#include "frames.hpp"
+#include "jntarray.hpp"
+#include "solveri.hpp"
+
+namespace KDL
+{
+
+    typedef std::vector<Wrench> Wrenches;
+
+	/**
+	 * \brief This <strong>abstract</strong> class encapsulates the inverse
+	 * dynamics solver for a KDL::Chain.
+	 *
+	 */
+	class ChainFdSolver : public KDL::SolverI
+	{
+		public:
+			/**
+			 * Calculate forward dynamics from joint positions, joint velocities, joint torques/forces,
+			 * and externally applied forces/torques to joint accelerations.
+			 *
+			 * @param q input joint positions
+			 * @param q_dot input joint velocities
+             * @param torque input joint torques
+             * @param f_ext external forces
+			 *
+             * @param q_dotdot output joint accelerations
+			 *
+			 * @return if < 0 something went wrong
+			 */
+        virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext,JntArray &q_dotdot)=0;
+	};
+
+}
+
+#endif
diff --git a/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp b/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp
new file mode 100644
index 000000000..3a1a0c3bf
--- /dev/null
+++ b/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp
@@ -0,0 +1,138 @@
+// Copyright  (C)  2018  Craig Carignan <craigc at ssl dot umd dot edu>
+
+// Version: 1.0
+// Author: Craig Carignan <craigc at ssl dot umd dot edu>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "chainfdsolver_recursive_newton_euler.hpp"
+#include "utilities/ldl_solver_eigen.hpp"
+#include "frames_io.hpp"
+#include "kinfam_io.hpp"
+
+namespace KDL{
+
+    ChainFdSolver_RNE::ChainFdSolver_RNE(const Chain& _chain, Vector _grav):
+        chain(_chain),
+        DynSolver(chain, _grav),
+        IdSolver(chain, _grav),
+        nj(chain.getNrOfJoints()),
+        ns(chain.getNrOfSegments()),
+        H(nj),
+        Tzeroacc(nj),
+        H_eig(nj,nj),
+        Tzeroacc_eig(nj),
+        L_eig(nj,nj),
+        D_eig(nj),
+        r_eig(nj),
+        acc_eig(nj)
+    {
+    }
+
+    void ChainFdSolver_RNE::updateInternalDataStructures() {
+        nj = chain.getNrOfJoints();
+        ns = chain.getNrOfSegments();
+    }
+
+    int ChainFdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext, JntArray &q_dotdot)
+    {
+        if(nj != chain.getNrOfJoints() || ns != chain.getNrOfSegments())
+            return (error = E_NOT_UP_TO_DATE);
+
+        //Check sizes of function parameters
+        if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns)
+            return (error = E_SIZE_MISMATCH);
+
+        // Inverse Dynamics:
+        //   T = H * qdd + Tcor + Tgrav - J^T * Fext
+        // Forward Dynamics:
+        //   1. Call ChainDynParam->JntToMass to get H
+        //   2. Call ChainIdSolver_RNE->CartToJnt with qdd=0 to get Tcor+Tgrav-J^T*Fext
+        //   3. Calculate qdd = H^-1 * T where T are applied joint torques minus non-inertial internal torques
+
+        // Calculate Joint Space Inertia Matrix
+        error = DynSolver.JntToMass(q, H);
+        if (error < 0)
+            return (error);
+
+        // Calculate non-inertial internal torques by inputting zero joint acceleration to ID
+        for(unsigned int i=0;i<nj;i++){
+            q_dotdot(i) = 0.;
+        }
+        error = IdSolver.CartToJnt(q, q_dot, q_dotdot, f_ext, Tzeroacc);
+        if (error < 0)
+            return (error);
+
+        // Calculate acceleration using inverse symmetric matrix times vector
+        for(unsigned int i=0;i<nj;i++){
+            Tzeroacc_eig(i) =  (torques(i)-Tzeroacc(i));
+            for(unsigned int j=0;j<nj;j++){
+                H_eig(i,j) =  H(i,j);
+            }
+        }
+        ldl_solver_eigen(H_eig, Tzeroacc_eig, L_eig, D_eig, r_eig, acc_eig);
+        for(unsigned int i=0;i<nj;i++){
+            q_dotdot(i) = acc_eig(i);
+        }
+
+        return (error = E_NOERROR);
+    }
+
+    void ChainFdSolver_RNE::RK4Integrator(unsigned int& nj, const double& t, double& dt, KDL::JntArray& q, KDL::JntArray& q_dot,
+                                          KDL::JntArray& torques, KDL::Wrenches& f_ext, KDL::ChainFdSolver_RNE& fdsolver,
+                                          KDL::JntArray& q_dotdot, KDL::JntArray& dq, KDL::JntArray& dq_dot,
+                                          KDL::JntArray& q_temp, KDL::JntArray& q_dot_temp)
+    {
+        fdsolver.CartToJnt(q, q_dot, torques, f_ext, q_dotdot);
+        for (unsigned int i=0; i<nj; ++i)
+        {
+            q_temp(i) = q(i) + q_dot(i)*dt/2.0;
+            q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt/2.0;
+            dq(i) = q_dot(i);
+            dq_dot(i) = q_dotdot(i);
+        }
+        fdsolver.CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
+        for (unsigned int i=0; i<nj; ++i)
+        {
+            q_temp(i) = q(i) + q_dot_temp(i)*dt/2.0;
+            q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt/2.0;
+            dq(i) += 2.0*q_dot_temp(i);
+            dq_dot(i) += 2.0*q_dotdot(i);
+        }
+        fdsolver.CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
+        for (unsigned int i=0; i<nj; ++i)
+        {
+            q_temp(i) = q(i) + q_dot_temp(i)*dt;
+            q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt;
+            dq(i) += 2.0*q_dot_temp(i);
+            dq_dot(i) += 2.0*q_dotdot(i);
+        }
+        fdsolver.CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
+        for (unsigned int i=0; i<nj; ++i)
+        {
+            dq(i) = (dq(i)+q_dot_temp(i))*dt/6.0;
+            dq_dot(i) = (dq_dot(i)+q_dotdot(i))*dt/6.0;
+        }
+        for (unsigned int i=0; i<nj; ++i)
+        {
+            q(i) += dq(i);
+            q_dot(i) += dq_dot(i);
+        }
+        return;
+    }
+
+}//namespace
diff --git a/orocos_kdl/src/chainfdsolver_recursive_newton_euler.hpp b/orocos_kdl/src/chainfdsolver_recursive_newton_euler.hpp
new file mode 100644
index 000000000..cc671f411
--- /dev/null
+++ b/orocos_kdl/src/chainfdsolver_recursive_newton_euler.hpp
@@ -0,0 +1,111 @@
+// Copyright  (C)  2018  Craig Carignan <craigc at ssl dot umd dot edu>
+
+// Version: 1.0
+// Author: Craig Carignan  <craigc at ssl dot umd dot edu>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_CHAIN_FDSOLVER_RECURSIVE_NEWTON_EULER_HPP
+#define KDL_CHAIN_FDSOLVER_RECURSIVE_NEWTON_EULER_HPP
+
+#include "chainfdsolver.hpp"
+#include "chainidsolver_recursive_newton_euler.hpp"
+#include "chaindynparam.hpp"
+
+namespace KDL{
+    /**
+     * \brief Recursive newton euler forward dynamics solver
+     *
+     * The algorithm implementation is based on the book "Rigid Body
+     * Dynamics Algorithms" of Roy Featherstone, 2008
+     * (ISBN:978-0-387-74314-1) See Chapter 6 for basic algorithm.
+     *
+     * It calculates the accelerations for the joints (qdotdot), given the
+     * position and velocity of the joints (q,qdot,qdotdot), external forces
+     * on the segments (expressed in the segments reference frame),
+     * and the dynamical parameters of the segments.
+     */
+    class ChainFdSolver_RNE : public ChainFdSolver{
+    public:
+        /**
+         * Constructor for the solver, it will allocate all the necessary memory
+         * \param chain The kinematic chain to calculate the forward dynamics for, an internal copy will be made.
+         * \param grav The gravity vector to use during the calculation.
+         */
+        ChainFdSolver_RNE(const Chain& chain, Vector grav);
+        ~ChainFdSolver_RNE(){};
+
+        /**
+         * Function to calculate from Cartesian forces to joint torques.
+         * Input parameters;
+         * \param q The current joint positions
+         * \param q_dot The current joint velocities
+         * \param torques The current joint torques (applied by controller)
+         * \param f_ext The external forces (no gravity) on the segments
+         * Output parameters:
+         * \param q_dotdot The resulting joint accelerations
+         */
+        int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext, JntArray &q_dotdot);
+
+        /// @copydoc KDL::SolverI::updateInternalDataStructures
+        virtual void updateInternalDataStructures();
+
+        /**
+         * Function to integrate the joint accelerations resulting from the forward dynamics solver.
+         * Input parameters;
+         * \param nj The number of joints
+         * \param t The current time
+         * \param dt The integration period
+         * \param q The current joint positions
+         * \param q_dot The current joint velocities
+         * \param torques The current joint torques (applied by controller)
+         * \param f_ext The external forces (no gravity) on the segments
+         * \param fdsolver The forward dynamics solver
+         * Output parameters:
+         * \param t The updated time
+         * \param q The updated joint positions
+         * \param q_dot The updated joint velocities
+         * \param q_dotdot The current joint accelerations
+         * \param dq The joint position increment
+         * \param dq_dot The joint velocity increment
+         * Temporary parameters:
+         * \param qtemp Intermediate joint positions
+         * \param qdtemp Intermediate joint velocities
+         */
+        void RK4Integrator(unsigned int& nj, const double& t, double& dt, KDL::JntArray& q, KDL::JntArray& q_dot,
+                           KDL::JntArray& torques, KDL::Wrenches& f_ext, KDL::ChainFdSolver_RNE& fdsolver,
+                           KDL::JntArray& q_dotdot, KDL::JntArray& dq, KDL::JntArray& dq_dot,
+                           KDL::JntArray& q_temp, KDL::JntArray& q_dot_temp);
+
+    private:
+        const Chain& chain;
+        ChainDynParam DynSolver;
+        ChainIdSolver_RNE IdSolver;
+        unsigned int nj;
+        unsigned int ns;
+        JntSpaceInertiaMatrix H;
+        JntArray Tzeroacc;
+        Eigen::MatrixXd H_eig;
+        Eigen::VectorXd Tzeroacc_eig;
+        Eigen::MatrixXd L_eig;
+        Eigen::VectorXd D_eig;
+        Eigen::VectorXd r_eig;
+        Eigen::VectorXd acc_eig;
+    };
+}
+
+#endif
diff --git a/orocos_kdl/src/chainfksolver.hpp b/orocos_kdl/src/chainfksolver.hpp
index f82c20d13..5b1c40624 100644
--- a/orocos_kdl/src/chainfksolver.hpp
+++ b/orocos_kdl/src/chainfksolver.hpp
@@ -108,7 +108,7 @@ namespace KDL {
     class ChainFkSolverAcc : public KDL::SolverI {
     public:
         /**
-         * Calculate forward position, velocity and accelaration
+         * Calculate forward position, velocity and acceleration
          * kinematics, from joint coordinates to cartesian coordinates
          *
          * @param q_in input joint coordinates (position, velocity and
@@ -120,7 +120,7 @@ namespace KDL {
          */
     virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0;
         /**
-         * Calculate forward position, velocity and accelaration
+         * Calculate forward position, velocity and acceleration
          * kinematics, from joint coordinates to cartesian coordinates
          *
          * @param q_in input joint coordinates (position, velocity and
diff --git a/orocos_kdl/src/chainfksolverpos_recursive.cpp b/orocos_kdl/src/chainfksolverpos_recursive.cpp
index 99d7d4793..c7bc8bbd3 100644
--- a/orocos_kdl/src/chainfksolverpos_recursive.cpp
+++ b/orocos_kdl/src/chainfksolverpos_recursive.cpp
@@ -1,9 +1,9 @@
-// Copyright  (C)  2007  Francois Cauwe <francois at cauwe dot org>
-// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 // Version: 1.0
-// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Author: Francois Cauwe <francois at cauwe dot org>
+// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 // URL: http://www.orocos.org/kdl
 
 // This library is free software; you can redistribute it and/or
@@ -46,7 +46,7 @@ namespace KDL {
         else{
             int j=0;
             for(unsigned int i=0;i<segmentNr;i++){
-                if(chain.getSegment(i).getJoint().getType()!=Joint::None){
+                if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) {
                     p_out = p_out*chain.getSegment(i).pose(q_in(j));
                     j++;
                 }else{
@@ -74,14 +74,14 @@ namespace KDL {
         else{
             int j=0;
             // Initialization
-            if(chain.getSegment(0).getJoint().getType()!=Joint::None){
+            if(chain.getSegment(0).getJoint().getType()!=Joint::Fixed) {
                 p_out[0] = chain.getSegment(0).pose(q_in(j));
                 j++;
             }else
                 p_out[0] = chain.getSegment(0).pose(0.0);
-            
+
             for(unsigned int i=1;i<segmentNr;i++){
-                if(chain.getSegment(i).getJoint().getType()!=Joint::None){
+                if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) {
                     p_out[i] = p_out[i-1]*chain.getSegment(i).pose(q_in(j));
                     j++;
                 }else{
diff --git a/orocos_kdl/src/chainfksolvervel_recursive.cpp b/orocos_kdl/src/chainfksolvervel_recursive.cpp
index 428299a09..d9376b1bb 100644
--- a/orocos_kdl/src/chainfksolvervel_recursive.cpp
+++ b/orocos_kdl/src/chainfksolvervel_recursive.cpp
@@ -1,9 +1,9 @@
-// Copyright  (C)  2007  Francois Cauwe <francois at cauwe dot org>
-// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 // Version: 1.0
-// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Author: Francois Cauwe <francois at cauwe dot org>
+// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 // URL: http://www.orocos.org/kdl
 
 // This library is free software; you can redistribute it and/or
@@ -51,7 +51,7 @@ namespace KDL
             int j=0;
             for (unsigned int i=0;i<segmentNr;i++) {
                 //Calculate new Frame_base_ee
-                if(chain.getSegment(i).getJoint().getType()!=Joint::None){
+                if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) {
                     out=out*FrameVel(chain.getSegment(i).pose(in.q(j)),
                                      chain.getSegment(i).twist(in.q(j),in.qdot(j)));
                     j++;//Only increase jointnr if the segment has a joint
@@ -85,17 +85,17 @@ namespace KDL
         else{
             int j=0;
             // Initialization
-            if(chain.getSegment(0).getJoint().getType()!=Joint::None){
+            if(chain.getSegment(0).getJoint().getType()!=Joint::Fixed) {
                 out[0] = FrameVel(chain.getSegment(0).pose(in.q(0)),
                                      chain.getSegment(0).twist(in.q(0),in.qdot(0)));
                 j++;
             }else
                 out[0] = FrameVel(chain.getSegment(0).pose(0.0),
                                      chain.getSegment(0).twist(0.0,0.0));
-            
+
             for (unsigned int i=1;i<segmentNr;i++) {
                 //Calculate new Frame_base_ee
-                if(chain.getSegment(i).getJoint().getType()!=Joint::None){
+                if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) {
                     out[i]=out[i-1]*FrameVel(chain.getSegment(i).pose(in.q(j)),
                                     chain.getSegment(i).twist(in.q(j),in.qdot(j)));
                     j++;//Only increase jointnr if the segment has a joint
diff --git a/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp b/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp
index 9a63fb5a1..c7fbd7d1b 100644
--- a/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp
+++ b/orocos_kdl/src/chainidsolver_recursive_newton_euler.cpp
@@ -1,8 +1,8 @@
-// Copyright  (C)  2009  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 // Version: 1.0
-// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 // URL: http://www.orocos.org/kdl
 
 // This library is free software; you can redistribute it and/or
@@ -23,7 +23,7 @@
 #include "frames_io.hpp"
 
 namespace KDL{
-    
+
     ChainIdSolver_RNE::ChainIdSolver_RNE(const Chain& chain_,Vector grav):
         chain(chain_),nj(chain.getNrOfJoints()),ns(chain.getNrOfSegments()),
         X(ns),S(ns),v(ns),a(ns),f(ns)
@@ -54,17 +54,17 @@ namespace KDL{
         //Sweep from root to leaf
         for(unsigned int i=0;i<ns;i++){
             double q_,qdot_,qdotdot_;
-            if(chain.getSegment(i).getJoint().getType()!=Joint::None){
+            if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) {
                 q_=q(j);
                 qdot_=q_dot(j);
                 qdotdot_=q_dotdot(j);
                 j++;
             }else
                 q_=qdot_=qdotdot_=0.0;
-            
+
             //Calculate segment properties: X,S,vj,cj
-            X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the 
-                                                //frame for transformations from 
+            X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the
+                                                //frame for transformations from
                                                 //the parent to the current coord frame
             //Transform velocity and unit velocity to segment frame
             Twist vj=X[i].M.Inverse(chain.getSegment(i).twist(q_,qdot_));
@@ -87,8 +87,11 @@ namespace KDL{
         //Sweep from leaf to root
         j=nj-1;
         for(int i=ns-1;i>=0;i--){
-            if(chain.getSegment(i).getJoint().getType()!=Joint::None)
-                torques(j--)=dot(S[i],f[i]);
+            if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) {
+                torques(j)=dot(S[i],f[i]);
+                torques(j)+=chain.getSegment(i).getJoint().getInertia()*q_dotdot(j);  // add torque from joint inertia
+                --j;
+            }
             if(i!=0)
                 f[i-1]=f[i-1]+X[i]*f[i];
         }
diff --git a/orocos_kdl/src/chainidsolver_vereshchagin.cpp b/orocos_kdl/src/chainidsolver_vereshchagin.cpp
index 880503a53..5b5e95a49 100644
--- a/orocos_kdl/src/chainidsolver_vereshchagin.cpp
+++ b/orocos_kdl/src/chainidsolver_vereshchagin.cpp
@@ -1,8 +1,10 @@
-// Copyright  (C)  2009, 2011
+// Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 // Version: 1.0
-// Author: Ruben Smits, Herman Bruyninckx, Azamat Shakhimardanov
-// Maintainer: Ruben Smits, Azamat Shakhimardanov
+// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+// Author: Herman Bruyninckx
+// Author: Azamat Shakhimardanov
+// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 // URL: http://www.orocos.org/kdl
 
 // This library is free software; you can redistribute it and/or
@@ -124,7 +126,7 @@ void ChainIdSolver_Vereshchagin::initial_upwards_sweep(const JntArray &q, const
         //external forces are taken into account through s.U.
         Wrench FextLocal = F_total.M.Inverse() * f_ext[i];
         s.U = s.v * (s.H * s.v) - FextLocal; //f_ext[i];
-        if (segment.getJoint().getType() != Joint::None)
+        if (segment.getJoint().getType() != Joint::Fixed)
             j++;
     }
 
@@ -240,7 +242,7 @@ void ChainIdSolver_Vereshchagin::downwards_sweep(const Jacobian& alfa, const Jnt
             vZ << Vector3d::Map(s.Z.rot.data), Vector3d::Map(s.Z.vel.data);
             s.EZ.noalias() = s.E.transpose() * vZ;
 
-            if (chain.getSegment(i - 1).getJoint().getType() != Joint::None)
+            if (chain.getSegment(i - 1).getJoint().getType() != Joint::Fixed)
                 j--;
         }
     }
@@ -325,7 +327,7 @@ void ChainIdSolver_Vereshchagin::final_upwards_sweep(JntArray &q_dotdot, JntArra
         // nullspace forces.
         q_dotdot(j) = (s.nullspaceAccComp + parentAccComp + s.constAccComp);
         s.acc = s.F.Inverse(a_p + s.Z * q_dotdot(j) + s.C);//returns acceleration in link distal tip coordinates. For use needs to be transformed
-        if (chain.getSegment(i - 1).getJoint().getType() != Joint::None)
+        if (chain.getSegment(i - 1).getJoint().getType() != Joint::Fixed)
             j++;
     }
 }
diff --git a/orocos_kdl/src/chainiksolverpos_lma.cpp b/orocos_kdl/src/chainiksolverpos_lma.cpp
index 8b11ae573..a9eb134bf 100644
--- a/orocos_kdl/src/chainiksolverpos_lma.cpp
+++ b/orocos_kdl/src/chainiksolverpos_lma.cpp
@@ -5,7 +5,7 @@
 
 /**************************************************************************
     begin                : May 2012
-    copyright            : (C) 2012 Erwin Aertbelien
+    copyright            : (C) 2020 Erwin Aertbelien
     email                : firstname.lastname@mech.kuleuven.ac.be
 
  History (only major changes)( AUTHOR-Description ) :
@@ -144,7 +144,7 @@ void ChainIkSolverPos_LMA::compute_fwdpos(const VectorXq& q) {
 	T_base_head = Frame::Identity(); // frame w.r.t. base of head
 	for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
 		const Segment& segment = chain.getSegment(i);
-		if (segment.getJoint().getType()!=Joint::None) {
+        if (segment.getJoint().getType()!=Joint::Fixed) {
 			T_base_jointroot[jointndx] = T_base_head;
 			T_base_head = T_base_head * segment.pose(q(jointndx));
 			T_base_jointtip[jointndx] = T_base_head;
@@ -160,7 +160,7 @@ void ChainIkSolverPos_LMA::compute_jacobian(const VectorXq& q) {
 	unsigned int jointndx=0;
 	for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
 		const Segment& segment = chain.getSegment(i);
-		if (segment.getJoint().getType()!=Joint::None) {
+        if (segment.getJoint().getType()!=Joint::Fixed) {
 			// compute twist of the end effector motion caused by joint [jointndx]; expressed in base frame, with vel. ref. point equal to the end effector
 			KDL::Twist t = ( T_base_jointroot[jointndx].M * segment.twist(q(jointndx),1.0) ).RefPoint( T_base_head.p - T_base_jointtip[jointndx].p);
 			jac(0,jointndx)=t[0];
@@ -187,7 +187,7 @@ void ChainIkSolverPos_LMA::display_jac(const KDL::JntArray& jval) {
 int ChainIkSolverPos_LMA::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out) {
   if (nj != chain.getNrOfJoints())
     return (error = E_NOT_UP_TO_DATE);
-  
+
   if (nj != q_init.rows() || nj != q_out.rows())
     return (error = E_SIZE_MISMATCH);
 
@@ -321,4 +321,4 @@ const char* ChainIkSolverPos_LMA::strError(const int error) const
         else return SolverI::strError(error);
     }
 
-};//namespace KDL
+}//namespace KDL
diff --git a/orocos_kdl/src/chainiksolverpos_lma.hpp b/orocos_kdl/src/chainiksolverpos_lma.hpp
index 19133aa15..c3410cf6b 100644
--- a/orocos_kdl/src/chainiksolverpos_lma.hpp
+++ b/orocos_kdl/src/chainiksolverpos_lma.hpp
@@ -246,7 +246,7 @@ class ChainIkSolverPos_LMA : public KDL::ChainIkSolverPos
 
 
 
-}; // namespace KDL
+} // namespace KDL
 
 
 
diff --git a/orocos_kdl/src/chainiksolvervel_pinv.cpp b/orocos_kdl/src/chainiksolvervel_pinv.cpp
index 8b5d70ec6..4ecc0cc49 100644
--- a/orocos_kdl/src/chainiksolvervel_pinv.cpp
+++ b/orocos_kdl/src/chainiksolvervel_pinv.cpp
@@ -25,8 +25,8 @@ namespace KDL
 {
     ChainIkSolverVel_pinv::ChainIkSolverVel_pinv(const Chain& _chain,double _eps,int _maxiter):
         chain(_chain),
-        nj(chain.getNrOfJoints()),
         jnt2jac(chain),
+        nj(chain.getNrOfJoints()),
         jac(nj),
         svd(jac),
         U(6,JntArray(nj)),
diff --git a/orocos_kdl/src/chainiksolvervel_pinv.hpp b/orocos_kdl/src/chainiksolvervel_pinv.hpp
index 8ddc32349..b31f2dd4e 100644
--- a/orocos_kdl/src/chainiksolvervel_pinv.hpp
+++ b/orocos_kdl/src/chainiksolvervel_pinv.hpp
@@ -77,7 +77,7 @@ namespace KDL
          * not (yet) implemented.
          *
          */
-        virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;};
+        virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return -1;};
 
         /**
          * Retrieve the number of singular values of the jacobian that are < eps;
diff --git a/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp b/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp
index e5ab28408..0d2e6a98d 100644
--- a/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp
+++ b/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp
@@ -38,7 +38,6 @@ namespace KDL
         B(m,n),
         S(n),
         tempi(m),
-        tempj(m),
         UY(VectorXd::Zero(6)),
         SUY(VectorXd::Zero(nj)),
         qdot_eigen(nj),
@@ -59,7 +58,6 @@ namespace KDL
         B.conservativeResize(m,n);
         S.conservativeResize(n);
         tempi.conservativeResize(m);
-        tempj.conservativeResize(n);
         SUY.conservativeResizeLike(VectorXd::Zero(nj));
         qdot_eigen.conservativeResize(nj);
     }
@@ -93,8 +91,7 @@ namespace KDL
                 else
                     jac_eigen(i,j)=jac(i,j);
         }
-        int ret = svd_eigen_Macie(jac_eigen,U,S,V,B,tempi,1e-15,toggle);
-        //std::cout<<"# sweeps: "<<ret<<std::endl;
+        svd_eigen_Macie(jac_eigen,U,S,V,B,tempi,1e-15,toggle);
 
         if(transpose)
             UY.noalias() = V.transpose() * v_in_eigen;
diff --git a/orocos_kdl/src/chainiksolvervel_pinv_givens.hpp b/orocos_kdl/src/chainiksolvervel_pinv_givens.hpp
index be27450be..cbe3f15cd 100644
--- a/orocos_kdl/src/chainiksolvervel_pinv_givens.hpp
+++ b/orocos_kdl/src/chainiksolvervel_pinv_givens.hpp
@@ -30,10 +30,6 @@ namespace KDL
          *
          * @param chain the chain to calculate the inverse velocity
          * kinematics for
-         * @param eps if a singular value is below this value, its
-         * inverse is set to zero, default: 0.00001
-         * @param maxiter maximum iterations for the svd calculation,
-         * default: 150
          *
          */
         explicit ChainIkSolverVel_pinv_givens(const Chain& chain);
@@ -44,7 +40,7 @@ namespace KDL
          * not (yet) implemented.
          *
          */
-        virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return (error = E_NOT_IMPLEMENTED);};
+        virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return (error = E_NOT_IMPLEMENTED);};
 
         /// @copydoc KDL::SolverI::updateInternalDataStructures
         virtual void updateInternalDataStructures();
diff --git a/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp b/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp
index 0690bd4a2..0cbe1ff69 100644
--- a/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp
+++ b/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp
@@ -69,7 +69,7 @@ namespace KDL
          * not (yet) implemented.
          *
          */
-        virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;};
+        virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return -1;};
 
         /**
          * Request the joint weights for optimization criterion
@@ -121,7 +121,7 @@ namespace KDL
         virtual int setOptPos(const JntArray &opt_pos);
 
         /**
-         *Set null psace velocity gain
+         *Set null space velocity gain
          *
          *@param alpha NUllspace velocity cgain
          *
diff --git a/orocos_kdl/src/chainiksolvervel_wdls.cpp b/orocos_kdl/src/chainiksolvervel_wdls.cpp
index 33ae9ff13..f46eb1c12 100644
--- a/orocos_kdl/src/chainiksolvervel_wdls.cpp
+++ b/orocos_kdl/src/chainiksolvervel_wdls.cpp
@@ -122,10 +122,10 @@ namespace KDL
             return (error = E_SIZE_MISMATCH);
         error = jnt2jac.JntToJac(q_in,jac);
         if ( error < E_NOERROR) return error;
-        
+
         double sum;
         unsigned int i,j;
-        
+
         // Initialize (internal) return values
         nrZeroSigmas = 0 ;
         sigmaMin = 0.;
@@ -137,11 +137,11 @@ namespace KDL
                 tmp_jac(i,j) = jac(i,j);
         }
         */
-        
+
         // Create the Weighted jacobian
         tmp_jac_weight1 = jac.data.lazyProduct(weight_js);
         tmp_jac_weight2 = weight_ts.lazyProduct(tmp_jac_weight1);
-   
+
         // Compute the SVD of the weighted jacobian
         svdResult = svd_eigen_HH(tmp_jac_weight2,U,S,V,tmp,maxiter);
         if (0 != svdResult)
@@ -176,22 +176,22 @@ namespace KDL
             // Note:  singular values are always positive so sigmaMin >=0
             if ( sigmaMin < eps )
             {
-			    lambda_scaled = sqrt(1.0-(sigmaMin/eps)*(sigmaMin/eps))*lambda ;
+                lambda_scaled = sqrt(1.0-(sigmaMin/eps)*(sigmaMin/eps))*lambda ;
+            }
+            if(fabs(S(i))<eps) {
+                if (i<6) {
+                    // Scale lambda to size of singular value sigmaMin
+                    tmp(i) = sum*((S(i)/(S(i)*S(i)+lambda_scaled*lambda_scaled)));
+                }
+                else {
+                    tmp(i)=0.0;  // S(i)=0 for i>=6 due to cols>rows
+                }
+                //  Count number of singular values near zero
+                ++nrZeroSigmas ;
             }
-			if(fabs(S(i))<eps) {
-				if (i<6) {
-					// Scale lambda to size of singular value sigmaMin
-					tmp(i) = sum*((S(i)/(S(i)*S(i)+lambda_scaled*lambda_scaled)));
-				}
-				else {
-					tmp(i)=0.0;  // S(i)=0 for i>=6 due to cols>rows
-				}
-				//  Count number of singular values near zero
-				++nrZeroSigmas ;
-			}
-			else {
+            else {
                 tmp(i) = sum/S(i);
-			}
+            }
         }
 
         /*
@@ -208,7 +208,7 @@ namespace KDL
 
         // If number of near zero singular values is greater than the full rank
         // of jac, then wdls is active
-        if ( nrZeroSigmas > (jac.columns()-jac.rows()) )	{
+        if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
             return (error = E_CONVERGE_PINV_SINGULAR);  // converged but pinv singular
         } else {
             return (error = E_NOERROR);                 // have converged
diff --git a/orocos_kdl/src/chainiksolvervel_wdls.hpp b/orocos_kdl/src/chainiksolvervel_wdls.hpp
index 604048b87..dbba19b1a 100644
--- a/orocos_kdl/src/chainiksolvervel_wdls.hpp
+++ b/orocos_kdl/src/chainiksolvervel_wdls.hpp
@@ -102,7 +102,7 @@ namespace KDL
          * not (yet) implemented.
          *
          */
-        virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;};
+        virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return -1;};
 
         /**
          * Set the joint space weighting matrix
diff --git a/orocos_kdl/src/chainjnttojacdotsolver.cpp b/orocos_kdl/src/chainjnttojacdotsolver.cpp
index cb47d750d..c7ab26b27 100644
--- a/orocos_kdl/src/chainjnttojacdotsolver.cpp
+++ b/orocos_kdl/src/chainjnttojacdotsolver.cpp
@@ -1,6 +1,6 @@
 /*
     Computes the Jacobian time derivative
-    Copyright (C) 2015  Antoine Hoarau <hoarau [at] isir.upmc.fr>
+    Copyright (C) 2020  Antoine Hoarau <hoarau [at] isir.upmc.fr>
 
     This library is free software; you can redistribute it and/or
     modify it under the terms of the GNU Lesser General Public
@@ -85,7 +85,7 @@ int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Jacobian& jdot,
         if (representation_ == BODYFIXED) {
             // Ref Frame {ee}, Ref Point {ee}
             jac_.changeBase(F_bs_ee_.M.Inverse());
-        } else if (representation_ == INTERTIAL) {
+        } else if (representation_ == INERTIAL) {
             // Ref Frame {bs}, Ref Point {bs}
             jac_.changeRefPoint(-F_bs_ee_.p);
         } else {
@@ -98,7 +98,7 @@ int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Jacobian& jdot,
     for(unsigned int i=0;i<segmentNr;++i)
     {
         //Only increase joint nr if the segment has a joint
-        if(chain.getSegment(i).getJoint().getType()!=Joint::None){
+        if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) {
 
             for(unsigned int j=0;j<chain.getNrOfJoints();++j)
             {
@@ -114,9 +114,9 @@ int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Jacobian& jdot,
     return (error = E_NOERROR);
 }
 
-const Twist& ChainJntToJacDotSolver::getPartialDerivative(const KDL::Jacobian& J, 
-                                                          const unsigned int& joint_idx, 
-                                                          const unsigned int& column_idx, 
+const Twist& ChainJntToJacDotSolver::getPartialDerivative(const KDL::Jacobian& J,
+                                                          const unsigned int& joint_idx,
+                                                          const unsigned int& column_idx,
                                                           const int& representation)
 {
     switch(representation)
@@ -125,7 +125,7 @@ const Twist& ChainJntToJacDotSolver::getPartialDerivative(const KDL::Jacobian& J
             return getPartialDerivativeHybrid(J,joint_idx,column_idx);
         case BODYFIXED:
             return getPartialDerivativeBodyFixed(J,joint_idx,column_idx);
-        case INTERTIAL:
+        case INERTIAL:
             return getPartialDerivativeInertial(J,joint_idx,column_idx);
         default:
             SetToZero(this->t_djdq_);
@@ -133,8 +133,8 @@ const Twist& ChainJntToJacDotSolver::getPartialDerivative(const KDL::Jacobian& J
     }
 }
 
-const Twist& ChainJntToJacDotSolver::getPartialDerivativeHybrid(const KDL::Jacobian& bs_J_ee, 
-                                                                const unsigned int& joint_idx, 
+const Twist& ChainJntToJacDotSolver::getPartialDerivativeHybrid(const KDL::Jacobian& bs_J_ee,
+                                                                const unsigned int& joint_idx,
                                                                 const unsigned int& column_idx)
 {
     int j=joint_idx;
@@ -164,8 +164,8 @@ const Twist& ChainJntToJacDotSolver::getPartialDerivativeHybrid(const KDL::Jacob
     return t_djdq_;
 }
 
-const Twist& ChainJntToJacDotSolver::getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee, 
-                                                            const unsigned int& joint_idx, 
+const Twist& ChainJntToJacDotSolver::getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee,
+                                                            const unsigned int& joint_idx,
                                                             const unsigned int& column_idx)
 {
     int j=joint_idx;
@@ -186,8 +186,8 @@ const Twist& ChainJntToJacDotSolver::getPartialDerivativeBodyFixed(const Jacobia
 
     return t_djdq_;
 }
-const Twist& ChainJntToJacDotSolver::getPartialDerivativeInertial(const KDL::Jacobian& bs_J_bs, 
-                                                                  const unsigned int& joint_idx, 
+const Twist& ChainJntToJacDotSolver::getPartialDerivativeInertial(const KDL::Jacobian& bs_J_bs,
+                                                                  const unsigned int& joint_idx,
                                                                   const unsigned int& column_idx)
 {
     int j=joint_idx;
@@ -211,7 +211,7 @@ void ChainJntToJacDotSolver::setRepresentation(const int& representation)
 {
     if(representation == HYBRID ||
         representation == BODYFIXED ||
-        representation == INTERTIAL)   
+        representation == INERTIAL)
     this->representation_ = representation;
 }
 
diff --git a/orocos_kdl/src/chainjnttojacdotsolver.hpp b/orocos_kdl/src/chainjnttojacdotsolver.hpp
index ecffa1355..b2d74d2fc 100644
--- a/orocos_kdl/src/chainjnttojacdotsolver.hpp
+++ b/orocos_kdl/src/chainjnttojacdotsolver.hpp
@@ -56,23 +56,23 @@ class ChainJntToJacDotSolver : public SolverI
     static const int HYBRID = 0;
     // Body-fixed representation ref Frame: end-effector, ref Point: end-effector
     static const int BODYFIXED = 1;
-    // Intertial representation ref Frame: base, ref Point: base
-    static const int INTERTIAL = 2;
+    // Inertial representation ref Frame: base, ref Point: base
+    static const int INERTIAL = 2;
 
     explicit ChainJntToJacDotSolver(const Chain& chain);
     virtual ~ChainJntToJacDotSolver();
     /**
      * @brief Computes \f$ {}_{bs}\dot{J}^{ee}.\dot{q} \f$
-     * 
+     *
      * @param q_in Current joint positions and velocities
      * @param jac_dot_q_dot The twist representing Jdot*qdot
      * @param seg_nr The final segment to compute
-     * @return int 0 if no errors happened 
+     * @return int 0 if no errors happened
      */
     virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Twist& jac_dot_q_dot, int seg_nr = -1);
     /**
-     * @brief Computes \f$ {}_{bs}\dot{J}^{ee} \f$ 
-     * 
+     * @brief Computes \f$ {}_{bs}\dot{J}^{ee} \f$
+     *
      * @param q_in Current joint positions and velocities
      * @param jdot The jacobian time derivative in the configured representation
      *        (HYBRID, BODYFIXED or INERTIAL)
@@ -81,34 +81,34 @@ class ChainJntToJacDotSolver : public SolverI
      */
     virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Jacobian& jdot, int seg_nr = -1);
     int setLockedJoints(const std::vector<bool>& locked_joints);
-    
+
     /**
      * @brief JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector)
-     * 
-     * 
+     *
+     *
      * @return void
      */
     void setHybridRepresentation(){setRepresentation(HYBRID);}
     /**
      * @brief JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector, ref Point: end-effector)
-     * 
+     *
      * @return void
      */
     void setBodyFixedRepresentation(){setRepresentation(BODYFIXED);}
     /**
      * @brief JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base)
-     * 
+     *
      * @return void
      */
-    void setInternialRepresentation(){setRepresentation(INTERTIAL);}
+    void setInertialRepresentation(){setRepresentation(INERTIAL);}
     /**
      * @brief Sets the internal variable for the representation (with a check on the value)
-     * 
-     * @param representation The representation for Jdot : HYBRID,BODYFIXED or INTERTIAL
+     *
+     * @param representation The representation for Jdot : HYBRID,BODYFIXED or INERTIAL
      * @return void
      */
     void setRepresentation(const int& representation);
-    
+
     /// @copydoc KDL::SolverI::updateInternalDataStructures
     virtual void updateInternalDataStructures();
 
@@ -119,7 +119,7 @@ class ChainJntToJacDotSolver : public SolverI
 protected:
      /**
      * @brief Computes \f$ \frac{\partial {}_{bs}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
-     * 
+     *
      * @param bs_J_ee The Jacobian expressed in the base frame with the end effector as the reference point (default in KDL Jacobian Solver)
      * @param joint_idx The index of the current joint (j in the formula)
      * @param column_idx The index of the current column (i in the formula)
@@ -130,41 +130,41 @@ class ChainJntToJacDotSolver : public SolverI
                                      const unsigned int& column_idx);
      /**
      * @brief Computes \f$ \frac{\partial {}_{ee}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
-     * 
+     *
      * @param bs_J_ee The Jacobian expressed in the end effector frame with the end effector as the reference point
      * @param joint_idx The indice of the current joint (j in the formula)
      * @param column_idx The indice of the current column (i in the formula)
      * @return Twist The twist representing dJi/dqj .qdotj
-     */    
+     */
     const Twist& getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee,
                                         const unsigned int& joint_idx,
                                         const unsigned int& column_idx);
      /**
      * @brief Computes \f$ \frac{\partial {}_{bs}J^{i,bs}}{\partial q^{j}}.\dot{q}^{j} \f$
-     * 
+     *
      * @param ee_J_ee The Jacobian expressed in the base frame with the base as the reference point
      * @param joint_idx The indice of the current joint (j in the formula)
      * @param column_idx The indice of the current column (i in the formula)
      * @return Twist The twist representing dJi/dqj .qdotj
-     */    
+     */
     const Twist& getPartialDerivativeInertial(const Jacobian& bs_J_bs,
                                        const unsigned int& joint_idx,
                                        const unsigned int& column_idx);
      /**
      * @brief Computes \f$ \frac{\partial J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
-     * 
+     *
      * @param bs_J_bs The Jacobian expressed in the base frame with the end effector as the reference point
      * @param joint_idx The indice of the current joint (j in the formula)
      * @param column_idx The indice of the current column (i in the formula)
-     * @param representation The representation (Hybrid,Body-fixed,Inertial) in which you want to get dJ/dqj .qdotj 
+     * @param representation The representation (Hybrid,Body-fixed,Inertial) in which you want to get dJ/dqj .qdotj
      * @return Twist The twist representing dJi/dqj .qdotj
-     */    
+     */
     const Twist& getPartialDerivative(const Jacobian& J,
                                const unsigned int& joint_idx,
                                const unsigned int& column_idx,
                                const int& representation);
 private:
-    
+
     const Chain& chain;
     std::vector<bool> locked_joints_;
     unsigned int nr_of_unlocked_joints_;
diff --git a/orocos_kdl/src/chainjnttojacsolver.cpp b/orocos_kdl/src/chainjnttojacsolver.cpp
index 098559c0b..1116ac02d 100644
--- a/orocos_kdl/src/chainjnttojacsolver.cpp
+++ b/orocos_kdl/src/chainjnttojacsolver.cpp
@@ -1,8 +1,8 @@
-// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 // Version: 1.0
-// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 // URL: http://www.orocos.org/kdl
 
 // This library is free software; you can redistribute it and/or
@@ -55,7 +55,7 @@ namespace KDL
         else
             segmentNr = seg_nr;
 
-        //Initialize Jacobian to zero since only segmentNr colunns are computed
+        //Initialize Jacobian to zero since only segmentNr columns are computed
         SetToZero(jac) ;
 
         if( q_in.rows()!=chain.getNrOfJoints() || jac.columns() != chain.getNrOfJoints())
@@ -70,7 +70,7 @@ namespace KDL
         Frame total;
         for (unsigned int i=0;i<segmentNr;i++) {
             //Calculate new Frame_base_ee
-            if(chain.getSegment(i).getJoint().getType()!=Joint::None){
+            if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) {
             	//pose of the new end-point expressed in the base
                 total = T_tmp*chain.getSegment(i).pose(q_in(j));
                 //changing base of new segment's twist to base frame if it is not locked
@@ -86,7 +86,7 @@ namespace KDL
             changeRefPoint(jac,total.p-T_tmp.p,jac);
 
             //Only increase jointnr if the segment has a joint
-            if(chain.getSegment(i).getJoint().getType()!=Joint::None){
+            if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) {
                 //Only put the twist inside if it is not locked
                 if(!locked_joints_[j])
                     jac.setColumn(k++,t_tmp);
diff --git a/orocos_kdl/src/chainjnttojacsolver.hpp b/orocos_kdl/src/chainjnttojacsolver.hpp
index eef3816fd..25c3f234c 100644
--- a/orocos_kdl/src/chainjnttojacsolver.hpp
+++ b/orocos_kdl/src/chainjnttojacsolver.hpp
@@ -47,7 +47,7 @@ namespace KDL
         /**
          * Calculate the jacobian expressed in the base frame of the
          * chain, with reference point at the end effector of the
-         * *chain. The alghoritm is similar to the one used in
+         * *chain. The algorithm is similar to the one used in
          * KDL::ChainFkSolverVel_recursive
          *
          * @param q_in input joint positions
@@ -56,7 +56,7 @@ namespace KDL
          * @return success/error code
          */
         virtual int JntToJac(const JntArray& q_in, Jacobian& jac, int segmentNR=-1);
-        
+
         /**
          *
          * @param locked_joints new values for locked joints
@@ -75,4 +75,3 @@ namespace KDL
     };
 }
 #endif
-
diff --git a/orocos_kdl/src/frameacc.hpp b/orocos_kdl/src/frameacc.hpp
index bdc636cb8..20d4631b3 100644
--- a/orocos_kdl/src/frameacc.hpp
+++ b/orocos_kdl/src/frameacc.hpp
@@ -76,7 +76,7 @@ class VectorAcc
     IMETHOD VectorAcc& operator -= (const VectorAcc& arg);
     IMETHOD static VectorAcc Zero();
     IMETHOD void ReverseSign();
-    IMETHOD doubleAcc Norm();
+    IMETHOD doubleAcc Norm(double eps=epsilon);
     IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2);
     IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2);
     IMETHOD friend VectorAcc operator + (const Vector& r1,const VectorAcc& r2);
diff --git a/orocos_kdl/src/frameacc.inl b/orocos_kdl/src/frameacc.inl
index dfbdc620e..fa9c23ce4 100644
--- a/orocos_kdl/src/frameacc.inl
+++ b/orocos_kdl/src/frameacc.inl
@@ -126,9 +126,9 @@ void VectorAcc::ReverseSign() {
     dv.ReverseSign();
 }
 
-doubleAcc VectorAcc::Norm() {
+doubleAcc VectorAcc::Norm(double eps) {
     doubleAcc res;
-    res.t  = p.Norm();
+    res.t  = p.Norm(eps);
     res.d  = dot(p,v)/res.t;
     res.dd = (dot(p,dv)+dot(v,v)-res.d*res.d)/res.t;
     return res;
diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp
index cb8cd0531..13d8aa7e8 100644
--- a/orocos_kdl/src/frames.cpp
+++ b/orocos_kdl/src/frames.cpp
@@ -26,9 +26,8 @@
  ***************************************************************************/
 
 #include "frames.hpp"
+#include "utilities/utility.h"
 
-#define _USE_MATH_DEFINES  // For MSVC
-#include <math.h>
 #include <algorithm>
 
 namespace KDL {
@@ -86,12 +85,17 @@ namespace KDL {
     }
 
     double Vector2::Norm() const
+    {
+        return Norm(KDL::epsilon);
+    }
+
+    double Vector2::Norm(double eps) const
     {
         double tmp1 = fabs(data[0]);
         double tmp2 = fabs(data[1]);
         
-        if (tmp1 == 0.0 && tmp2 == 0.0)
-            return 0.0;
+        if (tmp1 < eps && tmp2 < eps)
+            return 0;
 
         if (tmp1 > tmp2) {
             return tmp1*sqrt(1+sqr(data[1]/data[0]));
@@ -100,22 +104,26 @@ namespace KDL {
         }
     }
     // makes v a unitvector and returns the norm of v.
-    // if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
+    // if v is smaller than eps, Vector(1,0) is returned with norm 0.
     // if this is not good, check the return value of this method.
     double Vector2::Normalize(double eps) {
         double v = this->Norm();
         if (v < eps) {
             *this = Vector2(1,0);
-            return v;
+            return 0;
         } else {
             *this = (*this)/v;
             return v;
         }
     }
 
+    double Vector::Norm() const
+    {
+        return Norm(KDL::epsilon);
+    }
 
     // do some effort not to lose precision
-    double Vector::Norm() const
+    double Vector::Norm(double eps) const
     {
         double tmp1;
         double tmp2;
@@ -124,7 +132,7 @@ namespace KDL {
         if (tmp1 >= tmp2) {
             tmp2=fabs(data[2]);
             if (tmp1 >= tmp2) {
-                if (tmp1 == 0) {
+                if (tmp1 < eps) {
                     // only to everything exactly zero case, all other are handled correctly
                     return 0;
                 }
@@ -149,7 +157,7 @@ namespace KDL {
         double v = this->Norm();
         if (v < eps) {
             *this = Vector(1,0,0);
-            return v;
+            return 0;
         } else {
             *this = (*this)/v;
             return v;
@@ -251,7 +259,7 @@ void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const
     {
 		double epsilon=1E-12;
 		pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) )  );
-        if ( fabs(pitch) > (M_PI/2.0-epsilon) ) {
+        if ( fabs(pitch) > (PI_2-epsilon) ) {
             yaw = atan2(	-data[1], data[4]);
             roll  = 0.0 ;
         } else {
@@ -384,7 +392,7 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const {
         }
 
         // otherwise this singularity is angle = 180
-        angle = M_PI;
+        angle = PI;
         double xx = (data[0] + 1) / 2;
         double yy = (data[4] + 1) / 2;
         double zz = (data[8] + 1) / 2;
@@ -392,69 +400,38 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const {
         double xz = (data[2] + data[6]) / 4;
         double yz = (data[5] + data[7]) / 4;
 
-        double half_sqrt_2 = 0.5 * sqrt(2.0);
-
         if ((xx > yy) && (xx > zz))
         {
             // data[0] is the largest diagonal term
-            if (xx < epsilon)
-            {
-                x = 0;
-                y = half_sqrt_2;
-                z = half_sqrt_2;
-            }
-            else
-            {
-                x = sqrt(xx);
-                y = xy/x;
-                z = xz/x;
-            }
+            x = sqrt(xx);
+            y = xy/x;
+            z = xz/x;
         }
         else if (yy > zz)
         {
             // data[4] is the largest diagonal term
-            if (yy < epsilon)
-            {
-                x = half_sqrt_2;
-                y = 0;
-                z = half_sqrt_2;
-            }
-            else
-            {
-                y = sqrt(yy);
-                x = xy/y;
-                z = yz/y;
-            }
+            y = sqrt(yy);
+            x = xy/y;
+            z = yz/y;
         }
         else
         {
             // data[8] is the largest diagonal term so base result on this
-            if (zz < epsilon)
-            {
-                x = half_sqrt_2;
-                y = half_sqrt_2;
-                z = 0;
-            }
-            else
-            {
-                z = sqrt(zz);
-                x = xz/z;
-                y = yz/z;
-            }
+            z = sqrt(zz);
+            x = xz/z;
+            y = yz/z;
         }
         axis = KDL::Vector(x, y, z);
         return angle; // return 180 deg rotation
     }
 
-    // If the matrix is slightly non-orthogonal, `f` may be out of the (-1, +1) range.
-    // Therefore, clamp it between those values to avoid NaNs.
     double f = (data[0] + data[4] + data[8] - 1) / 2;
-    angle = acos(std::max(-1.0, std::min(1.0, f)));
 
     x = (data[7] - data[5]);
     y = (data[2] - data[6]);
     z = (data[3] - data[1]);
     axis = KDL::Vector(x, y, z);
+    angle = atan2(axis.Norm()/2,f);
     axis.Normalize();
     return angle;
 }
diff --git a/orocos_kdl/src/frames.hpp b/orocos_kdl/src/frames.hpp
index 1764c1487..52f854956 100644
--- a/orocos_kdl/src/frames.hpp
+++ b/orocos_kdl/src/frames.hpp
@@ -239,6 +239,9 @@ class Vector
      //!    @return the norm of the vector
      double Norm() const;
 
+     //!    @return the norm of the vector
+     double Norm(double eps) const;
+
 
 
      //! a 3D vector where the 2D vector v is put in the XY plane
@@ -310,8 +313,8 @@ class Rotation
                 double Xy,double Yy,double Zy,
                 double Xz,double Yz,double Zz);
     inline Rotation(const Vector& x,const Vector& y,const Vector& z);
-    // default copy constructor is sufficient
 
+    inline Rotation(const Rotation& arg);
 
      inline Rotation& operator=(const Rotation& arg);
 
@@ -1017,6 +1020,9 @@ class Vector2
      //!  @return the norm of the vector
      double Norm() const;
 
+     //!  @return the norm of the vector
+     double Norm(double eps) const;
+
      //! projects v in its XY plane, and sets *this to these values
      inline void Set3DXY(const Vector& v);
 
@@ -1060,6 +1066,8 @@ class Rotation2
 
     Rotation2(double ca,double sa):s(sa),c(ca){}
 
+    Rotation2(const Rotation2& arg);
+
      inline Rotation2& operator=(const Rotation2& arg);
      inline Vector2 operator*(const Vector2& v) const;
      //!    Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set
@@ -1250,19 +1258,9 @@ IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1);
 IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1);
 
 #ifdef KDL_INLINE
-//    #include "vector.inl"
-//   #include "wrench.inl"
-    //#include "rotation.inl"
-    //#include "frame.inl"
-    //#include "twist.inl"
-    //#include "vector2.inl"
-    //#include "rotation2.inl"
-    //#include "frame2.inl"
 #include "frames.inl"
 #endif
 
-
-
 }
 
 
diff --git a/orocos_kdl/src/frames.inl b/orocos_kdl/src/frames.inl
index ac62a997a..8349ff108 100644
--- a/orocos_kdl/src/frames.inl
+++ b/orocos_kdl/src/frames.inl
@@ -512,6 +512,11 @@ Rotation::Rotation(const Vector& x,const Vector& y,const Vector& z)
     data[2] = z.data[0];data[5] = z.data[1];data[8] = z.data[2];
 }
 
+Rotation::Rotation(const Rotation& arg) {
+    int count=9;
+    while (count--) data[count] = arg.data[count];
+}
+
 Rotation& Rotation::operator=(const Rotation& arg) {
     int count=9;
     while (count--) data[count] = arg.data[count];
@@ -839,7 +844,9 @@ IMETHOD void Vector2::Set3DPlane(const Frame& F_someframe_XY,const Vector& v_som
     data[1]=tmp(1);
 }
 
-
+IMETHOD Rotation2::Rotation2(const Rotation2& arg) {
+    c=arg.c;s=arg.s;
+}
 
 IMETHOD Rotation2& Rotation2::operator=(const Rotation2& arg) {
     c=arg.c;s=arg.s;
@@ -1151,7 +1158,7 @@ IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt) {
 }
 
 IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt) {
-	return a*Rot(a.Inverse(da)*dt);
+	return Rot(da*dt)*a;
 }
 IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt) {
 	return Frame(
diff --git a/orocos_kdl/src/framevel.hpp b/orocos_kdl/src/framevel.hpp
index 243898a60..650bd3c83 100644
--- a/orocos_kdl/src/framevel.hpp
+++ b/orocos_kdl/src/framevel.hpp
@@ -104,7 +104,7 @@ class VectorVel
     IMETHOD VectorVel& operator -= (const VectorVel& arg);
     IMETHOD static VectorVel Zero();
     IMETHOD void ReverseSign();
-    IMETHOD doubleVel Norm() const;
+    IMETHOD doubleVel Norm(double eps=epsilon) const;
     IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2);
     IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2);
     IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2);
@@ -128,6 +128,14 @@ class VectorVel
     IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps);
     IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps);
     IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps);
+
+    IMETHOD friend bool operator==(const VectorVel& r1,const VectorVel& r2);
+    IMETHOD friend bool operator!=(const VectorVel& r1,const VectorVel& r2);
+    IMETHOD friend bool operator==(const Vector& r1,const VectorVel& r2);
+    IMETHOD friend bool operator!=(const Vector& r1,const VectorVel& r2);
+    IMETHOD friend bool operator==(const VectorVel& r1,const Vector& r2);
+    IMETHOD friend bool operator!=(const VectorVel& r1,const Vector& r2);
+
     IMETHOD friend VectorVel operator - (const VectorVel& r);
     IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs);
     IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs);
@@ -185,6 +193,13 @@ class RotationVel
     IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps);
     IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps);
 
+    IMETHOD friend bool operator==(const RotationVel& r1,const RotationVel& r2);
+    IMETHOD friend bool operator!=(const RotationVel& r1,const RotationVel& r2);
+    IMETHOD friend bool operator==(const Rotation& r1,const RotationVel& r2);
+    IMETHOD friend bool operator!=(const Rotation& r1,const RotationVel& r2);
+    IMETHOD friend bool operator==(const RotationVel& r1,const Rotation& r2);
+    IMETHOD friend bool operator!=(const RotationVel& r1,const Rotation& r2);
+
     IMETHOD TwistVel Inverse(const TwistVel& arg) const;
     IMETHOD TwistVel Inverse(const Twist& arg) const;
     IMETHOD TwistVel operator * (const TwistVel& arg) const;
@@ -239,6 +254,13 @@ class FrameVel
     IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps);
     IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps);
 
+    IMETHOD friend bool operator==(const FrameVel& a,const FrameVel& b);
+    IMETHOD friend bool operator!=(const FrameVel& a,const FrameVel& b);
+    IMETHOD friend bool operator==(const Frame& a,const FrameVel& b);
+    IMETHOD friend bool operator!=(const Frame& a,const FrameVel& b);
+    IMETHOD friend bool operator==(const FrameVel& a,const Frame& b);
+    IMETHOD friend bool operator!=(const FrameVel& a,const Frame& b);
+
     IMETHOD TwistVel  Inverse(const TwistVel& arg) const;
     IMETHOD TwistVel  Inverse(const Twist& arg) const;
     IMETHOD TwistVel operator * (const TwistVel& arg) const;
@@ -311,6 +333,13 @@ class TwistVel
      IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps);
      IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps);
 
+     IMETHOD friend bool operator==(const TwistVel& a,const TwistVel& b);
+     IMETHOD friend bool operator!=(const TwistVel& a,const TwistVel& b);
+     IMETHOD friend bool operator==(const Twist& a,const TwistVel& b);
+     IMETHOD friend bool operator!=(const Twist& a,const TwistVel& b);
+     IMETHOD friend bool operator==(const TwistVel& a,const Twist& b);
+     IMETHOD friend bool operator!=(const TwistVel& a,const Twist& b);
+
 // = Conversion to other entities
      IMETHOD Twist GetTwist() const;
      IMETHOD Twist GetTwistDot() const;
diff --git a/orocos_kdl/src/framevel.inl b/orocos_kdl/src/framevel.inl
index 61a43fdc2..05aceb0af 100644
--- a/orocos_kdl/src/framevel.inl
+++ b/orocos_kdl/src/framevel.inl
@@ -81,6 +81,41 @@ bool Equal(const Frame& r1,const FrameVel& r2,double eps) {
 bool Equal(const FrameVel& r1,const Frame& r2,double eps) {
     return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
 }
+bool operator==(const FrameVel& r1,const FrameVel& r2) {
+#ifdef KDL_USE_EQUAL
+    return Equal(r1, r2);
+#else
+    return (r1.p == r2.p &&
+            r1.M == r2.M );
+#endif
+}
+bool operator!=(const FrameVel& r1,const FrameVel& r2) {
+    return !operator==(r1,r2);
+}
+bool operator==(const Frame& r1,const FrameVel& r2) {
+#ifdef KDL_USE_EQUAL
+    return Equal(r1, r2);
+#else
+    return (r1.p == r2.p &&
+            r1.M == r2.M );
+#endif
+}
+bool operator!=(const Frame& r1,const FrameVel& r2) {
+    return !operator==(r1,r2);
+}
+bool operator==(const FrameVel& r1,const Frame& r2) {
+#ifdef KDL_USE_EQUAL
+    return Equal(r1, r2);
+#else
+    return (r1.p == r2.p &&
+            r1.M == r2.M );
+#endif
+}
+bool operator!=(const FrameVel& r1,const Frame& r2) {
+    return !operator==(r1,r2);
+}
+
+
 
 Frame FrameVel::GetFrame() const {
     return Frame(M.R,p.p);
@@ -315,9 +350,11 @@ void VectorVel::ReverseSign() {
     p.ReverseSign();
     v.ReverseSign();
 }
-doubleVel VectorVel::Norm() const {
-    double n = p.Norm();
-    return doubleVel(n,dot(p,v)/n);
+doubleVel VectorVel::Norm(double eps) const {
+    double n = p.Norm(eps);
+    if (n < eps) // Setting norm  of p and v to 0 in case norm of p is smaller than eps
+        return doubleVel(0, 0);
+    return doubleVel(n, dot(p,v)/n);
 }
 
 bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) {
@@ -329,6 +366,40 @@ bool Equal(const Vector& r1,const VectorVel& r2,double eps) {
 bool Equal(const VectorVel& r1,const Vector& r2,double eps) {
     return (Equal(r1.p,r2,eps) && Equal(r1.v,Vector::Zero(),eps));
 }
+bool operator==(const VectorVel& r1,const VectorVel& r2) {
+#ifdef KDL_USE_EQUAL
+    return Equal(r1, r2);
+#else
+    return (r1.p == r2.p &&
+            r1.v == r2.v );
+#endif
+}
+bool operator!=(const VectorVel& r1,const VectorVel& r2) {
+    return !operator==(r1,r2);
+}
+bool operator==(const Vector& r1,const VectorVel& r2) {
+#ifdef KDL_USE_EQUAL
+    return Equal(r1, r2);
+#else
+    return (r1 == r2.p &&
+            Vector::Zero() == r2.v);
+#endif
+}
+bool operator!=(const Vector& r1,const VectorVel& r2) {
+    return !operator==(r1,r2);
+}
+bool operator==(const VectorVel& r1,const Vector& r2) {
+#ifdef KDL_USE_EQUAL
+    return Equal(r1, r2);
+#else
+    return (r1.p == r2 &&
+            r1.v == Vector::Zero() );
+#endif
+}
+bool operator!=(const VectorVel& r1,const Vector& r2) {
+    return !operator==(r1,r2);
+}
+
 
 bool Equal(const RotationVel& r1,const RotationVel& r2,double eps) {
     return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps));
@@ -339,6 +410,41 @@ bool Equal(const Rotation& r1,const RotationVel& r2,double eps) {
 bool Equal(const RotationVel& r1,const Rotation& r2,double eps) {
     return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps));
 }
+bool operator==(const RotationVel& r1,const RotationVel& r2) {
+#ifdef KDL_USE_EQUAL
+    return Equal(r1, r2);
+#else
+    return (r1.w == r2.w &&
+            r1.R == r2.R );
+#endif
+}
+bool operator!=(const RotationVel& r1,const RotationVel& r2) {
+    return !operator==(r1,r2);
+}
+bool operator==(const Rotation& r1,const RotationVel& r2) {
+#ifdef KDL_USE_EQUAL
+    return Equal(r1, r2);
+#else
+    return (Vector::Zero() == r2.w &&
+            r1 == r2.R);
+#endif
+}
+bool operator!=(const Rotation& r1,const RotationVel& r2) {
+    return !operator==(r1,r2);
+}
+bool operator==(const RotationVel& r1,const Rotation& r2) {
+#ifdef KDL_USE_EQUAL
+    return Equal(r1, r2);
+#else
+    return (r1.w == Vector::Zero() &&
+            r1.R == r2);
+#endif
+}
+bool operator!=(const RotationVel& r1,const Rotation& r2) {
+    return !operator==(r1,r2);
+}
+
+
 bool Equal(const TwistVel& a,const TwistVel& b,double eps) {
         return (Equal(a.rot,b.rot,eps)&&
                 Equal(a.vel,b.vel,eps)  );
@@ -351,7 +457,39 @@ bool Equal(const TwistVel& a,const Twist& b,double eps) {
         return (Equal(a.rot,b.rot,eps)&&
                 Equal(a.vel,b.vel,eps)  );
 }
-
+bool operator==(const TwistVel& a,const TwistVel& b) {
+#ifdef KDL_USE_EQUAL
+    return Equal(a, b);
+#else
+    return (a.rot == b.rot &&
+            a.vel == b.vel );
+#endif
+}
+bool operator!=(const TwistVel& a,const TwistVel& b) {
+    return !operator==(a,b);
+}
+bool operator==(const Twist& a,const TwistVel& b) {
+#ifdef KDL_USE_EQUAL
+    return Equal(a, b);
+#else
+    return (a.rot == b.rot &&
+            a.vel == b.vel );
+#endif
+}
+bool operator!=(const Twist& r1,const TwistVel& r2) {
+    return !operator==(r1,r2);
+}
+bool operator==(const TwistVel& r1,const Twist& r2) {
+#ifdef KDL_USE_EQUAL
+    return Equal(r1, r2);
+#else
+    return (a.rot == b.rot &&
+            a.vel == b.vel );
+#endif
+}
+bool operator!=(const TwistVel& r1,const Twist& r2) {
+    return !operator==(r1,r2);
+}
 
 
 IMETHOD doubleVel dot(const VectorVel& lhs,const VectorVel& rhs) {
@@ -364,17 +502,6 @@ IMETHOD doubleVel dot(const Vector& lhs,const VectorVel& rhs) {
     return doubleVel(dot(lhs,rhs.p),dot(lhs,rhs.v));
 }
 
-
-
-
-
-
-
-
-
-
-
-
 TwistVel TwistVel::Zero()
 {
     return TwistVel(VectorVel::Zero(),VectorVel::Zero());
diff --git a/orocos_kdl/src/framevel_io.hpp b/orocos_kdl/src/framevel_io.hpp
index 2124c13c4..a3dcc0743 100644
--- a/orocos_kdl/src/framevel_io.hpp
+++ b/orocos_kdl/src/framevel_io.hpp
@@ -21,7 +21,6 @@
 #include "utilities/utility_io.h"
 #include "utilities/rall1d_io.h"
 
-#include "framevel_io.hpp"
 #include "frames_io.hpp"
 
 namespace KDL {
diff --git a/orocos_kdl/src/jacobian.cpp b/orocos_kdl/src/jacobian.cpp
index 839ad0011..8c4259321 100644
--- a/orocos_kdl/src/jacobian.cpp
+++ b/orocos_kdl/src/jacobian.cpp
@@ -1,8 +1,8 @@
-// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 // Version: 1.0
-// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 // URL: http://www.orocos.org/kdl
 
 // This library is free software; you can redistribute it and/or
@@ -33,15 +33,16 @@ namespace KDL
     Jacobian::Jacobian(unsigned int nr_of_columns):
         data(6,nr_of_columns)
     {
+        data.setZero();
     }
-    
+
     Jacobian::Jacobian(const Jacobian& arg):
         data(arg.data)
     {
     }
 
     Jacobian& Jacobian::operator = (const Jacobian& arg)
-    { 
+    {
         this->data=arg.data;
         return *this;
     }
@@ -49,7 +50,7 @@ namespace KDL
 
     Jacobian::~Jacobian()
     {
-        
+
     }
 
     void Jacobian::resize(unsigned int new_nr_of_columns)
@@ -95,7 +96,7 @@ namespace KDL
             dest.setColumn(i,src1.getColumn(i).RefPoint(base_AB));
         return true;
     }
-    
+
     void Jacobian::changeBase(const Rotation& rot){
         for(unsigned int i=0;i<data.cols();i++)
             this->setColumn(i,rot*this->getColumn(i));;
@@ -114,7 +115,7 @@ namespace KDL
         for(unsigned int i=0;i<data.cols();i++)
             this->setColumn(i,frame*this->getColumn(i));
     }
-    
+
     bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest)
     {
         if(src1.columns()!=dest.columns())
@@ -128,24 +129,24 @@ namespace KDL
     {
         return Equal((*this),arg);
     }
-    
+
     bool Jacobian::operator!=(const Jacobian& arg)const
     {
         return !Equal((*this),arg);
     }
-    
+
     bool Equal(const Jacobian& a,const Jacobian& b,double eps)
     {
         if(a.rows()==b.rows()&&a.columns()==b.columns()){
-            return a.data.isApprox(b.data,eps);
+            return (a.data-b.data).isZero(eps);
         }else
             return false;
     }
-    
+
     Twist Jacobian::getColumn(unsigned int i) const{
         return Twist(Vector(data(0,i),data(1,i),data(2,i)),Vector(data(3,i),data(4,i),data(5,i)));
     }
-    
+
     void Jacobian::setColumn(unsigned int i,const Twist& t){
         data.col(i).head<3>()=Eigen::Map<const Vector3d>(t.vel.data);
         data.col(i).tail<3>()=Eigen::Map<const Vector3d>(t.rot.data);
diff --git a/orocos_kdl/src/jntarray.cpp b/orocos_kdl/src/jntarray.cpp
index 2db5bd9e1..daa28a40e 100644
--- a/orocos_kdl/src/jntarray.cpp
+++ b/orocos_kdl/src/jntarray.cpp
@@ -114,11 +114,11 @@ namespace KDL
     {
         if(src1.rows()!=src2.rows())
             return false;
-        return src1.data.isApprox(src2.data,eps);
+        return (src1.data-src2.data).isZero(eps);
     }
 
-    bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
-    //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
+    bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}
+    //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}
 
 }
 
diff --git a/orocos_kdl/src/jntspaceinertiamatrix.cpp b/orocos_kdl/src/jntspaceinertiamatrix.cpp
index dea5cb646..3103c919f 100644
--- a/orocos_kdl/src/jntspaceinertiamatrix.cpp
+++ b/orocos_kdl/src/jntspaceinertiamatrix.cpp
@@ -112,7 +112,7 @@ namespace KDL
     {
         if(src1.rows()!=src2.rows()||src1.columns()!=src2.columns())
             return false;
-        return src1.data.isApprox(src2.data,eps);
+        return (src1.data-src2.data).isZero(eps);
     }
 
     bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);};
diff --git a/orocos_kdl/src/joint.cpp b/orocos_kdl/src/joint.cpp
index e487334b8..b2e7c4a3b 100644
--- a/orocos_kdl/src/joint.cpp
+++ b/orocos_kdl/src/joint.cpp
@@ -1,8 +1,8 @@
-// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 // Version: 1.0
-// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 // URL: http://www.orocos.org/kdl
 
 // This library is free software; you can redistribute it and/or
@@ -42,7 +42,7 @@ namespace KDL {
     }
 
     // constructor for joint along arbitrary axis, at arbitrary origin
-    Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, 
+    Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale,
                  const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
       name(_name), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
       , axis(_axis / _axis.Norm()), origin(_origin)
@@ -56,7 +56,7 @@ namespace KDL {
     }
 
     // constructor for joint along arbitrary axis, at arbitrary origin
-    Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, 
+    Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale,
                  const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
           name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness),
           axis(_axis / _axis.Norm()),origin(_origin)
@@ -97,7 +97,7 @@ namespace KDL {
             return Frame(Vector(0.0,scale*q+offset,0.0));
         case TransZ:
             return Frame(Vector(0.0,0.0,scale*q+offset));
-        case None:
+        case Fixed:
             return Frame::Identity();
         }
         return Frame::Identity();
@@ -122,7 +122,7 @@ namespace KDL {
             return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0));
         case TransZ:
             return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0));
-        case None:
+        case Fixed:
             return Twist::Zero();
         }
         return Twist::Zero();
@@ -148,7 +148,7 @@ namespace KDL {
         return Vector(0.,1.,0.);
       case TransZ:
         return Vector(0.,0.,1.);
-      case None:
+      case Fixed:
         return Vector::Zero();
       }
     return Vector::Zero();
diff --git a/orocos_kdl/src/joint.hpp b/orocos_kdl/src/joint.hpp
index a188aff95..49c3e391f 100644
--- a/orocos_kdl/src/joint.hpp
+++ b/orocos_kdl/src/joint.hpp
@@ -1,8 +1,8 @@
-// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 // Version: 1.0
-// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 // URL: http://www.orocos.org/kdl
 
 // This library is free software; you can redistribute it and/or
@@ -44,12 +44,12 @@ namespace KDL {
      */
     class Joint {
     public:
-        typedef enum { RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,None} JointType;
+        typedef enum { RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,Fixed,None=Fixed} JointType;
         /**
          * Constructor of a joint.
          *
          * @param name of the joint
-         * @param type type of the joint, default: Joint::None
+         * @param type type of the joint, default: Joint::Fixed
          * @param scale scale between joint input and actual geometric
          * movement, default: 1
          * @param offset offset between joint input and actual
@@ -59,12 +59,12 @@ namespace KDL {
          * @param stiffness 1D stiffness along the joint axis,
          * default: 0
          */
-        explicit Joint(const std::string& name, const JointType& type=None,const double& scale=1,const double& offset=0,
+        explicit Joint(const std::string& name, const JointType& type=Fixed,const double& scale=1,const double& offset=0,
               const double& inertia=0,const double& damping=0,const double& stiffness=0);
         /**
          * Constructor of a joint.
          *
-         * @param type type of the joint, default: Joint::None
+         * @param type type of the joint, default: Joint::Fixed
          * @param scale scale between joint input and actual geometric
          * movement, default: 1
          * @param offset offset between joint input and actual
@@ -74,7 +74,7 @@ namespace KDL {
          * @param stiffness 1D stiffness along the joint axis,
          * default: 0
          */
-        explicit Joint(const JointType& type=None,const double& scale=1,const double& offset=0,
+        explicit Joint(const JointType& type=Fixed,const double& scale=1,const double& offset=0,
                const double& inertia=0,const double& damping=0,const double& stiffness=0);
         /**
          * Constructor of a joint.
@@ -128,16 +128,16 @@ namespace KDL {
          */
         Twist twist(const double& qdot)const;
 
-        /**                                                                     
-         * Request the Vector corresponding to the axis of a revolute joint.    
-         *                                                                      
-         * @return Vector. e.g (1,0,0) for RotX etc.                            
+        /**
+         * Request the Vector corresponding to the axis of a revolute joint.
+         *
+         * @return Vector. e.g (1,0,0) for RotX etc.
          */
         Vector JointAxis() const;
 
-        /**                                                                     
-         * Request the Vector corresponding to the origin of a revolute joint.    
-         *                                                                      
+        /**
+         * Request the Vector corresponding to the origin of a revolute joint.
+         *
          * @return Vector
          */
         Vector JointOrigin() const;
@@ -160,8 +160,8 @@ namespace KDL {
         {
             return type;
         };
-      
-        /** 
+
+        /**
          * Request the stringified type of the joint.
          *
          * @return const string
@@ -185,13 +185,23 @@ namespace KDL {
                 return "TransY";
             case TransZ:
                 return "TransZ";
-            case None:
-                return "None";
+            case Fixed:
+                return "Fixed";
             default:
-                return "None";
+                return "Fixed";
             }
         };
 
+        /**
+         * Request the inertia of the joint.
+         *
+         * @return const reference to the inertia of the joint
+         */
+        const double& getInertia() const
+        {
+            return inertia;
+        };
+
         virtual ~Joint();
 
     private:
@@ -209,7 +219,7 @@ namespace KDL {
         mutable double q_previous;
 
 
-      
+
       class joint_type_exception: public std::exception{
 	virtual const char* what() const throw(){
 	  return "Joint Type excption";}
diff --git a/orocos_kdl/src/path.cpp b/orocos_kdl/src/path.cpp
index 9b14b0813..b22f2ab93 100644
--- a/orocos_kdl/src/path.cpp
+++ b/orocos_kdl/src/path.cpp
@@ -43,6 +43,7 @@
 
 #include "utilities/error.h"
 #include "utilities/error_stack.h"
+#include "utilities/scoped_ptr.hpp"
 #include "path.hpp"
 #include "path_line.hpp"
 #include "path_point.hpp"
@@ -78,7 +79,7 @@ Path* Path::Read(istream& is) {
 		Frame endpos;
 		is >> startpos;
 		is >> endpos;
-		auto_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
+		scoped_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
 		double eqradius;
 		is >> eqradius;
 		EatEnd(is,']');
@@ -99,7 +100,7 @@ Path* Path::Read(istream& is) {
 		is >> R_base_end;
 		is >> alpha;
 		alpha *= deg2rad;
-		auto_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
+		scoped_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
 		is >> eqradius;
 		EatEnd(is,']');
 		IOTracePop();
@@ -119,8 +120,8 @@ Path* Path::Read(istream& is) {
 		is >> radius;
 		double eqradius;
 		is >> eqradius;
-		auto_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
-		auto_ptr<Path_RoundedComposite> tr(
+		scoped_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
+		scoped_ptr<Path_RoundedComposite> tr(
 			new Path_RoundedComposite(radius,eqradius,orient.release())
 		);
 		int size;
@@ -139,7 +140,7 @@ Path* Path::Read(istream& is) {
 	} else if (strcmp(storage,"COMPOSITE")==0) {
 		IOTrace("COMPOSITE");
 		int size;
-		auto_ptr<Path_Composite> tr( new Path_Composite() );
+		scoped_ptr<Path_Composite> tr( new Path_Composite() );
 		is >> size;
 		int i;
 		for (i=0;i<size;i++) {
@@ -152,7 +153,7 @@ Path* Path::Read(istream& is) {
 	} else if (strcmp(storage,"CYCLIC_CLOSED")==0) {
 		IOTrace("CYCLIC_CLOSED");
 		int times;
-		auto_ptr<Path> tr( Path::Read(is) );
+		scoped_ptr<Path> tr( Path::Read(is) );
 		is >> times;
 		EatEnd(is,']');
 		IOTracePop();
diff --git a/orocos_kdl/src/path_circle.cpp b/orocos_kdl/src/path_circle.cpp
index 5efd22bc8..53261e832 100644
--- a/orocos_kdl/src/path_circle.cpp
+++ b/orocos_kdl/src/path_circle.cpp
@@ -45,51 +45,56 @@
 
 namespace KDL {
 
-
-
 Path_Circle::Path_Circle(const Frame& F_base_start,
-			const Vector& _V_base_center,
-			const Vector& V_base_p,
-			const Rotation& R_base_end,
-			double alpha,
-			RotationalInterpolation* _orient,
-			double _eqradius,
-            bool _aggregate) :
-				orient(_orient) ,
-				eqradius(_eqradius),
-                aggregate(_aggregate)
-			{
-					F_base_center.p = _V_base_center;
-					orient->SetStartEnd(F_base_start.M,R_base_end);
-					double oalpha = orient->Angle();
-
-					Vector x(F_base_start.p - F_base_center.p);
-					radius = x.Normalize();
-					if (radius < epsilon) throw Error_MotionPlanning_Circle_ToSmall();
-					Vector tmpv(V_base_p-F_base_center.p);
-					tmpv.Normalize();
-					Vector z( x * tmpv);
-                    double n = z.Normalize();
-				    if (n < epsilon) throw Error_MotionPlanning_Circle_No_Plane();
-					F_base_center.M = Rotation(x,z*x,z);
-					double dist = alpha*radius;
-					// See what has the slowest eq. motion, and adapt
-					// the other to this slower motion
-					// use eqradius to transform between rot and transl.
-					// the same as for lineair motion
-					if (oalpha*eqradius > dist) {
-						// rotational_interpolation is the limitation
-						pathlength = oalpha*eqradius;
-						scalerot   = 1/eqradius;
-						scalelin   = dist/pathlength;
-					} else {
-						// translation is the limitation
-						pathlength = dist;
-						scalerot   = oalpha/pathlength;
-						scalelin   = 1;
-					}
-			}
-
+                         const Vector& _V_base_center,
+                         const Vector& V_base_p,
+                         const Rotation& R_base_end,
+                         double alpha,
+                         RotationalInterpolation* _orient,
+                         double _eqradius,
+                         bool _aggregate) :
+    orient(_orient) ,
+    eqradius(_eqradius),
+    aggregate(_aggregate)
+{
+    F_base_center.p = _V_base_center;
+    orient->SetStartEnd(F_base_start.M,R_base_end);
+    double oalpha = orient->Angle();
+
+    Vector x(F_base_start.p - F_base_center.p);
+    radius = x.Normalize();
+    if (radius < epsilon) {
+        if (aggregate)
+            delete orient;
+        throw Error_MotionPlanning_Circle_ToSmall();
+    }
+    Vector tmpv(V_base_p-F_base_center.p);
+    tmpv.Normalize();
+    Vector z( x * tmpv);
+    double n = z.Normalize();
+    if (n < epsilon) {
+        if (aggregate)
+            delete orient;
+        throw Error_MotionPlanning_Circle_No_Plane();
+    }
+    F_base_center.M = Rotation(x,z*x,z);
+    double dist = alpha*radius;
+    // See what has the slowest eq. motion, and adapt
+    // the other to this slower motion
+    // use eqradius to transform between rot and transl.
+    // the same as for lineair motion
+    if (oalpha*eqradius > dist) {
+        // rotational_interpolation is the limitation
+        pathlength = oalpha*eqradius;
+        scalerot = 1/eqradius;
+        scalelin = dist/pathlength;
+    } else {
+        // translation is the limitation
+        pathlength = dist;
+        scalerot = oalpha/pathlength;
+        scalelin = 1;
+    }
+}
 
 
 double Path_Circle::LengthToS(double length) {
@@ -150,8 +155,6 @@ Path_Circle::~Path_Circle() {
         delete orient;
 }
 
-
-
 void Path_Circle::Write(std::ostream& os) {
 	os << "CIRCLE[ ";
 	os << "  " << Pos(0) << std::endl;
@@ -164,6 +167,4 @@ void Path_Circle::Write(std::ostream& os) {
 	os << "]"<< std::endl;
 }
 
-
 }
-
diff --git a/orocos_kdl/src/path_composite.cpp b/orocos_kdl/src/path_composite.cpp
index 62e099d51..c93f5a293 100644
--- a/orocos_kdl/src/path_composite.cpp
+++ b/orocos_kdl/src/path_composite.cpp
@@ -42,6 +42,7 @@
 
 #include "path_composite.hpp"
 #include "utilities/error.h"
+#include "utilities/scoped_ptr.hpp"
 #include <memory>
 
 namespace KDL {
@@ -50,7 +51,7 @@ namespace KDL {
 // simple linear search : TODO : make it binary search
 // uses cached_... variables
 // returns the relative path length within the segment
-// you propably want to use the cached_index variable
+// you probably want to use the cached_index variable
 double Path_Composite::Lookup(double s) const
 {
 	assert(s>=-1e-12);
@@ -110,7 +111,7 @@ Twist Path_Composite::Acc(double s,double sd,double sdd) const {
 }
 
 Path* Path_Composite::Clone()  {
-	std::auto_ptr<Path_Composite> comp( new Path_Composite() );
+	scoped_ptr<Path_Composite> comp( new Path_Composite() );
 	for (unsigned int i = 0; i < dv.size(); ++i) {
 		comp->Add(gv[i].first->Clone(), gv[i].second);
 	}
@@ -127,18 +128,18 @@ void Path_Composite::Write(std::ostream& os)  {
 }
 
 int Path_Composite::GetNrOfSegments() {
-	return dv.size();
+	return static_cast<int>(dv.size());
 }
 
 Path* Path_Composite::GetSegment(int i) {
 	assert(i>=0);
-	assert(i<dv.size());
+	assert(i<static_cast<int>(dv.size()));
 	return gv[i].first;
 }
 
 double Path_Composite::GetLengthToEndOfSegment(int i) {
 	assert(i>=0);
-	assert(i<dv.size());
+	assert(i<static_cast<int>(dv.size()));
 	return dv[i];
 }
 
diff --git a/orocos_kdl/src/path_line.hpp b/orocos_kdl/src/path_line.hpp
index 548aaf4aa..7f27ba906 100644
--- a/orocos_kdl/src/path_line.hpp
+++ b/orocos_kdl/src/path_line.hpp
@@ -98,7 +98,7 @@ class Path_Line : public Path
          * of the frame in which you express your path.
          * Other implementations for RotationalInterpolations COULD be
          *    (not implemented) (yet) :
-         *    1) quaternion interpolation : but this is more difficult for the human to interprete
+         *    1) quaternion interpolation : but this is more difficult for the human to interpret
          *    2) 3-axis interpolation : express the orientation of the frame in e.g.
          *       euler zyx angles alfa,beta, gamma  and interpolate these parameters.
          *       But this is dependent of the frame you choose as a reference and
diff --git a/orocos_kdl/src/path_roundedcomposite.cpp b/orocos_kdl/src/path_roundedcomposite.cpp
index b79e1f9fd..2916505c0 100644
--- a/orocos_kdl/src/path_roundedcomposite.cpp
+++ b/orocos_kdl/src/path_roundedcomposite.cpp
@@ -44,6 +44,7 @@
 #include "path_line.hpp"
 #include "path_circle.hpp"
 #include "utilities/error.h"
+#include "utilities/scoped_ptr.hpp"
 #include <memory>
 
 
@@ -85,7 +86,8 @@ void Path_RoundedComposite::Add(const Frame& F_base_point) {
 		if (bcdist < eps) {
 			throw Error_MotionPlanning_Not_Feasible(3);
 		}
-		double alpha = acos(dot(ab, bc) / abdist / bcdist);
+		// Clamp to avoid rounding errors (acos is defined between [-1 ; 1])
+		double alpha = acos(std::max(-1., std::min(dot(ab, bc) / abdist / bcdist, 1.)));
 		if ((PI - alpha) < eps) {
 			throw Error_MotionPlanning_Not_Feasible(4);
 		}
@@ -104,11 +106,11 @@ void Path_RoundedComposite::Add(const Frame& F_base_point) {
 			if (d >= bcdist)
 				throw Error_MotionPlanning_Not_Feasible(6);
 
-			std::auto_ptr < Path
+			scoped_ptr < Path
 					> line1(
 							new Path_Line(F_base_start, F_base_via,
 									orient->Clone(), eqradius));
-			std::auto_ptr < Path
+			scoped_ptr < Path
 					> line2(
 							new Path_Line(F_base_via, F_base_point,
 									orient->Clone(), eqradius));
@@ -187,8 +189,8 @@ void Path_RoundedComposite::GetCurrentSegmentLocation(double s,
 
 
 Path_RoundedComposite::~Path_RoundedComposite() {
-    if (aggregate)
-        delete orient;
+	if (aggregate)
+		delete orient;
 	delete comp;
 }
 
diff --git a/orocos_kdl/src/path_roundedcomposite.hpp b/orocos_kdl/src/path_roundedcomposite.hpp
index 79c2657f5..2ccb860a0 100644
--- a/orocos_kdl/src/path_roundedcomposite.hpp
+++ b/orocos_kdl/src/path_roundedcomposite.hpp
@@ -96,7 +96,7 @@ class Path_RoundedComposite : public Path
 		 * - 3101 if the eq. radius <= 0
 		 * - 3102 if the first segment in a rounding has zero length.
 		 * - 3103 if the second segment in a rounding has zero length.
-		 * - 3104 if the angle between the first and the second segment is close to M_PI.
+		 * - 3104 if the angle between the first and the second segment is close to PI.
 		 *         (meaning that the segments are on top of each other)
 		 * - 3105 if the distance needed for the rounding is larger then the first segment.
 		 * - 3106 if the distance needed for the rounding is larger then the second segment.
diff --git a/orocos_kdl/src/rotational_interpolation.cpp b/orocos_kdl/src/rotational_interpolation.cpp
index f8186e67a..bb388c53d 100644
--- a/orocos_kdl/src/rotational_interpolation.cpp
+++ b/orocos_kdl/src/rotational_interpolation.cpp
@@ -51,7 +51,6 @@ namespace KDL {
 using namespace std;
 
 RotationalInterpolation* RotationalInterpolation::Read(istream& is) {
-	// auto_ptr because exception can be thrown !
 	IOTrace("RotationalInterpolation::Read");
 	char storage[64];
 	EatWord(is,"[",storage,sizeof(storage));
diff --git a/orocos_kdl/src/rotational_interpolation_sa.cpp b/orocos_kdl/src/rotational_interpolation_sa.cpp
index 6a0accd14..6b5a56b5c 100644
--- a/orocos_kdl/src/rotational_interpolation_sa.cpp
+++ b/orocos_kdl/src/rotational_interpolation_sa.cpp
@@ -47,7 +47,7 @@ namespace KDL {
 
 
 RotationalInterpolation_SingleAxis::RotationalInterpolation_SingleAxis()
-	{};
+	{}
 
 void RotationalInterpolation_SingleAxis::SetStartEnd(Rotation start,Rotation end) {
 	R_base_start = start;
diff --git a/orocos_kdl/src/segment.hpp b/orocos_kdl/src/segment.hpp
index 934b17a07..fff85297e 100644
--- a/orocos_kdl/src/segment.hpp
+++ b/orocos_kdl/src/segment.hpp
@@ -1,8 +1,8 @@
-// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 // Version: 1.0
-// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 // URL: http://www.orocos.org/kdl
 
 // This library is free software; you can redistribute it and/or
@@ -57,22 +57,22 @@ namespace KDL {
          *
          * @param name name of the segment
          * @param joint joint of the segment, default:
-         * Joint(Joint::None)
+         * Joint(Joint::Fixed)
          * @param f_tip frame from the end of the joint to the tip of
          * the segment, default: Frame::Identity()
          * @param M rigid body inertia of the segment, default: Inertia::Zero()
          */
-        explicit Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
+        explicit Segment(const std::string& name, const Joint& joint=Joint(Joint::Fixed), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
         /**
          * Constructor of the segment
          *
          * @param joint joint of the segment, default:
-         * Joint(Joint::None)
+         * Joint(Joint::Fixed)
          * @param f_tip frame from the end of the joint to the tip of
          * the segment, default: Frame::Identity()
          * @param M rigid body inertia of the segment, default: Inertia::Zero()
          */
-        explicit Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
+        explicit Segment(const Joint& joint=Joint(Joint::Fixed), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
         Segment(const Segment& in);
         Segment& operator=(const Segment& arg);
 
@@ -148,7 +148,7 @@ namespace KDL {
          */
         Frame getFrameToTip()const
         {
-            
+
             return joint.pose(0)*f_tip;
         }
 
diff --git a/orocos_kdl/src/trajectory.cpp b/orocos_kdl/src/trajectory.cpp
index 6a3e78639..d7e66ca0e 100644
--- a/orocos_kdl/src/trajectory.cpp
+++ b/orocos_kdl/src/trajectory.cpp
@@ -42,6 +42,7 @@
 
 #include "utilities/error.h"
 #include "utilities/error_stack.h"
+#include "utilities/scoped_ptr.hpp"
 #include "trajectory.hpp"
 #include "path.hpp"
 #include "trajectory_segment.hpp"
@@ -55,15 +56,14 @@ namespace KDL {
 using namespace std;
 
 Trajectory* Trajectory::Read(std::istream& is) {
-	// auto_ptr because exception can be thrown !
 	IOTrace("Trajectory::Read");
 	char storage[64];
 	EatWord(is,"[",storage,sizeof(storage));
 	Eat(is,'[');
 	if (strcmp(storage,"SEGMENT")==0) {
 		IOTrace("SEGMENT");
-		auto_ptr<Path>      geom(    Path::Read(is)       );
-		auto_ptr<VelocityProfile> motprof( VelocityProfile::Read(is)  );
+		scoped_ptr<Path>      geom(    Path::Read(is)       );
+		scoped_ptr<VelocityProfile> motprof( VelocityProfile::Read(is)  );
 		EatEnd(is,']');
 		IOTracePop();
 		IOTracePop();
diff --git a/orocos_kdl/src/tree.cpp b/orocos_kdl/src/tree.cpp
index 1ca751489..37446052a 100644
--- a/orocos_kdl/src/tree.cpp
+++ b/orocos_kdl/src/tree.cpp
@@ -1,8 +1,8 @@
-// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 // Version: 1.0
-// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 // URL: http://www.orocos.org/kdl
 
 // This library is free software; you can redistribute it and/or
@@ -59,7 +59,7 @@ bool Tree::addSegment(const Segment& segment, const std::string& hook_name) {
         return false;
     pair<SegmentMap::iterator, bool> retval;
     //insert new element
-    unsigned int q_nr = segment.getJoint().getType() != Joint::None ? nrOfJoints : 0;
+    unsigned int q_nr = segment.getJoint().getType() != Joint::Fixed ? nrOfJoints : 0;
 
 #ifdef KDL_USE_NEW_TREE_INTERFACE
     retval = segments.insert(make_pair(segment.getName(), TreeElementType( new TreeElement(segment, parent, q_nr))));
@@ -75,7 +75,7 @@ bool Tree::addSegment(const Segment& segment, const std::string& hook_name) {
     //increase number of segments
     nrOfSegments++;
     //increase number of joints
-    if (segment.getJoint().getType() != Joint::None)
+    if (segment.getJoint().getType() != Joint::Fixed)
         nrOfJoints++;
     return true;
 }
diff --git a/orocos_kdl/src/treefksolver.hpp b/orocos_kdl/src/treefksolver.hpp
index 3d521d340..80ca1fc70 100644
--- a/orocos_kdl/src/treefksolver.hpp
+++ b/orocos_kdl/src/treefksolver.hpp
@@ -89,7 +89,7 @@ namespace KDL {
 //    class TreeFkSolverAcc {
 //   public:
         /**
-         * Calculate forward position, velocity and accelaration
+         * Calculate forward position, velocity and acceleration
          * kinematics, from joint coordinates to cartesian coordinates
          *
          * @param q_in input joint coordinates (position, velocity and
diff --git a/orocos_kdl/src/treeidsolver.hpp b/orocos_kdl/src/treeidsolver.hpp
new file mode 100644
index 000000000..1eef27547
--- /dev/null
+++ b/orocos_kdl/src/treeidsolver.hpp
@@ -0,0 +1,62 @@
+// Copyright  (C)  2009  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Franco Fusco <franco dot fusco at ls2n dot fr>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_TREE_IDSOLVER_HPP
+#define KDL_TREE_IDSOLVER_HPP
+
+#include "tree.hpp"
+#include "frames.hpp"
+#include "jntarray.hpp"
+#include "solveri.hpp"
+
+namespace KDL
+{
+
+  typedef std::map<std::string,Wrench> WrenchMap;
+
+	/**
+	 * \brief This <strong>abstract</strong> class encapsulates the inverse
+	 * dynamics solver for a KDL::Tree.
+	 *
+	 */
+	class TreeIdSolver : public KDL::SolverI
+	{
+		public:
+			/**
+			 * Calculate inverse dynamics, from joint positions, velocity, acceleration, external forces
+			 * to joint torques/forces.
+			 *
+			 * @param q input joint positions
+			 * @param q_dot input joint velocities
+			 * @param q_dotdot input joint accelerations
+			 * @param f_ext the external forces (no gravity) on the segments
+			 * @param torque output joint torques
+			 *
+			 * @return if < 0 something went wrong
+			 */
+        virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext,JntArray &torques)=0;
+
+        // Need functions to return the manipulator mass, coriolis and gravity matrices - Lagrangian Formulation.
+	};
+
+}
+
+#endif
diff --git a/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp b/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp
new file mode 100644
index 000000000..c4cf3fe76
--- /dev/null
+++ b/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp
@@ -0,0 +1,140 @@
+// Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
+
+// Version: 1.0
+// Author: Franco Fusco <franco dot fusco at ls2n dot fr>
+// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
+
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "treeidsolver_recursive_newton_euler.hpp"
+#include "frames_io.hpp"
+#include <stdexcept>
+
+namespace KDL{
+
+    TreeIdSolver_RNE::TreeIdSolver_RNE(const Tree& tree_, Vector grav):
+        tree(tree_), nj(tree.getNrOfJoints()), ns(tree.getNrOfSegments())
+    {
+      ag=-Twist(grav,Vector::Zero());
+      initAuxVariables();
+    }
+
+    void TreeIdSolver_RNE::updateInternalDataStructures() {
+      nj = tree.getNrOfJoints();
+      ns = tree.getNrOfSegments();
+      initAuxVariables();
+    }
+
+    void TreeIdSolver_RNE::initAuxVariables() {
+      const SegmentMap& segments = tree.getSegments();
+      for(SegmentMap::const_iterator seg = segments.begin(); seg != segments.end(); seg++) {
+        X[seg->first] = Frame();
+        S[seg->first] = Twist();
+        v[seg->first] = Twist();
+        a[seg->first] = Twist();
+        f[seg->first] = Wrench();
+      }
+    }
+
+    int TreeIdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray &torques)
+    {
+      //Check that the tree was not modified externally
+      if(nj != tree.getNrOfJoints() || ns != tree.getNrOfSegments())
+        return (error = E_NOT_UP_TO_DATE);
+
+      //Check sizes of joint vectors
+      if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj)
+        return (error = E_SIZE_MISMATCH);
+
+      try {
+        //Do the recursion here
+        rne_step(tree.getRootSegment(), q, q_dot, q_dotdot, f_ext, torques);
+      }
+      catch(const std::out_of_range&) {
+        //If in rne_step we get an out_of_range exception it means that some call
+        //to map::at failed. This can happen only if updateInternalDataStructures
+        //was not called after changing something in the tree. Note that it
+        //should be impossible to reach this point as consistency of the tree is
+        //checked above.
+        return (error = E_NOT_UP_TO_DATE);
+      }
+      return (error = E_NOERROR);
+    }
+
+
+    void TreeIdSolver_RNE::rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray& torques) {
+      const Segment& seg = GetTreeElementSegment(segment->second);
+      const std::string& segname = segment->first;
+      const std::string& parname = GetTreeElementParent(segment->second)->first;
+
+      //Do forward calculations involving velocity & acceleration of this segment
+      double q_, qdot_, qdotdot_;
+      unsigned int j = GetTreeElementQNr(segment->second);
+      if(seg.getJoint().getType()!=Joint::Fixed) {
+        q_ = q(j);
+        qdot_ = q_dot(j);
+        qdotdot_ = q_dotdot(j);
+      }
+      else
+        q_ = qdot_ = qdotdot_ = 0.0;
+
+      //Calculate segment properties: X,S,vj,cj
+
+      //Remark this is the inverse of the frame for transformations from the parent to the current coord frame
+      X.at(segname) = seg.pose(q_);
+
+      //Transform velocity and unit velocity to segment frame
+      Twist vj = X.at(segname).M.Inverse( seg.twist(q_,qdot_) );
+      S.at(segname) = X.at(segname).M.Inverse( seg.twist(q_,1.0) );
+
+      //calculate velocity and acceleration of the segment (in segment coordinates)
+      if(segment == tree.getRootSegment()) {
+        v.at(segname) = vj;
+        a.at(segname) = X.at(segname).Inverse(ag) + S.at(segname)*qdotdot_+ v.at(segname)*vj;
+      }
+      else {
+        v.at(segname) = X.at(segname).Inverse(v.at(parname)) + vj;
+        a.at(segname) = X.at(segname).Inverse(a.at(parname)) + S.at(segname)*qdotdot_ + v.at(segname)*vj;
+      }
+
+      //Calculate the force for the joint
+      //Collect RigidBodyInertia and external forces
+      const RigidBodyInertia& I = seg.getInertia();
+      f.at(segname) = I*a.at(segname) + v.at(segname)*(I*v.at(segname));
+      if(f_ext.find(segname) != f_ext.end())
+        f.at(segname) = f.at(segname) - f_ext.at(segname);
+
+      //propagate calculations over each child segment
+      SegmentMap::const_iterator child;
+      for (unsigned int i = 0; i < GetTreeElementChildren(segment->second).size(); i++) {
+        child = GetTreeElementChildren(segment->second)[i];
+        rne_step(child, q, q_dot, q_dotdot, f_ext, torques);
+      }
+
+      //do backward calculations involving wrenches and joint efforts
+
+      //If there is a moving joint, evaluate its effort
+      if(seg.getJoint().getType()!=Joint::Fixed) {
+        torques(j) = dot(S.at(segname), f.at(segname));
+        torques(j) += seg.getJoint().getInertia()*q_dotdot(j);  // add torque from joint inertia
+      }
+
+      //add reaction forces to parent segment
+      if(segment != tree.getRootSegment())
+        f.at(parname) = f.at(parname) + X.at(segname)*f.at(segname);
+    }
+}//namespace
diff --git a/orocos_kdl/src/treeidsolver_recursive_newton_euler.hpp b/orocos_kdl/src/treeidsolver_recursive_newton_euler.hpp
new file mode 100644
index 000000000..95af77a01
--- /dev/null
+++ b/orocos_kdl/src/treeidsolver_recursive_newton_euler.hpp
@@ -0,0 +1,84 @@
+// Copyright  (C)  2009  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Franco Fusco <franco dot fusco at ls2n dot fr>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_TREE_IDSOLVER_RECURSIVE_NEWTON_EULER_HPP
+#define KDL_TREE_IDSOLVER_RECURSIVE_NEWTON_EULER_HPP
+
+#include "treeidsolver.hpp"
+
+namespace KDL{
+    /**
+     * \brief Recursive newton euler inverse dynamics solver for kinematic trees.
+     *
+     * It calculates the torques for the joints, given the motion of
+     * the joints (q,qdot,qdotdot), external forces on the segments
+     * (expressed in the segments reference frame) and the dynamical
+     * parameters of the segments.
+     *
+     * This is an extension of the inverse dynamic solver for kinematic chains,
+     * \see ChainIdSolver_RNE. The main difference is the use of STL maps
+     * instead of vectors to represent external wrenches (as well as internal
+     * variables exploited during the recursion).
+     */
+    class TreeIdSolver_RNE : public TreeIdSolver {
+    public:
+        /**
+         * Constructor for the solver, it will allocate all the necessary memory
+         * \param tree The kinematic tree to calculate the inverse dynamics for, an internal reference will be stored.
+         * \param grav The gravity vector to use during the calculation.
+         */
+        TreeIdSolver_RNE(const Tree& tree, Vector grav);
+
+        /**
+         * Function to calculate from Cartesian forces to joint torques.
+         * Input parameters;
+         * \param q The current joint positions
+         * \param q_dot The current joint velocities
+         * \param q_dotdot The current joint accelerations
+         * \param f_ext The external forces (no gravity) on the segments
+         * Output parameters:
+         * \param torques the resulting torques for the joints
+         */
+        int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray &torques);
+
+        /// @copydoc KDL::SolverI::updateInternalDataStructures
+        virtual void updateInternalDataStructures();
+
+    private:
+        ///Helper function to initialize private members X, S, v, a, f
+        void initAuxVariables();
+
+        ///One recursion step
+        void rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray& torques);
+
+        const Tree& tree;
+        unsigned int nj;
+        unsigned int ns;
+        std::map<std::string,Frame> X;
+        std::map<std::string,Twist> S;
+        std::map<std::string,Twist> v;
+        std::map<std::string,Twist> a;
+        std::map<std::string,Wrench> f;
+        Twist ag;
+    };
+}
+
+#endif
diff --git a/orocos_kdl/src/treejnttojacsolver.cpp b/orocos_kdl/src/treejnttojacsolver.cpp
index 9b5ff856a..0afb4d1c5 100644
--- a/orocos_kdl/src/treejnttojacsolver.cpp
+++ b/orocos_kdl/src/treejnttojacsolver.cpp
@@ -47,7 +47,7 @@ int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, const std:
         T_total = T_local * T_total;
         
         //get the twist of the segment:
-        if (GetTreeElementSegment(it->second).getJoint().getType() != Joint::None) {
+        if (GetTreeElementSegment(it->second).getJoint().getType() != Joint::Fixed) {
             Twist t_local = GetTreeElementSegment(it->second).twist(q_in(q_nr), 1.0);
             //transform the endpoint of the local twist to the global endpoint:
             t_local = t_local.RefPoint(T_total.p - T_local.p);
diff --git a/orocos_kdl/src/utilities/error_stack.cxx b/orocos_kdl/src/utilities/error_stack.cxx
index 864f333c0..78c5027a9 100644
--- a/orocos_kdl/src/utilities/error_stack.cxx
+++ b/orocos_kdl/src/utilities/error_stack.cxx
@@ -23,7 +23,7 @@
 namespace KDL {
 
 // Trace of the call stack of the I/O routines to help user
-// interprete error messages from I/O
+// interpret error messages from I/O
 typedef std::stack<std::string>  ErrorStack;
 
 // should be in Thread Local Storage if this gets multithreaded one day...
diff --git a/orocos_kdl/src/utilities/ldl_solver_eigen.cpp b/orocos_kdl/src/utilities/ldl_solver_eigen.cpp
new file mode 100644
index 000000000..8466c5ffa
--- /dev/null
+++ b/orocos_kdl/src/utilities/ldl_solver_eigen.cpp
@@ -0,0 +1,82 @@
+// Copyright  (C)  2018  Craig Carignan <craigc at ssl dot umd dot edu>
+
+// Version: 1.0
+// Author: Craig Carignan
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "ldl_solver_eigen.hpp"
+
+namespace KDL{
+
+    int ldl_solver_eigen(const Eigen::MatrixXd& A, const Eigen::VectorXd& v, Eigen::MatrixXd& L, Eigen::VectorXd& D, Eigen::VectorXd& vtmp, Eigen::VectorXd& q)
+    {
+        const int n = A.rows();
+        int error=SolverI::E_NOERROR;
+
+        //Check sizes
+        if(A.cols()!=n || v.rows()!=n || L.rows()!=n || L.cols()!=n || D.rows()!=n || vtmp.rows()!=n || q.rows()!=n)
+            return (error = SolverI::E_SIZE_MISMATCH);
+
+        for(int i=0;i<n;++i)  {
+            D(i)=A(i,i);
+            if(i>0) {
+                for(int j=0;j<=i-1;++j)
+                    D(i) -= D(j)*L(i,j)*L(i,j);
+            }
+            for(int j=1;j<n;++j)  {
+                if(j>i)  {
+                    L(j,i)=A(i,j)/D(i);
+                    if(i>0)  {
+                        for(int k=0;k<=i-1;++k)
+                            L(j,i) -= L(j,k)*L(i,k)*D(k)/D(i);
+                    }
+                }
+            }
+        }
+        for(int i=0;i<n;++i)  {
+            vtmp(i)=v(i);
+            if(i>0) {
+                for(int j=0;j<=i-1;++j)
+                    vtmp(i) -= L(i,j)*vtmp(j);
+            }
+        }
+        for(int i=n-1;i>=0;--i) {
+            q(i)=vtmp(i)/D(i);
+            if(i<n-1)  {
+                for(int j=i+1;j<n;++j)
+                    q(i) -= L(j,i)*q(j);
+            }
+        }
+        // optional: changes diagonal elements of L to 1 as per LDL decomposition
+        //           A = L * Ddiag * L^T where Ddiag(i,i) = D(i)
+        for(int i=0;i<n;++i)  {
+            L(i,i) = 1.;
+        }
+        // optional: sets upper triangular, off-diagonal elements of L to 0
+        //           because algorithm does not do this automatically
+        if ( n > 1 )
+        {
+            for(int i=0;i<n;++i)  {
+                for(int j=i+1;j<n;++j)  {
+                    L(i,j) = 0.0;
+                }
+            }
+        }
+        return(error);
+    }
+}
diff --git a/orocos_kdl/src/utilities/ldl_solver_eigen.hpp b/orocos_kdl/src/utilities/ldl_solver_eigen.hpp
new file mode 100644
index 000000000..c4778f021
--- /dev/null
+++ b/orocos_kdl/src/utilities/ldl_solver_eigen.hpp
@@ -0,0 +1,59 @@
+// Copyright  (C)  2018  Craig Carignan <craigc at ssl dot umd dot edu>
+
+// Version: 1.0
+// Author: Craig Carignan
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+
+// Inverse of a positive definite symmetric matrix times a vector
+// based on LDL^T Decomposition
+#ifndef LDL_SOLVER_EIGEN_HPP
+#define LDL_SOLVER_EIGEN_HPP
+
+
+#include <Eigen/Core>
+#include "../solveri.hpp"
+
+using namespace Eigen;
+
+namespace KDL
+{
+    /**
+     * \brief Solves the system of equations Aq = v for q via LDL decomposition,
+     *        where A is a square positive definite matrix
+     *
+     * The algorithm factor A into the product of three matrices LDL^T, where L
+     * is a lower triangular matrix and D is a diagonal matrix.  This allows q
+     * to be computed without explicity inverting A.  Note that the LDL decomposition
+     * is a variant of the classical Cholesky Decomposition that does not require
+     * the computation of square roots.
+     * Input parameters:
+     * @param A matrix<double>(nxn)
+     * @param v vector<double> n
+     * @param vtmp vector<double> n [temp variable]
+     * Output parameters:
+     * @param L matrix<double>(nxn)
+     * @param D vector<double> n
+     * @param q vector<double> n
+     * @return 0 if successful, E_SIZE_MISMATCH if dimensions do not match
+     * References:
+     * https://en.wikipedia.org/wiki/Cholesky_decomposition
+     */
+    int ldl_solver_eigen(const Eigen::MatrixXd& A, const Eigen::VectorXd& v, Eigen::MatrixXd& L, Eigen::VectorXd& D, Eigen::VectorXd& vtmp, Eigen::VectorXd& q);
+}
+#endif
diff --git a/orocos_kdl/src/utilities/rall1d.h b/orocos_kdl/src/utilities/rall1d.h
index 4b3ba1ac1..812c69fe3 100644
--- a/orocos_kdl/src/utilities/rall1d.h
+++ b/orocos_kdl/src/utilities/rall1d.h
@@ -56,7 +56,7 @@ class Rall1d
         T t;        //!< value
         V grad;     //!< gradient
     public :
-        INLINE Rall1d() {}
+        INLINE Rall1d():t(),grad() {};
 
         T value() const {
             return t;
@@ -471,6 +471,22 @@ INLINE  bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps=epsi
     return (Equal(x.t,y.t,eps)&&Equal(x.grad,y.grad,eps));
 }
 
+template <class T,class V,class S>
+INLINE  bool operator==(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
+{
+#ifdef KDL_USE_EQUAL
+    return Equal(y, x);
+#else
+    return (x.t == y.t && x.grad == y.grad);
+#endif
+}
+
+template <class T,class V,class S>
+INLINE  bool operator!=(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
+{
+    return !operator==(y, x);
+}
+
 }
 
 
diff --git a/orocos_kdl/src/utilities/rall1d_io.h b/orocos_kdl/src/utilities/rall1d_io.h
index d69b26177..ac2115904 100644
--- a/orocos_kdl/src/utilities/rall1d_io.h
+++ b/orocos_kdl/src/utilities/rall1d_io.h
@@ -18,6 +18,7 @@
 #ifndef Rall_IO_H
 #define Rall_IO_H
 
+#include <typeinfo>
 #include "utility_io.h"
 #include "rall1d.h"
 
@@ -26,7 +27,7 @@ namespace KDL {
 template <class T,class V,class S>
 inline std::ostream& operator << (std::ostream& os,const Rall1d<T,V,S>& r)
             {
-            os << "Rall1d(" << r.t <<"," << r.grad <<")";
+            os << "Rall1d<" << typeid(T).name() << ", "<< typeid(V).name() << ", " << typeid(S).name() << ">(" << r.t <<"," << r.grad <<")";
             return os;
             }
 
diff --git a/orocos_kdl/src/utilities/rall2d.h b/orocos_kdl/src/utilities/rall2d.h
index 811bad553..fd6f6f965 100644
--- a/orocos_kdl/src/utilities/rall2d.h
+++ b/orocos_kdl/src/utilities/rall2d.h
@@ -59,7 +59,7 @@ class Rall2d
         V dd;   //!< 2nd derivative
     public :
         // = Constructors
-        INLINE Rall2d() {}
+        INLINE Rall2d():t(),d(),dd() {};
 
         explicit INLINE  Rall2d(typename TI<T>::Arg c)
             {t=c;SetToZero(d);SetToZero(dd);}
@@ -531,6 +531,24 @@ INLINE  bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps=epsi
             );
 }
 
+template <class T,class V,class S>
+INLINE  bool operator==(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
+{
+#ifdef KDL_USE_EQUAL
+    return Equal(y, x);
+#else
+    return (x.t == y.t &&
+            x.d == y.d &&
+            x.dd == y.dd);
+#endif
+
+}
+
+template <class T,class V,class S>
+INLINE  bool operator!=(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
+{
+    return !operator==(y, x);
+}
 
 }
 
diff --git a/orocos_kdl/src/utilities/rall2d_io.h b/orocos_kdl/src/utilities/rall2d_io.h
index 8870243a5..5ea145cd2 100644
--- a/orocos_kdl/src/utilities/rall2d_io.h
+++ b/orocos_kdl/src/utilities/rall2d_io.h
@@ -19,7 +19,7 @@
 #define Rall2d_IO_H
 
 
-
+#include <typeinfo>
 #include "utility_io.h"
 #include "rall2d.h"
 
@@ -28,7 +28,7 @@ namespace KDL {
 template <class T,class V,class S>
 std::ostream& operator << (std::ostream& os,const Rall2d<T,V,S>& r)
             {
-            os << "Rall2d(" << r.t <<"," << r.d <<","<<r.dd<<")";
+            os << "Rall2d<" << typeid(T).name() << ", "<< typeid(V).name() << ", " << typeid(S).name() << ">(" << r.t <<"," << r.d <<","<<r.dd<<")";
             return os;
             }
 
diff --git a/orocos_kdl/src/utilities/scoped_ptr.hpp b/orocos_kdl/src/utilities/scoped_ptr.hpp
new file mode 100644
index 000000000..a7f6f73d4
--- /dev/null
+++ b/orocos_kdl/src/utilities/scoped_ptr.hpp
@@ -0,0 +1,78 @@
+/***************************************************************************
+    scoped_ptr - a not too smart pointer for exception safe heap objects
+                       -------------------------
+    begin                : May 2019
+    copyright            : (C) 2019 Intermodalics
+ 
+ ***************************************************************************
+ *   This library is free software; you can redistribute it and/or         *
+ *   modify it under the terms of the GNU Lesser General Public            *
+ *   License as published by the Free Software Foundation; either          *
+ *   version 2.1 of the License, or (at your option) any later version.    *
+ *                                                                         *
+ *   This library is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
+ *   Lesser General Public License for more details.                       *
+ *                                                                         *
+ *   You should have received a copy of the GNU Lesser General Public      *
+ *   License along with this library; if not, write to the Free Software   *
+ *   Foundation, Inc., 59 Temple Place,                                    *
+ *   Suite 330, Boston, MA  02111-1307  USA                                *
+ *                                                                         *
+ ***************************************************************************/
+
+#if (__cplusplus > 199711L)
+#include <utility>
+#else
+#include <algorithm>
+#endif
+
+namespace KDL {
+
+template<typename T> class scoped_ptr;
+
+template<typename T> void swap(scoped_ptr<T>&, scoped_ptr<T>&);
+
+template<typename T>
+class scoped_ptr {
+ public:
+  scoped_ptr() : ptr_(0) { }
+  explicit scoped_ptr(T* p) : ptr_(p) { }
+
+  ~scoped_ptr() { delete ptr_; }
+
+  T* operator->() { return ptr_; }
+  const T* operator->() const { return ptr_; }
+
+  T* get() const { return ptr_; }
+
+  void reset(T* p = 0) {
+    T* old = ptr_;
+    ptr_ = p;
+    delete old;
+  }
+
+  T* release() {
+    T* old = ptr_;
+    ptr_ = 0;
+    return old;
+  }
+
+  friend void swap<>(scoped_ptr<T>& a, scoped_ptr<T>& b);
+
+ private:
+  scoped_ptr(const scoped_ptr&);  // not-copyable
+  scoped_ptr& operator=(const scoped_ptr&); // not-copyable
+
+  T* ptr_;
+};
+
+template<typename T>
+void swap(scoped_ptr<T>& a, scoped_ptr<T>& b) {
+  using std::swap;
+  swap(a.ptr_, b.ptr_);
+}
+
+
+} // namespace KDL
diff --git a/orocos_kdl/src/utilities/svd_HH.cpp b/orocos_kdl/src/utilities/svd_HH.cpp
index 296565b30..b82467cd0 100644
--- a/orocos_kdl/src/utilities/svd_HH.cpp
+++ b/orocos_kdl/src/utilities/svd_HH.cpp
@@ -130,7 +130,7 @@ namespace KDL
             }
             maxarg1=anorm;
             maxarg2=(fabs(w(i))+fabs(tmp(i)));
-            anorm = maxarg1 > maxarg2 ?	maxarg1 : maxarg2;
+            anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
         }
         /* Accumulation of right-hand transformations. */
         for (i=cols-1;i>=0;i--) {
diff --git a/orocos_kdl/src/utilities/svd_eigen_HH.cpp b/orocos_kdl/src/utilities/svd_eigen_HH.cpp
index 36da50914..46ce575dc 100644
--- a/orocos_kdl/src/utilities/svd_eigen_HH.cpp
+++ b/orocos_kdl/src/utilities/svd_eigen_HH.cpp
@@ -53,14 +53,14 @@ namespace KDL{
                         s += U(k,i)*U(k,i);
                     }
                     f=U(i,i);  // f is the diag elem
-					if (!(s>=0)) return -3;
+                    if (!(s>=0)) return -3;
                     g = -SIGN(sqrt(s),f);
                     h=f*g-s;
                     U(i,i)=f-g;
                     for (j=ppi;j<cols;j++) {
                         // dot product of columns i and j, starting from the i-th row
                         for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j);
-						if (!(h!=0)) return -4;
+                        if (!(h!=0)) return -4;
                         f=s/h;
                         // copy the scaled i-th column into the j-th column
                         for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
@@ -80,11 +80,11 @@ namespace KDL{
                         s += U(i,k)*U(i,k);
                     }
                     f=U(i,ppi);
-					if (!(s>=0)) return -5;
+                    if (!(s>=0)) return -5;
                     g = -SIGN(sqrt(s),f);
                     h=f*g-s;
                     U(i,ppi)=f-g;
-					if (!(h!=0)) return -6;
+                    if (!(h!=0)) return -6;
                     for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h;
                     for (j=ppi;j<rows;j++) {
                         for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k);
@@ -95,13 +95,13 @@ namespace KDL{
             }
             maxarg1=anorm;
             maxarg2=(fabs(S(i))+fabs(tmp(i)));
-            anorm = maxarg1 > maxarg2 ?	maxarg1 : maxarg2;		
+            anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
         }
         /* Accumulation of right-hand transformations. */
         for (i=cols-1;i>=0;i--) {
             if (i<cols-1) {
                 if (fabs(g)>epsilon) {
-					if (!(U(i,ppi)!=0)) return -7;
+                    if (!(U(i,ppi)!=0)) return -7;
                     for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
                     for (j=ppi;j<cols;j++) {
                         for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j);
@@ -123,7 +123,7 @@ namespace KDL{
                 g=1.0/g;
                 for (j=ppi;j<cols;j++) {
                     for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j);
-					if (!(U(i,i)!=0)) return -8;
+                    if (!(U(i,i)!=0)) return -8;
                     f=(s/U(i,i))*g;
                     for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
                 }
@@ -156,7 +156,7 @@ namespace KDL{
                         g=S(i);
                         h=PYTHAG(f,g);
                         S(i)=h;
-						if (!(h!=0)) return -9;
+                        if (!(h!=0)) return -9;
                         h=1.0/h;
                         c=g*h;
                         s=(-f*h);
@@ -183,12 +183,12 @@ namespace KDL{
                 y=S(nm);
                 g=tmp(nm);
                 h=tmp(k);
-				if (!(h!=0&&y!=0)) return -10;
+                if (!(h!=0&&y!=0)) return -10;
                 f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
                 
                 g=PYTHAG(f,1.0);
-				if (!(x!=0)) return -11;
-				if (!((f+SIGN(g,f))!=0)) return -12;
+                if (!(x!=0)) return -11;
+                if (!((f+SIGN(g,f))!=0)) return -12;
                 f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
                 
                 /* Next QR transformation: */
@@ -201,7 +201,7 @@ namespace KDL{
                     g=c*g;
                     z=PYTHAG(f,h);
                     tmp(j)=z;
-					if (!(z!=0)) return -13;
+                    if (!(z!=0)) return -13;
                     c=f/z;
                     s=h/z;
                     f=x*c+g*s;
diff --git a/orocos_kdl/src/utilities/svd_eigen_Macie.cpp b/orocos_kdl/src/utilities/svd_eigen_Macie.cpp
new file mode 100644
index 000000000..b4d1c63ee
--- /dev/null
+++ b/orocos_kdl/src/utilities/svd_eigen_Macie.cpp
@@ -0,0 +1,162 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "svd_eigen_Macie.hpp"
+
+namespace KDL{
+    int svd_eigen_Macie(const Eigen::MatrixXd& A,Eigen::MatrixXd& U,Eigen::VectorXd& S, Eigen::MatrixXd& V,
+                        Eigen::MatrixXd& B, Eigen::VectorXd& tempi,
+                        double threshold,bool toggle)
+    {
+        bool rotate = true;
+        unsigned int sweeps=0;
+        unsigned int rotations=0;
+        if(toggle){
+            //Calculate B from new A and previous V
+            B=A.lazyProduct(V);
+            while(rotate){
+                rotate=false;
+                rotations=0;
+                //Perform rotations between columns of B
+                for(unsigned int i=0;i<B.cols();i++){
+                    for(unsigned int j=i+1;j<B.cols();j++){
+                        //calculate plane rotation
+                        double p = B.col(i).dot(B.col(j));
+                        double qi =B.col(i).dot(B.col(i));
+                        double qj = B.col(j).dot(B.col(j));
+                        double q=qi-qj;
+                        double alpha = pow(p,2.0)/(qi*qj);
+                        //if columns are orthogonal with precision
+                        //threshold, don't perform rotation and continue
+                        if(alpha<threshold)
+                            continue;
+                        rotations++;
+                        double c = sqrt(4*pow(p,2.0)+pow(q,2.0));
+                        double cos,sin;
+                        if(q>=0){
+                            cos=sqrt((c+q)/(2*c));
+                            sin=p/(c*cos);
+                        }else{
+                            if(p<0)
+                                sin=-sqrt((c-q)/(2*c));
+                            else
+                                sin=sqrt((c-q)/(2*c));
+                            cos=p/(c*sin);
+                        }
+
+                        //Apply plane rotation to columns of B
+                        tempi = cos*B.col(i) + sin*B.col(j);
+                        B.col(j) = - sin*B.col(i) + cos*B.col(j);
+                        B.col(i) = tempi;
+                        
+                        //Apply plane rotation to columns of V
+                        tempi.head(V.rows()) = cos*V.col(i) + sin*V.col(j);
+                        V.col(j) = - sin*V.col(i) + cos*V.col(j);
+                        V.col(i) = tempi.head(V.rows());
+
+                        rotate=true;
+                    }
+                }
+                //Only calculate new U and S if there were any rotations
+                if(rotations!=0){
+                    for(unsigned int i=0;i<U.rows();i++) {
+                        if(i<B.cols()){
+                            double si=sqrt(B.col(i).dot(B.col(i)));
+                            if(si==0)
+                                U.col(i) = B.col(i);
+                            else
+                                U.col(i) = B.col(i)/si;
+                            S(i)=si;
+                        }
+                        else
+                            U.col(i) = 0*tempi;
+                    }
+                    sweeps++;
+                }
+            }
+            return sweeps;
+        }else{
+            //Calculate B from new A and previous U'
+            B = U.transpose().lazyProduct(A);
+            while(rotate){
+                rotate=false;
+                rotations=0;
+                //Perform rotations between rows of B
+                for(unsigned int i=0;i<B.cols();i++){
+                    for(unsigned int j=i+1;j<B.cols();j++){
+                        //calculate plane rotation
+                        double p = B.row(i).dot(B.row(j));
+                        double qi = B.row(i).dot(B.row(i));
+                        double qj = B.row(j).dot(B.row(j));
+
+                        double q=qi-qj;
+                        double alpha = pow(p,2.0)/(qi*qj);
+                        //if columns are orthogonal with precision
+                        //threshold, don't perform rotation and
+                        //continue
+                        if(alpha<threshold)
+                            continue;
+                        rotations++;
+                        double c = sqrt(4*pow(p,2.0)+pow(q,2.0));
+                        double cos,sin;
+                        if(q>=0){
+                            cos=sqrt((c+q)/(2*c));
+                            sin=p/(c*cos);
+                        }else{
+                            if(p<0)
+                                sin=-sqrt((c-q)/(2*c));
+                            else
+                                sin=sqrt((c-q)/(2*c));
+                            cos=p/(c*sin);
+                        }
+
+                        //Apply plane rotation to rows of B
+                        tempi.head(B.cols()) =  cos*B.row(i) + sin*B.row(j);
+                        B.row(j) =  - sin*B.row(i) + cos*B.row(j);
+                        B.row(i) =  tempi.head(B.cols());
+
+
+                        //Apply plane rotation to rows of U
+                        tempi.head(U.rows()) = cos*U.col(i) + sin*U.col(j);
+                        U.col(j) = - sin*U.col(i) + cos*U.col(j);
+                        U.col(i) = tempi.head(U.rows());
+
+                        rotate=true;
+                    }
+                }
+
+                //Only calculate new U and S if there were any rotations
+                if(rotations!=0){
+                    for(unsigned int i=0;i<V.rows();i++) {
+                        double si=sqrt(B.row(i).dot(B.row(i)));
+                        if(si==0)
+                            V.col(i) = B.row(i);
+                        else
+                            V.col(i) = B.row(i)/si;
+                        S(i)=si;
+                    }
+                    sweeps++;
+                }
+            }
+            return sweeps;
+        }
+    }
+} // namespace KDL
diff --git a/orocos_kdl/src/utilities/svd_eigen_Macie.hpp b/orocos_kdl/src/utilities/svd_eigen_Macie.hpp
index e868287d4..2f6263de6 100644
--- a/orocos_kdl/src/utilities/svd_eigen_Macie.hpp
+++ b/orocos_kdl/src/utilities/svd_eigen_Macie.hpp
@@ -32,7 +32,6 @@
 #define SVD_EIGEN_MACIE
 
 #include <Eigen/Core>
-using namespace Eigen;
 
 
 namespace KDL
@@ -56,143 +55,9 @@ namespace KDL
 	 * \param toggle [INPUT] toggle this boolean variable on each call of this routine.
 	 * \return number of sweeps.
 	 */
-    int svd_eigen_Macie(const MatrixXd& A,MatrixXd& U,VectorXd& S, MatrixXd& V,
-                        MatrixXd& B, VectorXd& tempi,
-                        double threshold,bool toggle)
-    {
-        bool rotate = true;
-        unsigned int sweeps=0;
-        unsigned int rotations=0;
-        if(toggle){
-            //Calculate B from new A and previous V
-            B=A.lazyProduct(V);
-            while(rotate){
-                rotate=false;
-                rotations=0;
-                //Perform rotations between columns of B
-                for(unsigned int i=0;i<B.cols();i++){
-                    for(unsigned int j=i+1;j<B.cols();j++){
-                        //calculate plane rotation
-                        double p = B.col(i).dot(B.col(j));
-                        double qi =B.col(i).dot(B.col(i));
-                        double qj = B.col(j).dot(B.col(j));
-                        double q=qi-qj;
-                        double alpha = pow(p,2.0)/(qi*qj);
-                        //if columns are orthogonal with precision
-                        //threshold, don't perform rotation and continue
-                        if(alpha<threshold)
-                            continue;
-                        rotations++;
-                        double c = sqrt(4*pow(p,2.0)+pow(q,2.0));
-                        double cos,sin;
-                        if(q>=0){
-                            cos=sqrt((c+q)/(2*c));
-                            sin=p/(c*cos);
-                        }else{
-                            if(p<0)
-                                sin=-sqrt((c-q)/(2*c));
-                            else
-                                sin=sqrt((c-q)/(2*c));
-                            cos=p/(c*sin);
-                        }
-
-                        //Apply plane rotation to columns of B
-                        tempi = cos*B.col(i) + sin*B.col(j);
-                        B.col(j) = - sin*B.col(i) + cos*B.col(j);
-                        B.col(i) = tempi;
-                        
-                        //Apply plane rotation to columns of V
-                        tempi.head(V.rows()) = cos*V.col(i) + sin*V.col(j);
-                        V.col(j) = - sin*V.col(i) + cos*V.col(j);
-                        V.col(i) = tempi.head(V.rows());
-
-                        rotate=true;
-                    }
-                }
-                //Only calculate new U and S if there were any rotations
-                if(rotations!=0){
-                    for(unsigned int i=0;i<U.rows();i++) {
-                        if(i<B.cols()){
-                            double si=sqrt(B.col(i).dot(B.col(i)));
-                            if(si==0)
-                                U.col(i) = B.col(i);
-                            else
-                                U.col(i) = B.col(i)/si;
-                            S(i)=si;
-                        }
-                        else
-                            U.col(i) = 0*tempi;
-                    }
-                    sweeps++;
-                }
-            }
-            return sweeps;
-        }else{
-            //Calculate B from new A and previous U'
-            B = U.transpose().lazyProduct(A);
-            while(rotate){
-                rotate=false;
-                rotations=0;
-                //Perform rotations between rows of B
-                for(unsigned int i=0;i<B.cols();i++){
-                    for(unsigned int j=i+1;j<B.cols();j++){
-                        //calculate plane rotation
-                        double p = B.row(i).dot(B.row(j));
-                        double qi = B.row(i).dot(B.row(i));
-                        double qj = B.row(j).dot(B.row(j));
-
-                        double q=qi-qj;
-                        double alpha = pow(p,2.0)/(qi*qj);
-                        //if columns are orthogonal with precision
-                        //threshold, don't perform rotation and
-                        //continue
-                        if(alpha<threshold)
-                            continue;
-                        rotations++;
-                        double c = sqrt(4*pow(p,2.0)+pow(q,2.0));
-                        double cos,sin;
-                        if(q>=0){
-                            cos=sqrt((c+q)/(2*c));
-                            sin=p/(c*cos);
-                        }else{
-                            if(p<0)
-                                sin=-sqrt((c-q)/(2*c));
-                            else
-                                sin=sqrt((c-q)/(2*c));
-                            cos=p/(c*sin);
-                        }
-
-                        //Apply plane rotation to rows of B
-                        tempi.head(B.cols()) =  cos*B.row(i) + sin*B.row(j);
-                        B.row(j) =  - sin*B.row(i) + cos*B.row(j);
-                        B.row(i) =  tempi.head(B.cols());
-
-
-                        //Apply plane rotation to rows of U
-                        tempi.head(U.rows()) = cos*U.col(i) + sin*U.col(j);
-                        U.col(j) = - sin*U.col(i) + cos*U.col(j);
-                        U.col(i) = tempi.head(U.rows());
-
-                        rotate=true;
-                    }
-                }
-
-                //Only calculate new U and S if there were any rotations
-                if(rotations!=0){
-                    for(unsigned int i=0;i<V.rows();i++) {
-                        double si=sqrt(B.row(i).dot(B.row(i)));
-                        if(si==0)
-                            V.col(i) = B.row(i);
-                        else
-                            V.col(i) = B.row(i)/si;
-                        S(i)=si;
-                    }
-                    sweeps++;
-                }
-            }
-            return sweeps;
-        }
-    }
+    int svd_eigen_Macie(const Eigen::MatrixXd& A,Eigen::MatrixXd& U,Eigen::VectorXd& S, Eigen::MatrixXd& V,
+                        Eigen::MatrixXd& B, Eigen::VectorXd& tempi,
+                        double threshold,bool toggle);
 
 
 }
diff --git a/orocos_kdl/src/utilities/utility.cxx b/orocos_kdl/src/utilities/utility.cxx
index ad995a047..7cc7e9663 100644
--- a/orocos_kdl/src/utilities/utility.cxx
+++ b/orocos_kdl/src/utilities/utility.cxx
@@ -11,10 +11,12 @@
 
 namespace KDL {
 
-int STREAMBUFFERSIZE=10000;
+int STREAMBUFFERSIZE = 10000;
 int MAXLENFILENAME = 255;
-const double PI=       3.1415926535897932384626433832795;
-const double deg2rad = 0.01745329251994329576923690768488;
-const double rad2deg = 57.2957795130823208767981548141052;
-double epsilon = 0.000001;
+const double PI      = 3.141592653589793238462643383279502884; // PI
+const double PI_2    = 1.570796326794896619231321691639751442; // PI/2
+const double PI_4    = 0.785398163397448309615660845819875721; // PI/4
+const double deg2rad = 0.017453292519943295769236907684886127; // PI/180
+const double rad2deg = 57.29577951308232087679815481410517033; // 180/PI
+double epsilon = 1e-6;
 }
diff --git a/orocos_kdl/src/utilities/utility.h b/orocos_kdl/src/utilities/utility.h
index 83266ed48..651dd7660 100644
--- a/orocos_kdl/src/utilities/utility.h
+++ b/orocos_kdl/src/utilities/utility.h
@@ -179,6 +179,12 @@ extern int          MAXLENFILENAME;
 //! the value of pi
 extern const double PI;
 
+//! the value of pi/2
+extern const double PI_2;
+
+//! the value of pi/4
+extern const double PI_4;
+
 //! the value pi/180
 extern const double deg2rad;
 
diff --git a/orocos_kdl/src/velocityprofile_dirac.cpp b/orocos_kdl/src/velocityprofile_dirac.cpp
index 9f4bc7639..23c915b59 100644
--- a/orocos_kdl/src/velocityprofile_dirac.cpp
+++ b/orocos_kdl/src/velocityprofile_dirac.cpp
@@ -42,7 +42,7 @@ namespace KDL {
     }
 
     void VelocityProfile_Dirac::
-	SetProfileDuration(double pos1,double pos2,double duration)
+    SetProfileDuration(double pos1,double pos2,double duration)
     {
         SetProfile(pos1,pos2);
         t = duration;
@@ -53,10 +53,15 @@ namespace KDL {
     }
 
     double VelocityProfile_Dirac::Pos(double time) const {
-        if ( t == 0 )
-            return time == 0 ? p1 : p2;
-        else
+        if ( t == 0 ) {
+            return time <= 0 ? p1 : p2;
+        } else if (time < 0) {
+            return p1;
+        } else if (time <= t) {
             return p1 + (( p2 - p1)/t)*time;
+        } else {
+            return p2;
+        }
     }
 
     double VelocityProfile_Dirac::Vel(double time) const {
diff --git a/orocos_kdl/src/velocityprofile_dirac.hpp b/orocos_kdl/src/velocityprofile_dirac.hpp
index ce06b6d44..07aa62b5f 100644
--- a/orocos_kdl/src/velocityprofile_dirac.hpp
+++ b/orocos_kdl/src/velocityprofile_dirac.hpp
@@ -33,37 +33,37 @@
 
 
 namespace KDL {
-	/**
-	 * A Dirac VelocityProfile generates an infinite velocity
-	 * so that the position jumps from A to B in in infinite short time.
-	 * In practice, this means that the maximum values are ignored and
-	 * for any t : Vel(t) == 0 and Acc(t) == 0.
-	 * Further Pos( -0 ) = pos1 and Pos( +0 ) = pos2.
-	 *
-	 * However, if a duration is given, it will create an unbound
-	 * rectangular velocity profile for that duration, otherwise,
-	 * Duration() == 0;
-	 * @ingroup Motion
-	 */
-	class VelocityProfile_Dirac : public VelocityProfile
-	{
-		double p1,p2,t;
-	public:
-		void SetProfile(double pos1,double pos2);
-		virtual void SetProfileDuration(double pos1,double pos2,double duration);
-		virtual double Duration() const;
-		virtual double Pos(double time) const;
-		virtual double Vel(double time) const;
-		virtual double Acc(double time) const;
-		virtual void Write(std::ostream& os) const;
-		virtual VelocityProfile* Clone() const {
-			VelocityProfile_Dirac* res =  new VelocityProfile_Dirac();
-			res->SetProfileDuration( p1, p2, t );
-			return res;
-		}
+    /**
+     * A Dirac VelocityProfile generates an infinite velocity
+     * so that the position jumps from A to B in in infinite short time.
+     * In practice, this means that the maximum values are ignored and
+     * for any t : Vel(t) == 0 and Acc(t) == 0.
+     * Further Pos( -0 ) = pos1 and Pos( +0 ) = pos2.
+     *
+     * However, if a duration is given, it will create an unbound
+     * rectangular velocity profile for that duration, otherwise,
+     * Duration() == 0;
+     * @ingroup Motion
+     */
+    class VelocityProfile_Dirac : public VelocityProfile
+    {
+        double p1,p2,t;
+    public:
+        void SetProfile(double pos1,double pos2);
+        virtual void SetProfileDuration(double pos1,double pos2,double duration);
+        virtual double Duration() const;
+        virtual double Pos(double time) const;
+        virtual double Vel(double time) const;
+        virtual double Acc(double time) const;
+        virtual void Write(std::ostream& os) const;
+        virtual VelocityProfile* Clone() const {
+            VelocityProfile_Dirac* res =  new VelocityProfile_Dirac();
+            res->SetProfileDuration( p1, p2, t );
+            return res;
+        }
 
-		virtual ~VelocityProfile_Dirac() {}
-	};
+        virtual ~VelocityProfile_Dirac() {}
+    };
 
 }
 
diff --git a/orocos_kdl/src/velocityprofile_spline.hpp b/orocos_kdl/src/velocityprofile_spline.hpp
index 0ce9ea896..3d17abdba 100644
--- a/orocos_kdl/src/velocityprofile_spline.hpp
+++ b/orocos_kdl/src/velocityprofile_spline.hpp
@@ -16,10 +16,10 @@ class VelocityProfile_Spline : public VelocityProfile
     VelocityProfile_Spline(const VelocityProfile_Spline &p);
 
 	virtual ~VelocityProfile_Spline();
-	
+
     virtual void SetProfile(double pos1, double pos2);
     /**
-     * Generate linear interpolation coeffcients.
+     * Generate linear interpolation coefficients.
      *
      * @param pos1 begin position.
      * @param pos2 end position.
@@ -29,7 +29,7 @@ class VelocityProfile_Spline : public VelocityProfile
       double pos1, double pos2, double duration);
 
     /**
-     * Generate cubic spline interpolation coeffcients.
+     * Generate cubic spline interpolation coefficients.
      *
      * @param pos1 begin position.
      * @param vel1 begin velocity.
@@ -41,7 +41,7 @@ class VelocityProfile_Spline : public VelocityProfile
       double pos1, double vel1, double pos2, double vel2, double duration);
 
     /**
-     * Generate quintic spline interpolation coeffcients.
+     * Generate quintic spline interpolation coefficients.
      *
      * @param pos1 begin position.
      * @param vel1 begin velocity.
diff --git a/orocos_kdl/src/velocityprofile_trap.cpp b/orocos_kdl/src/velocityprofile_trap.cpp
index 798caa917..1bee674e5 100644
--- a/orocos_kdl/src/velocityprofile_trap.cpp
+++ b/orocos_kdl/src/velocityprofile_trap.cpp
@@ -91,12 +91,12 @@ void VelocityProfile_Trap::SetProfile(double pos1,double pos2) {
 void VelocityProfile_Trap::SetProfileDuration(
 	double pos1,double pos2,double newduration) {
 	// duration should be longer than originally planned duration
-    // Fastest :
+	// Fastest :
 	SetProfile(pos1,pos2);
-    // Must be Slower  :
+	// Must be Slower  :
 	double factor = duration/newduration;
-    if (factor > 1)
-        return; // do not exceed max
+	if (factor > 1)
+		return; // do not exceed max
 	a2*=factor;
 	a3*=factor*factor;
 	b2*=factor;
@@ -110,9 +110,9 @@ void VelocityProfile_Trap::SetProfileDuration(
 
 void VelocityProfile_Trap::SetProfileVelocity(
 	double pos1,double pos2,double newvelocity) {
-    // Max velocity
+	// Max velocity
 	SetProfile(pos1,pos2);
-    // Must be Slower  :
+	// Must be Slower  :
 	double factor = newvelocity;		// valid = [KDL::epsilon, 1.0]
 	if (1.0 < factor) factor = 1.0;
 	if (KDL::epsilon > factor) factor = KDL::epsilon;
@@ -129,7 +129,7 @@ void VelocityProfile_Trap::SetProfileVelocity(
 
 void VelocityProfile_Trap::SetMax(double _maxvel,double _maxacc)
 {
-    maxvel = _maxvel; maxacc = _maxacc;
+	maxvel = _maxvel; maxacc = _maxacc;
 }
 
 double VelocityProfile_Trap::Duration() const {
@@ -178,8 +178,8 @@ double VelocityProfile_Trap::Acc(double time) const {
 }
 
 VelocityProfile* VelocityProfile_Trap::Clone() const {
-    VelocityProfile_Trap* res =  new VelocityProfile_Trap(maxvel,maxacc);
-    res->SetProfileDuration( this->startpos, this->endpos, this->duration );
+	VelocityProfile_Trap* res =  new VelocityProfile_Trap(maxvel,maxacc);
+	res->SetProfileDuration( this->startpos, this->endpos, this->duration );
 	return res;
 }
 
diff --git a/orocos_kdl/src/velocityprofile_traphalf.cpp b/orocos_kdl/src/velocityprofile_traphalf.cpp
index 80b63e0d6..05ff180bc 100644
--- a/orocos_kdl/src/velocityprofile_traphalf.cpp
+++ b/orocos_kdl/src/velocityprofile_traphalf.cpp
@@ -52,7 +52,7 @@ VelocityProfile_TrapHalf::VelocityProfile_TrapHalf(double _maxvel,double _maxacc
 
 void VelocityProfile_TrapHalf::SetMax(double _maxvel,double _maxacc, bool _starting)
 {
-    maxvel = _maxvel; maxacc = _maxacc; starting = _starting;
+	maxvel = _maxvel; maxacc = _maxacc; starting = _starting;
 }
 
 void VelocityProfile_TrapHalf::PlanProfile1(double v,double a) {
@@ -100,11 +100,11 @@ void VelocityProfile_TrapHalf::SetProfile(double pos1,double pos2) {
 void VelocityProfile_TrapHalf::SetProfileDuration(
 	double pos1,double pos2,double newduration)
 {
-    SetProfile(pos1,pos2);
-    double factor = duration/newduration;
+	SetProfile(pos1,pos2);
+	double factor = duration/newduration;
 
-    if ( factor > 1 )
-        return;
+	if ( factor > 1 )
+		return;
 
 	double s        = sign(endpos-startpos);
 	double tmp      = 2.0*s*(endpos-startpos)/maxvel;
@@ -183,8 +183,8 @@ double VelocityProfile_TrapHalf::Acc(double time) const {
 }
 
 VelocityProfile* VelocityProfile_TrapHalf::Clone() const {
-    VelocityProfile_TrapHalf* res =  new VelocityProfile_TrapHalf(maxvel,maxacc, starting);
-    res->SetProfileDuration( this->startpos, this->endpos, this->duration );
+	VelocityProfile_TrapHalf* res =  new VelocityProfile_TrapHalf(maxvel,maxacc, starting);
+	res->SetProfileDuration( this->startpos, this->endpos, this->duration );
 	return res;
 }
 
diff --git a/orocos_kdl/tests/CMakeLists.txt b/orocos_kdl/tests/CMakeLists.txt
index 06382cdb4..b81a2c539 100644
--- a/orocos_kdl/tests/CMakeLists.txt
+++ b/orocos_kdl/tests/CMakeLists.txt
@@ -9,7 +9,7 @@ IF(ENABLE_TESTS)
   SET_TARGET_PROPERTIES( framestest PROPERTIES
    COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ")
   ADD_TEST(framestest framestest)
-  
+
  ADD_EXECUTABLE(kinfamtest kinfamtest.cpp test-runner.cpp)
  TARGET_LINK_LIBRARIES(kinfamtest orocos-kdl ${CPPUNIT})
  SET(TESTNAME "kinfamtest")
@@ -44,7 +44,7 @@ IF(ENABLE_TESTS)
  SET_TARGET_PROPERTIES( jacobiandottest PROPERTIES
   COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ")
  ADD_TEST(jacobiandottest jacobiandottest)
- 
+
  ADD_EXECUTABLE(velocityprofiletest velocityprofiletest.cpp test-runner.cpp)
  SET(TESTNAME "velocityprofiletest")
  TARGET_LINK_LIBRARIES(velocityprofiletest orocos-kdl ${CPPUNIT})
@@ -52,6 +52,13 @@ IF(ENABLE_TESTS)
    COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ")
  ADD_TEST(velocityprofiletest velocityprofiletest)
 
+ ADD_EXECUTABLE(treeinvdyntest treeinvdyntest.cpp test-runner.cpp)
+ SET(TESTNAME "treeinvdyntest")
+ TARGET_LINK_LIBRARIES(treeinvdyntest orocos-kdl ${CPPUNIT})
+ SET_TARGET_PROPERTIES( treeinvdyntest PROPERTIES
+   COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ")
+ ADD_TEST(treeinvdyntest treeinvdyntest)
+
 #  ADD_EXECUTABLE(rframestest  rframestest.cpp)
 #  TARGET_LINK_LIBRARIES(rframestest orocos-kdl)
 #  ADD_TEST(rframestest rframestest)
@@ -59,29 +66,5 @@ IF(ENABLE_TESTS)
 #  ADD_EXECUTABLE(rallnumbertest  rallnumbertest.cpp)
 #  TARGET_LINK_LIBRARIES(rallnumbertest orocos-kdl)
 #  ADD_TEST(rallnumbertest rallnumbertest)
-#
-#  
-# IF(OROCOS_PLUGIN)
-#   ADD_EXECUTABLE(toolkittest toolkittest.cpp)
-#   LINK_DIRECTORIES(${OROCOS_RTT_LINK_DIRS})
-#   TARGET_LINK_LIBRARIES(toolkittest orocos-kdltk orocos-kdl ${OROCOS_RTT_LIBS})
-#   ADD_TEST(toolkittest toolkittest)
-# ENDIF(OROCOS_PLUGIN)
-#
-# IF(PYTHON_BINDINGS)
-#   CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/framestest.py
-#     ${CMAKE_CURRENT_BINARY_DIR}/framestest.py COPY_ONLY)
-#   CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/kinfamtest.py
-#     ${CMAKE_CURRENT_BINARY_DIR}/kinfamtest.py COPY_ONLY)
-#   CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/frameveltest.py
-#     ${CMAKE_CURRENT_BINARY_DIR}/frameveltest.py COPY_ONLY)
-#   CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/PyKDLtest.py
-#     ${CMAKE_CURRENT_BINARY_DIR}/PyKDLtest.py COPY_ONLY)
-#   ADD_TEST(PyKDLtest PyKDLtest.py)
-#   
-#  
-# ENDIF(PYTHON_BINDINGS)
-  
   ADD_CUSTOM_TARGET(check ctest -V WORKING_DIRECTORY ${PROJ_BINARY_DIR}/tests)
-ENDIF(ENABLE_TESTS)  
-
+ENDIF(ENABLE_TESTS)
diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp
index 51ef5511e..6b1e62357 100644
--- a/orocos_kdl/tests/framestest.cpp
+++ b/orocos_kdl/tests/framestest.cpp
@@ -1,6 +1,7 @@
-#include <math.h>
 #include "framestest.hpp"
 #include <frames_io.hpp>
+#include <utilities/utility.h>
+
 CPPUNIT_TEST_SUITE_REGISTRATION( FramesTest );
 
 using namespace KDL;
@@ -237,37 +238,37 @@ void FramesTest::TestEuler() {
 	TESTEULERZYX(0.1,0,0)
 	TESTEULERZYX(0,0,0.3)
 	TESTEULERZYX(0,0,0)
-	TESTEULERZYX(0.3,0.999*M_PI/2,0.1)
-	// if beta== +/- M_PI/2 => multiple solutions available, gamma will be chosen to be zero !
+    TESTEULERZYX(0.3,0.999*PI_2,0.1)
+	// if beta== +/- PI/2 => multiple solutions available, gamma will be chosen to be zero !
 	// so we test with gamma==0 !
-	TESTEULERZYX(0.3,0.9999999999*M_PI/2,0)
-	TESTEULERZYX(0.3,0.99999999*M_PI/2,0)
-	TESTEULERZYX(0.3,0.999999*M_PI/2,0)
-	TESTEULERZYX(0.3,0.9999*M_PI/2,0)
-	TESTEULERZYX(0.3,0.99*M_PI/2,0)
-	//TESTEULERZYX(0.1,-M_PI/2,0.3)
-	TESTEULERZYX(0,M_PI/2,0)
-
-	TESTEULERZYX(0.3,-M_PI/2,0)
-	TESTEULERZYX(0.3,-0.9999999999*M_PI/2,0)
-	TESTEULERZYX(0.3,-0.99999999*M_PI/2,0)
-	TESTEULERZYX(0.3,-0.999999*M_PI/2,0)
-	TESTEULERZYX(0.3,-0.9999*M_PI/2,0)
-	TESTEULERZYX(0.3,-0.99*M_PI/2,0)
-	TESTEULERZYX(0,-M_PI/2,0)
+    TESTEULERZYX(0.3,0.9999999999*PI_2,0)
+    TESTEULERZYX(0.3,0.99999999*PI_2,0)
+    TESTEULERZYX(0.3,0.999999*PI_2,0)
+    TESTEULERZYX(0.3,0.9999*PI_2,0)
+    TESTEULERZYX(0.3,0.99*PI_2,0)
+    //TESTEULERZYX(0.1,-PI_2,0.3)
+    TESTEULERZYX(0,PI_2,0)
+
+    TESTEULERZYX(0.3,-PI_2,0)
+    TESTEULERZYX(0.3,-0.9999999999*PI_2,0)
+    TESTEULERZYX(0.3,-0.99999999*PI_2,0)
+    TESTEULERZYX(0.3,-0.999999*PI_2,0)
+    TESTEULERZYX(0.3,-0.9999*PI_2,0)
+    TESTEULERZYX(0.3,-0.99*PI_2,0)
+    TESTEULERZYX(0,-PI_2,0)
 
 	// extremes of the range:
-	TESTEULERZYX(M_PI,-M_PI/2,0)
-	TESTEULERZYX(-M_PI,-M_PI/2,0)
-	TESTEULERZYX(M_PI,1,0)
-	TESTEULERZYX(-M_PI,1,0)
-	//TESTEULERZYX(0,-M_PI/2,M_PI)  gamma will be chosen zero
-	//TESTEULERZYX(0,M_PI/2,-M_PI)  gamma will be chosen zero
-	TESTEULERZYX(0,1,M_PI)
+    TESTEULERZYX(PI,-PI_2,0)
+    TESTEULERZYX(-PI,-PI_2,0)
+	TESTEULERZYX(PI,1,0)
+	TESTEULERZYX(-PI,1,0)
+    //TESTEULERZYX(0,-PI_2,PI)  gamma will be chosen zero
+    //TESTEULERZYX(0,PI_2,-PI)  gamma will be chosen zero
+	TESTEULERZYX(0,1,PI)
 
 	TESTEULERZYZ(0.1,0.2,0.3)
 	TESTEULERZYZ(-0.1,0.2,0.3)
-	TESTEULERZYZ(0.1,0.9*M_PI,0.3)
+	TESTEULERZYZ(0.1,0.9*PI,0.3)
 	TESTEULERZYZ(0.1,0.2,-0.3)
 	TESTEULERZYZ(0,0,0)
 	TESTEULERZYZ(0,0,0)
@@ -277,22 +278,22 @@ void FramesTest::TestEuler() {
 	TESTEULERZYZ(0.4,PI,0)
 	TESTEULERZYZ(0,PI,0)
 	TESTEULERZYZ(PI,PI,0)
-	TESTEULERZYX(0.3,M_PI/2,0)
-	TESTEULERZYZ(0.3,0.9999999999*M_PI/2,0)
-	TESTEULERZYZ(0.3,0.99999999*M_PI/2,0)
-	TESTEULERZYZ(0.3,0.999999*M_PI/2,0)
-	TESTEULERZYZ(0.3,0.9999*M_PI/2,0)
-	TESTEULERZYZ(0.3,0.99*M_PI/2,0)
-
-	TESTEULERZYX_INVARIANT(0.1,0.2,0.3,   0.1+M_PI,  M_PI-0.2, 0.3+M_PI);
-	TESTEULERZYX_INVARIANT(0.1,0.2,0.3,   0.1-M_PI,  M_PI-0.2, 0.3-M_PI);
-	TESTEULERZYX_INVARIANT(0.1,0.2,0.3,   0.1-M_PI,  M_PI-0.2, 0.3+M_PI);
-	TESTEULERZYX_INVARIANT(0.1,0.2,0.3,   0.1+M_PI,  M_PI-0.2, 0.3-M_PI);
-
-	TESTEULERZYZ_INVARIANT(0.1,0.2,0.3,   0.1+M_PI,  -0.2, 0.3+M_PI);
-	TESTEULERZYZ_INVARIANT(0.1,0.2,0.3,   0.1-M_PI,  -0.2, 0.3+M_PI);
-	TESTEULERZYZ_INVARIANT(0.1,0.2,0.3,   0.1+M_PI,  -0.2, 0.3-M_PI);
-	TESTEULERZYZ_INVARIANT(0.1,0.2,0.3,   0.1-M_PI,  -0.2, 0.3-M_PI);
+    TESTEULERZYX(0.3,PI_2,0)
+    TESTEULERZYZ(0.3,0.9999999999*PI_2,0)
+    TESTEULERZYZ(0.3,0.99999999*PI_2,0)
+    TESTEULERZYZ(0.3,0.999999*PI_2,0)
+    TESTEULERZYZ(0.3,0.9999*PI_2,0)
+    TESTEULERZYZ(0.3,0.99*PI_2,0)
+
+	TESTEULERZYX_INVARIANT(0.1,0.2,0.3,   0.1+PI,  PI-0.2, 0.3+PI);
+	TESTEULERZYX_INVARIANT(0.1,0.2,0.3,   0.1-PI,  PI-0.2, 0.3-PI);
+	TESTEULERZYX_INVARIANT(0.1,0.2,0.3,   0.1-PI,  PI-0.2, 0.3+PI);
+	TESTEULERZYX_INVARIANT(0.1,0.2,0.3,   0.1+PI,  PI-0.2, 0.3-PI);
+
+	TESTEULERZYZ_INVARIANT(0.1,0.2,0.3,   0.1+PI,  -0.2, 0.3+PI);
+	TESTEULERZYZ_INVARIANT(0.1,0.2,0.3,   0.1-PI,  -0.2, 0.3+PI);
+	TESTEULERZYZ_INVARIANT(0.1,0.2,0.3,   0.1+PI,  -0.2, 0.3-PI);
+	TESTEULERZYZ_INVARIANT(0.1,0.2,0.3,   0.1-PI,  -0.2, 0.3-PI);
 }
 
 void FramesTest::TestRangeArbitraryRotation(const std::string& msg,
@@ -408,9 +409,8 @@ void FramesTest::TestRotation() {
 }
 
 void FramesTest::TestGetRotAngle() {
-    static const double pi = atan(1)*4;
     double roll = -2.9811968953315162;
-    double pitch = -pi/2;
+    double pitch = -PI_2;
     double yaw = -0.1603957582582825;
 
     // rpy -> rotation
@@ -436,8 +436,32 @@ void FramesTest::TestGetRotAngle() {
   {
     Vector axis;
     double angle = KDL::Rotation(-1, 0, 0 + 1e-6, 0, 1, 0, 0, 0, -1 - 1e-6).GetRotAngle(axis);
-    CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, PI)", M_PI, angle, epsilon);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, PI)", PI, angle, epsilon);
+  }
+
+  // Tests to show that GetRotAngle does not work for an improper rotation matrix which has a determinant of -1;
+  // an improper rotation matrix corresponds to a rotation between a right-hand and left-hand coordinate system
+  {
+    Vector axis;
+    double angle;
+    Rotation R, Rout;
+    double det;
+    // Improper Rotation Matrix for 120 deg rotation
+    R = KDL::Rotation( 0, -1, 0, 0, 0, -1, -1, 0, 0);
+    det = +R(0,0)*(R(1,1)*R(2,2)-R(2,1)*R(1,2))-R(0,1)*(R(1,0)*R(2,2)-R(2,0)*R(1,2))+R(0,2)*(R(1,0)*R(2,1)-R(2,0)*R(1,1));
+    CPPUNIT_ASSERT_EQUAL(det,-1.0);
+    angle = R.GetRotAngle(axis);
+    Rout = KDL::Rotation::Rot(axis, angle);
+    CPPUNIT_ASSERT_ASSERTION_FAIL(CPPUNIT_ASSERT_EQUAL(R,Rout));
+    // Improper Rotation matrix for 180 deg rotation (singular)
+    R = KDL::Rotation( -1, 0, 0, 0, -1, 0, 0, 0, -1);
+    det = +R(0,0)*(R(1,1)*R(2,2)-R(2,1)*R(1,2))-R(0,1)*(R(1,0)*R(2,2)-R(2,0)*R(1,2))+R(0,2)*(R(1,0)*R(2,1)-R(2,0)*R(1,1));
+    CPPUNIT_ASSERT_EQUAL(det,-1.0);
+    angle = R.GetRotAngle(axis);
+    Rout = KDL::Rotation::Rot(axis, angle);
+    CPPUNIT_ASSERT_ASSERTION_FAIL(CPPUNIT_ASSERT_EQUAL(R,Rout));
   }
+
 }
 
 void FramesTest::TestQuaternion() {
@@ -482,60 +506,107 @@ void FramesTest::TestQuaternion() {
 	CPPUNIT_ASSERT_DOUBLES_EQUAL(w, w2, epsilon);
 }
 
+void FramesTest::TestOneRotationDiff(
+        const std::string& msg,
+        const Rotation& R_a_b1,
+        const Rotation& R_a_b2,
+        const Vector& expectedDiff) {
+    CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, diff(R_a_b1, R_a_b2), expectedDiff);
+    CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, addDelta(R_a_b1, expectedDiff), R_a_b2);
+}
 
 void FramesTest::TestRotationDiff() {
 
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(0*deg2rad)), Vector(0,0,0));		// no rotation
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(90*deg2rad)), Vector(M_PI/2,0,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(180*deg2rad)), Vector(M_PI,0,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(270*deg2rad)), Vector(-M_PI/2,0,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(360*deg2rad)), Vector(0,0,0));		// no rotation
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad)), Vector(0,0,0));	// no rotation
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-270*deg2rad)), Vector(M_PI/2,0,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-180*deg2rad)), Vector(M_PI,0,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-90*deg2rad)), Vector(-M_PI/2,0,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-0*deg2rad)), Vector(0,0,0));		// no rotation
-
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad)), Vector(0,0,0));		// no rotation
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(180*deg2rad)), Vector(0,M_PI,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(270*deg2rad)), Vector(0,-M_PI/2,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(360*deg2rad)), Vector(0,0,0));		// no rotation
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad)), Vector(0,0,0));	// no rotation
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-270*deg2rad)), Vector(0,M_PI/2,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-180*deg2rad)), Vector(0,M_PI,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-90*deg2rad)), Vector(0,-M_PI/2,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-0*deg2rad)), Vector(0,0,0));		// no rotation
-
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad)), Vector(0,0,0));		// no rotation
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(180*deg2rad)), Vector(0,0,M_PI));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(270*deg2rad)), Vector(0,0,-M_PI/2));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(360*deg2rad)), Vector(0,0,0));		// no rotation
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad)), Vector(0,0,0));	// no rotation
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-270*deg2rad)), Vector(0,0,M_PI/2));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-180*deg2rad)), Vector(0,0,M_PI));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-90*deg2rad)), Vector(0,0,-M_PI/2));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-0*deg2rad)), Vector(0,0,0));		// no rotation
-
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2));
-
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotX(90*deg2rad)), Vector(M_PI/2,0,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2));
-
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RPY(+0*deg2rad,0,-90*deg2rad),
-								   Rotation::RPY(-0*deg2rad,0,+90*deg2rad)),
-						 Vector(0,0,M_PI));
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RPY(+5*deg2rad,0,-0*deg2rad),
-								   Rotation::RPY(-5*deg2rad,0,+0*deg2rad)),
-						 Vector(-10*deg2rad,0,0));
+    TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(0*deg2rad))",
+        Rotation::RotX(0*deg2rad), Rotation::RotX(0*deg2rad), Vector(0,0,0));		// no rotation
+    TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(90*deg2rad))",
+        Rotation::RotX(0*deg2rad), Rotation::RotX(90*deg2rad), Vector(PI_2,0,0));
+    TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(180*deg2rad))",
+        Rotation::RotX(0*deg2rad), Rotation::RotX(180*deg2rad), Vector(PI,0,0));
+    TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(270*deg2rad))",
+        Rotation::RotX(0*deg2rad), Rotation::RotX(270*deg2rad), Vector(-PI_2,0,0));
+    TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(360*deg2rad))",
+        Rotation::RotX(0*deg2rad), Rotation::RotX(360*deg2rad), Vector(0,0,0));		// no rotation
+    TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-360*deg2rad))",
+        Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad), Vector(0,0,0));	// no rotation
+    TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-270*deg2rad))",
+        Rotation::RotX(0*deg2rad), Rotation::RotX(-270*deg2rad), Vector(PI_2,0,0));
+    TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-180*deg2rad))",
+        Rotation::RotX(0*deg2rad), Rotation::RotX(-180*deg2rad), Vector(PI,0,0));
+    TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-90*deg2rad))",
+        Rotation::RotX(0*deg2rad), Rotation::RotX(-90*deg2rad), Vector(-PI_2,0,0));
+    TestOneRotationDiff("diff(RotX(0*deg2rad),RotX(-0*deg2rad))",
+        Rotation::RotX(0*deg2rad), Rotation::RotX(-0*deg2rad), Vector(0,0,0));		// no rotation
+
+    TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(0*deg2rad))",
+        Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad), Vector(0,0,0));		// no rotation
+    TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(90*deg2rad))",
+        Rotation::RotY(0*deg2rad), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0));
+    TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(180*deg2rad))",
+        Rotation::RotY(0*deg2rad), Rotation::RotY(180*deg2rad), Vector(0,PI,0));
+    TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(270*deg2rad))",
+        Rotation::RotY(0*deg2rad), Rotation::RotY(270*deg2rad), Vector(0,-PI_2,0));
+    TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(360*deg2rad))",
+        Rotation::RotY(0*deg2rad), Rotation::RotY(360*deg2rad), Vector(0,0,0));		// no rotation
+    TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-360*deg2rad))",
+        Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad), Vector(0,0,0));	// no rotation
+    TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-270*deg2rad))",
+        Rotation::RotY(0*deg2rad), Rotation::RotY(-270*deg2rad), Vector(0,PI_2,0));
+    TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-180*deg2rad))",
+        Rotation::RotY(0*deg2rad), Rotation::RotY(-180*deg2rad), Vector(0,PI,0));
+    TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-90*deg2rad))",
+        Rotation::RotY(0*deg2rad), Rotation::RotY(-90*deg2rad), Vector(0,-PI_2,0));
+    TestOneRotationDiff("diff(RotY(0*deg2rad),RotY(-0*deg2rad))",
+        Rotation::RotY(0*deg2rad), Rotation::RotY(-0*deg2rad), Vector(0,0,0));		// no rotation
+
+    TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(0*deg2rad))",
+        Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad), Vector(0,0,0));		// no rotation
+    TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(90*deg2rad))",
+        Rotation::RotZ(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2));
+    TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(180*deg2rad))",
+        Rotation::RotZ(0*deg2rad), Rotation::RotZ(180*deg2rad), Vector(0,0,PI));
+    TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(270*deg2rad))",
+        Rotation::RotZ(0*deg2rad), Rotation::RotZ(270*deg2rad), Vector(0,0,-PI_2));
+    TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(360*deg2rad))",
+        Rotation::RotZ(0*deg2rad), Rotation::RotZ(360*deg2rad), Vector(0,0,0));		// no rotation
+    TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-360*deg2rad))",
+        Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad), Vector(0,0,0));	// no rotation
+    TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-270*deg2rad))",
+        Rotation::RotZ(0*deg2rad), Rotation::RotZ(-270*deg2rad), Vector(0,0,PI_2));
+    TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-180*deg2rad))",
+        Rotation::RotZ(0*deg2rad), Rotation::RotZ(-180*deg2rad), Vector(0,0,PI));
+    TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-90*deg2rad))",
+        Rotation::RotZ(0*deg2rad), Rotation::RotZ(-90*deg2rad), Vector(0,0,-PI_2));
+    TestOneRotationDiff("diff(RotZ(0*deg2rad),RotZ(-0*deg2rad))",
+        Rotation::RotZ(0*deg2rad), Rotation::RotZ(-0*deg2rad), Vector(0,0,0));		// no rotation
+
+    TestOneRotationDiff("diff(RotX(0*deg2rad),RotZ(90*deg2rad))",
+        Rotation::RotX(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2));
+    TestOneRotationDiff("diff(RotX(0*deg2rad),RotY(90*deg2rad))",
+        Rotation::RotX(0*deg2rad), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0));
+    TestOneRotationDiff("diff(RotY(0*deg2rad),RotZ(90*deg2rad))",
+        Rotation::RotY(0*deg2rad), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2));
+
+    TestOneRotationDiff("diff(Identity(),RotX(90*deg2rad))",
+        Rotation::Identity(), Rotation::RotX(90*deg2rad), Vector(PI_2,0,0));
+    TestOneRotationDiff("diff(Identity(),RotY(0*deg2rad))",
+        Rotation::Identity(), Rotation::RotY(90*deg2rad), Vector(0,PI_2,0));
+    TestOneRotationDiff("diff(Identity(),RotZ(0*deg2rad))",
+        Rotation::Identity(), Rotation::RotZ(90*deg2rad), Vector(0,0,PI_2));
+
+    TestOneRotationDiff("diff(Rotation::RPY(+0*deg2rad,0,-90*deg2rad),Rotation::RPY(-0*deg2rad,0,+90*deg2rad))",
+        Rotation::RPY(+0*deg2rad,0,-90*deg2rad),
+        Rotation::RPY(-0*deg2rad,0,+90*deg2rad),
+        Vector(0,0,PI));
+    TestOneRotationDiff("diff(Rotation::RPY(+5*deg2rad,0,-0*deg2rad),Rotation::RPY(-5*deg2rad,0,+0*deg2rad))",
+        Rotation::RPY(+5*deg2rad,0,-0*deg2rad),
+        Rotation::RPY(-5*deg2rad,0,+0*deg2rad),
+        Vector(-10*deg2rad,0,0));
 
     KDL::Rotation R1 = Rotation::RPY(+5*deg2rad,0,-90*deg2rad);
-	CPPUNIT_ASSERT_EQUAL(KDL::diff(R1, Rotation::RPY(-5*deg2rad,0,+90*deg2rad)),
-						           R1*Vector(0, 0, 180*deg2rad));
+    TestOneRotationDiff("diff(Rotation::RPY(+5*deg2rad,0,-90*deg2rad),Rotation::RPY(-5*deg2rad,0,+90*deg2rad))",
+        R1, Rotation::RPY(-5*deg2rad,0,+90*deg2rad),
+        R1*Vector(0, 0, 180*deg2rad));
 }
 
 void FramesTest::TestFrame() {
@@ -564,50 +635,63 @@ void FramesTest::TestFrame() {
 	CPPUNIT_ASSERT_EQUAL(F.Inverse()*v,F.Inverse(v));
 }
 
+JntArray CreateRandomJntArray(int size)
+{
+    JntArray j(size);
+    for (int i = 0; i<size; ++i)
+        random(j(i));
+    return j;
+}
+
 void FramesTest::TestJntArray()
 {
-    JntArray a1(4);
-    random(a1(0));
-    random(a1(1));
-    random(a1(2));
-    random(a1(3));
-    JntArray a2(a1);
-    CPPUNIT_ASSERT(Equal(a2,a1));
+    JntArray random1 = CreateRandomJntArray(4);
+    JntArray random1_copy(random1);
+    CPPUNIT_ASSERT(Equal(random1_copy,random1));
 
-    SetToZero(a2);
-    CPPUNIT_ASSERT(!Equal(a1,a2));
-    
-    JntArray a3(4);
-    CPPUNIT_ASSERT(Equal(a2,a3));
-    
-    a1=a2;
-    CPPUNIT_ASSERT(Equal(a1,a3));
+    JntArray zero_set_to_zero(4);
+    SetToZero(zero_set_to_zero);
+    CPPUNIT_ASSERT(!Equal(random1,zero_set_to_zero));
+
+    JntArray zero(4);
+    CPPUNIT_ASSERT(Equal(zero_set_to_zero,zero));
+
+    JntArray almost_zero = CreateRandomJntArray(4);
+    almost_zero(0) = almost_zero(0)*1e-7;
+    almost_zero(1) = almost_zero(1)*1e-7;
+    almost_zero(2) = almost_zero(2)*1e-7;
+    almost_zero(3) = almost_zero(3)*1e-7;
+
+    // This should obviously be equal, but fails in old buggy implementation
+    CPPUNIT_ASSERT(Equal(almost_zero,zero,1));
+    CPPUNIT_ASSERT(Equal(almost_zero,zero,1e-6));
+    CPPUNIT_ASSERT(!Equal(almost_zero,zero,1e-8));
+
+    JntArray sum_random_zero(4);
+
+    Add(random1,zero_set_to_zero,sum_random_zero);
+    CPPUNIT_ASSERT(Equal(random1,sum_random_zero));
+
+    JntArray add_subtract(4);
+    JntArray random2 = CreateRandomJntArray(4);
+
+    Add(random1,random2,add_subtract);
+    Subtract(add_subtract,random2,add_subtract);
+    CPPUNIT_ASSERT(Equal(random1,add_subtract));
+
+    JntArray random_multiply_by_2(4);
+    JntArray sum_random_same_random(4);
+    Multiply(random1,2,random_multiply_by_2);
+    Add(random1,random1,sum_random_same_random);
+    CPPUNIT_ASSERT(Equal(sum_random_same_random,random_multiply_by_2));
 
-    random(a1(0));
-    random(a1(1));
-    random(a1(2));
-    random(a1(3));
-    
-    Add(a1,a2,a3);
-    CPPUNIT_ASSERT(Equal(a1,a3));
-    
-    random(a2(0));
-    random(a2(1));
-    random(a2(2));
-    random(a2(3));
-    Add(a1,a2,a3);
-    Subtract(a3,a2,a3);
-    CPPUNIT_ASSERT(Equal(a1,a3));
-    
-    Multiply(a1,2,a3);
-    Add(a1,a1,a2);
-    CPPUNIT_ASSERT(Equal(a2,a3));
-    
     double a;
     random(a);
-    Multiply(a1,a,a3);
-    Divide(a3,a,a2);
-    CPPUNIT_ASSERT(Equal(a2,a1));
+
+    JntArray random_multiply_devide(4);
+    Multiply(random1,a,random_multiply_devide);
+    Divide(random_multiply_devide,a,random_multiply_devide);
+    CPPUNIT_ASSERT(Equal(random_multiply_devide,random1));
 }
 
  
diff --git a/orocos_kdl/tests/framestest.hpp b/orocos_kdl/tests/framestest.hpp
index 31f9e3de0..0c3ca1531 100644
--- a/orocos_kdl/tests/framestest.hpp
+++ b/orocos_kdl/tests/framestest.hpp
@@ -57,6 +57,10 @@ class FramesTest : public CppUnit::TestFixture
     void TestRangeArbitraryRotation(const std::string& msg,
                                     const KDL::Vector& v,
                                     const KDL::Vector& expectedVector);
+    void TestOneRotationDiff(const std::string& msg,
+                             const KDL::Rotation& R_a_b1,
+                             const KDL::Rotation& R_a_b2,
+                             const KDL::Vector& expectedDiff);
  
 };
 
diff --git a/orocos_kdl/tests/inertiatest.cpp b/orocos_kdl/tests/inertiatest.cpp
index add23182c..816cea686 100644
--- a/orocos_kdl/tests/inertiatest.cpp
+++ b/orocos_kdl/tests/inertiatest.cpp
@@ -25,14 +25,14 @@ void InertiaTest::TestRotationalInertia() {
     CPPUNIT_ASSERT(Map<Matrix3d>(I0.data).isZero());
     RotationalInertia I1;
     CPPUNIT_ASSERT(Map<Matrix3d>(I1.data).isZero());
-    CPPUNIT_ASSERT(Map<Matrix3d>(I0.data).isApprox(Map<Matrix3d>(I1.data)));
+    CPPUNIT_ASSERT((Map<Matrix3d>(I0.data)-Map<Matrix3d>(I1.data)).isZero());
     RotationalInertia I2(1,2,3,4,5,6);
     
     //Check if copying works fine
     RotationalInertia I3=I2;
-    CPPUNIT_ASSERT(Map<Matrix3d>(I3.data).isApprox(Map<Matrix3d>(I2.data)));
+    CPPUNIT_ASSERT((Map<Matrix3d>(I3.data)-Map<Matrix3d>(I2.data)).isZero());
     I2.data[0]=0.0;
-    CPPUNIT_ASSERT(!Map<Matrix3d>(I3.data).isApprox(Map<Matrix3d>(I2.data)));
+    CPPUNIT_ASSERT(!(Map<Matrix3d>(I3.data)-Map<Matrix3d>(I2.data)).isZero());
 
     //Check if addition and multiplication works fine
     Map<Matrix3d>(I0.data).setRandom();
@@ -42,15 +42,14 @@ void InertiaTest::TestRotationalInertia() {
     CPPUNIT_ASSERT(Map<Matrix3d>(I1.data).isZero());
 
     //Check if matrix is symmetric
-    CPPUNIT_ASSERT(Map<Matrix3d>(I2.data).isApprox(Map<Matrix3d>(I2.data).transpose()));
+    CPPUNIT_ASSERT((Map<Matrix3d>(I2.data)-Map<Matrix3d>(I2.data).transpose()).isZero());
 
     //Check if angular momentum is correctly calculated:
     Vector omega;
     random(omega);
     Vector momentum=I2*omega;
-    
-    CPPUNIT_ASSERT(Map<Vector3d>(momentum.data).isApprox(Map<Matrix3d>(I2.data)*Map<Vector3d>(omega.data)));
 
+    CPPUNIT_ASSERT((Map<Vector3d>(momentum.data)-(Map<Matrix3d>(I2.data)*Map<Vector3d>(omega.data))).isZero());
 }
 
 void InertiaTest::TestRigidBodyInertia() {
@@ -68,13 +67,13 @@ void InertiaTest::TestRigidBodyInertia() {
     CPPUNIT_ASSERT_EQUAL(I2.getMass(),mass);
     CPPUNIT_ASSERT(!Map<Matrix3d>(I2.getRotationalInertia().data).isZero());
     CPPUNIT_ASSERT_EQUAL(I2.getCOG(),c);
-    CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(Ic.data)-mass*(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity()))));
+    CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-(Map<Matrix3d>(Ic.data)-mass*(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity())))).isZero());
     
     RigidBodyInertia I3=I2;
     //check if copying works fine
     CPPUNIT_ASSERT_EQUAL(I2.getMass(),I3.getMass());
     CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I3.getCOG());
-    CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I3.getRotationalInertia().data)));
+    CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-Map<Matrix3d>(I3.getRotationalInertia().data)).isZero());
     //Check if multiplication and addition works fine
     RigidBodyInertia I4=-2*I2 +I3+I3;
     CPPUNIT_ASSERT_DOUBLES_EQUAL(I4.getMass(),0.0,epsilon);
@@ -90,13 +89,13 @@ void InertiaTest::TestRigidBodyInertia() {
     I4 = R.Inverse()*I3;
     CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(), epsilon);
     CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
-    CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
+    CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
     //rotation and total with p=0
     Frame T(R);
     I4 = T*I2;
     CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(), epsilon);
     CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
-    CPPUNIT_ASSERT(Map<Matrix3d>(I3.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
+    CPPUNIT_ASSERT((Map<Matrix3d>(I3.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
     
     //Check only transformation
     Vector p;
@@ -105,12 +104,12 @@ void InertiaTest::TestRigidBodyInertia() {
     I4 = I3.RefPoint(-p);
     CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon);
     CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
-    CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
+    CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
     T=Frame(-p);
     I4 = T*I2;
     CPPUNIT_ASSERT_DOUBLES_EQUAL(I3.getMass(),I4.getMass(),epsilon);
     CPPUNIT_ASSERT_EQUAL(I3.getCOG(),I4.getCOG());
-    CPPUNIT_ASSERT(Map<Matrix3d>(I3.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
+    CPPUNIT_ASSERT((Map<Matrix3d>(I3.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
     
     random(T);
     I3=T*I2;
@@ -126,7 +125,7 @@ void InertiaTest::TestRigidBodyInertia() {
     I4 = T.Inverse()*I3;
     CPPUNIT_ASSERT_DOUBLES_EQUAL(I2.getMass(),I4.getMass(),epsilon);
     CPPUNIT_ASSERT_EQUAL(I2.getCOG(),I4.getCOG());
-    CPPUNIT_ASSERT(Map<Matrix3d>(I2.getRotationalInertia().data).isApprox(Map<Matrix3d>(I4.getRotationalInertia().data)));
+    CPPUNIT_ASSERT((Map<Matrix3d>(I2.getRotationalInertia().data)-Map<Matrix3d>(I4.getRotationalInertia().data)).isZero());
 
 }
 void InertiaTest::TestArticulatedBodyInertia() {
@@ -150,18 +149,18 @@ void InertiaTest::TestArticulatedBodyInertia() {
 
     CPPUNIT_ASSERT_EQUAL(I2.M,(Matrix3d::Identity()*mass).eval());
     CPPUNIT_ASSERT(!I2.I.isZero());
-    //CPPUNIT_ASSERT(I2.I.isApprox(Map<Matrix3d>(Ic.data)-mass*(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity())),epsilon));
-    //CPPUNIT_ASSERT(I2.H.isApprox(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity()),epsilon));
+//    CPPUNIT_ASSERT((I2.I-(Map<Matrix3d>(Ic.data)-mass*(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity())))).isZero());
+//    CPPUNIT_ASSERT((I2.H-(Map<Vector3d>(c.data)*Map<Vector3d>(c.data).transpose()-(Map<Vector3d>(c.data).dot(Map<Vector3d>(c.data))*Matrix3d::Identity()))).isZero());
     ArticulatedBodyInertia I3=I2;
     //check if copying works fine
-    CPPUNIT_ASSERT(I2.M.isApprox(I3.M));
-    CPPUNIT_ASSERT(I2.H.isApprox(I3.H));
-    CPPUNIT_ASSERT(I2.I.isApprox(I3.I));
+    CPPUNIT_ASSERT((I2.M-I3.M).isZero());
+    CPPUNIT_ASSERT((I2.H-I3.H).isZero());
+    CPPUNIT_ASSERT((I2.I-I3.I).isZero());
     //Check if multiplication and addition works fine
     ArticulatedBodyInertia I4=-2*I2 +I3+I3;
-    CPPUNIT_ASSERT(I4.M.isApprox(Matrix3d::Zero().eval()));
-    CPPUNIT_ASSERT(I4.H.isApprox(Matrix3d::Zero().eval()));
-    CPPUNIT_ASSERT(I4.I.isApprox(Matrix3d::Zero().eval()));
+    CPPUNIT_ASSERT(I4.M.isZero());
+    CPPUNIT_ASSERT(I4.H.isZero());
+    CPPUNIT_ASSERT(I4.I.isZero());
     
     //Check if transformations work fine
     //Check only rotation transformation
@@ -171,36 +170,36 @@ void InertiaTest::TestArticulatedBodyInertia() {
     I3 = R*I2;
     Map<Matrix3d> E(R.data);
     Matrix3d tmp = E.transpose()*I2.M*E;
-    CPPUNIT_ASSERT(I3.M.isApprox(tmp));
+    CPPUNIT_ASSERT((I3.M-tmp).isZero());
     tmp = E.transpose()*I2.H*E;
-    CPPUNIT_ASSERT(I3.H.isApprox(tmp));
+    CPPUNIT_ASSERT((I3.H-tmp).isZero());
     tmp = E.transpose()*I2.I*E;
-    CPPUNIT_ASSERT(I3.I.isApprox(tmp));
+    CPPUNIT_ASSERT((I3.I-tmp).isZero());
 
     I4 = R.Inverse()*I3;
-    CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
-    CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
-    CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
+    CPPUNIT_ASSERT((I2.M-I4.M).isZero());
+    CPPUNIT_ASSERT((I2.H-I4.H).isZero());
+    CPPUNIT_ASSERT((I2.I-I4.I).isZero());
     //rotation and total with p=0
     Frame T(R);
     I4 = T*I2;
-    CPPUNIT_ASSERT(I3.M.isApprox(I4.M));
-    CPPUNIT_ASSERT(I3.H.isApprox(I4.H));
-    CPPUNIT_ASSERT(I3.I.isApprox(I4.I));
+    CPPUNIT_ASSERT((I3.M-I4.M).isZero());
+    CPPUNIT_ASSERT((I3.H-I4.H).isZero());
+    CPPUNIT_ASSERT((I3.I-I4.I).isZero());
 
     //Check only transformation
     Vector p;
     random(p);
     I3 = I2.RefPoint(p);
     I4 = I3.RefPoint(-p);
-    CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
-    CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
-    CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
+    CPPUNIT_ASSERT((I2.M-I4.M).isZero());
+    CPPUNIT_ASSERT((I2.H-I4.H).isZero());
+    CPPUNIT_ASSERT((I2.I-I4.I).isZero());
     T=Frame(-p);
     I4 = T*I2;
-    CPPUNIT_ASSERT(I3.M.isApprox(I4.M));
-    CPPUNIT_ASSERT(I3.H.isApprox(I4.H));
-    CPPUNIT_ASSERT(I3.I.isApprox(I4.I));
+    CPPUNIT_ASSERT((I3.M-I4.M).isZero());
+    CPPUNIT_ASSERT((I3.H-I4.H).isZero());
+    CPPUNIT_ASSERT((I3.I-I4.I).isZero());
 
     
     random(T);
@@ -215,7 +214,7 @@ void InertiaTest::TestArticulatedBodyInertia() {
     random(T);
     I3 = T*I2;
     I4 = T.Inverse()*I3;
-    CPPUNIT_ASSERT(I2.M.isApprox(I4.M));
-    CPPUNIT_ASSERT(I2.H.isApprox(I4.H));
-    CPPUNIT_ASSERT(I2.I.isApprox(I4.I));
+    CPPUNIT_ASSERT((I2.M-I4.M).isZero());
+    CPPUNIT_ASSERT((I2.H-I4.H).isZero());
+    CPPUNIT_ASSERT((I2.I-I4.I).isZero());
 }
diff --git a/orocos_kdl/tests/jacobiandottest.cpp b/orocos_kdl/tests/jacobiandottest.cpp
index 31950184a..97de6d9ec 100644
--- a/orocos_kdl/tests/jacobiandottest.cpp
+++ b/orocos_kdl/tests/jacobiandottest.cpp
@@ -100,7 +100,7 @@ void changeRepresentation(Jacobian& J,const Frame& F_bs_ee,const int& representa
             // Ref Frame {ee}, Ref Point {ee}
             J.changeBase(F_bs_ee.M.Inverse());
             break;
-        case ChainJntToJacDotSolver::INTERTIAL:
+        case ChainJntToJacDotSolver::INERTIAL:
             // Ref Frame {bs}, Ref Point {bs}
             J.changeRefPoint(-F_bs_ee.p);
             break;
@@ -113,8 +113,8 @@ void Jdot_diff(const Jacobian& J_q,
 {
     assert(J_q.columns() == J_qdt.columns());
     assert(J_q.columns() == Jdot.columns());
-    for(int l=0;l<6;l++)
-    for(int c=0;c<J_q.columns();c++)
+    for(unsigned int l=0;l<6;l++)
+    for(unsigned int c=0;c<J_q.columns();c++)
         Jdot(l,c) = (J_qdt(l,c) - J_q(l,c))/dt;
 }
 
@@ -146,14 +146,14 @@ Jacobian J_d2_symbolic(const JntArray& q,const JntArray& qdot)
 JntArray diff(const JntArray& q,const JntArray& qdot,const double& dt)
 {
     JntArray q_qdqt(q);
-    for(int i=0; i<q.rows(); i++)
+    for(unsigned int i=0; i<q.rows(); i++)
         q_qdqt(i) += dt*qdot(i);
     return q_qdqt;
 }
 
 void random(JntArray& q)
 {
-    for(int i=0; i<q.rows(); i++)
+    for(unsigned int i=0; i<q.rows(); i++)
         random(q(i));
 }
 
@@ -251,7 +251,7 @@ bool runTest(const Chain& chain,const int& representation)
     
     for(double dt=1e-6;dt<0.1;dt*=10)
     {
-        double eps_diff_vs_solver = 3.0*dt; // Apparently :)
+        double eps_diff_vs_solver = 4.0*dt; // Apparently :)
 
         for(int i=0;i<100;i++)
         {
diff --git a/orocos_kdl/tests/jacobiantest.cpp b/orocos_kdl/tests/jacobiantest.cpp
index bf58d38d2..a7b792181 100644
--- a/orocos_kdl/tests/jacobiantest.cpp
+++ b/orocos_kdl/tests/jacobiantest.cpp
@@ -92,6 +92,18 @@ void JacobianTest::TestConstructor(){
     CPPUNIT_ASSERT_EQUAL(j2.columns(),(unsigned int)5);
 }
 
-void JacobianTest::TestGetSetColumn(){}
+void JacobianTest::TestEqual(){
+    Jacobian j1(1);
+    Jacobian j2(j1);
+    for (unsigned int i=0; i<6; ++i)
+    {
+        random(j1(i,0));
+        j1(i,0) = j1(i,0)*1e-7;
+    }
+    CPPUNIT_ASSERT(Equal(j1,j2,1));
+    CPPUNIT_ASSERT(Equal(j1,j2,1e-6));
+    CPPUNIT_ASSERT(!Equal(j1,j2,1e-8));
+
+}
 
 
diff --git a/orocos_kdl/tests/jacobiantest.hpp b/orocos_kdl/tests/jacobiantest.hpp
index f1f9be5fa..5fc9a7fdd 100644
--- a/orocos_kdl/tests/jacobiantest.hpp
+++ b/orocos_kdl/tests/jacobiantest.hpp
@@ -13,7 +13,7 @@ class JacobianTest : public CppUnit::TestFixture
     CPPUNIT_TEST(TestChangeRefFrame);
     CPPUNIT_TEST(TestChangeBase);
     CPPUNIT_TEST(TestConstructor);
-    CPPUNIT_TEST(TestGetSetColumn);
+    CPPUNIT_TEST(TestEqual);
     CPPUNIT_TEST_SUITE_END();
 
 public:
@@ -24,7 +24,7 @@ class JacobianTest : public CppUnit::TestFixture
     void TestChangeRefFrame();
     void TestChangeBase();
     void TestConstructor();
-    void TestGetSetColumn();
+    void TestEqual();
 };
 
 #endif
diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp
index 76b5eb43e..914f8a694 100644
--- a/orocos_kdl/tests/solvertest.cpp
+++ b/orocos_kdl/tests/solvertest.cpp
@@ -3,7 +3,7 @@
 #include <framevel_io.hpp>
 #include <kinfam_io.hpp>
 #include <time.h>
-
+#include <utilities/utility.h>
 
 CPPUNIT_TEST_SUITE_REGISTRATION( SolverTest );
 
@@ -106,19 +106,70 @@ void SolverTest::setUp()
 	motomansia10.addSegment(Segment(Joint(Joint::None),
 									Frame::DH_Craig1989(0.0, 0.0, 0.36, 0.0)));
 	motomansia10.addSegment(Segment(Joint(Joint::RotZ),
-									Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0)));
+									Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0)));
 	motomansia10.addSegment(Segment(Joint(Joint::RotZ),
-									Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0)));
+									Frame::DH_Craig1989(0.0, -PI_2, 0.36, 0.0)));
 	motomansia10.addSegment(Segment(Joint(Joint::RotZ),
-									Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0)));
+									Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0)));
 	motomansia10.addSegment(Segment(Joint(Joint::RotZ),
-									Frame::DH_Craig1989(0.0, -M_PI_2, 0.36, 0.0)));
+									Frame::DH_Craig1989(0.0, -PI_2, 0.36, 0.0)));
 	motomansia10.addSegment(Segment(Joint(Joint::RotZ),
-									Frame::DH_Craig1989(0.0, M_PI_2, 0.0, 0.0)));
+									Frame::DH_Craig1989(0.0, PI_2, 0.0, 0.0)));
 	motomansia10.addSegment(Segment(Joint(Joint::RotZ),
-									Frame::DH_Craig1989(0.0, -M_PI_2, 0.0, 0.0)));
+									Frame::DH_Craig1989(0.0, -PI_2, 0.0, 0.0)));
 	motomansia10.addSegment(Segment(Joint(Joint::RotZ),
 									Frame(Rotation::Identity(),Vector(0.0,0.0,0.155))));
+
+    // Motoman SIA10 Chain with Mass Parameters (for forward dynamics tests)
+
+    //  effective motor inertia is included as joint inertia
+    static const double scale=1;
+    static const double offset=0;
+    static const double inertiamotorA=5.0;      // effective motor inertia kg-m^2
+    static const double inertiamotorB=3.0;      // effective motor inertia kg-m^2
+    static const double inertiamotorC=1.0;      // effective motor inertia kg-m^2
+    static const double damping=0;
+    static const double stiffness=0;
+
+    //  Segment Inertias
+    KDL::RigidBodyInertia inert1(15.0, KDL::Vector(0.0, -0.02, 0.0),                       // mass, CM
+                                 KDL::RotationalInertia(0.1, 0.05, 0.1, 0.0, 0.0, 0.0));   // inertia
+    KDL::RigidBodyInertia inert2(5.0, KDL::Vector(0.0, -0.02, -0.1),
+                                 KDL::RotationalInertia(0.01, 0.1, 0.1, 0.0, 0.0, 0.0));
+    KDL::RigidBodyInertia inert3(5.0, KDL::Vector(0.0, -0.05, 0.02),
+                                 KDL::RotationalInertia(0.05, 0.01, 0.05, 0.0, 0.0, 0.0));
+    KDL::RigidBodyInertia inert4(3.0, KDL::Vector(0.0, 0.02, -0.15),
+                                 KDL::RotationalInertia(0.1, 0.1, 0.01, 0.0, 0.0, 0.0));
+    KDL::RigidBodyInertia inert5(3.0, KDL::Vector(0.0, -0.05, 0.01),
+                                 KDL::RotationalInertia(0.02, 0.01, 0.02, 0.0, 0.0, 0.0));
+    KDL::RigidBodyInertia inert6(3.0, KDL::Vector(0.0, -0.01, -0.1),
+                                 KDL::RotationalInertia(0.1, 0.1, 0.01, 0.0, 0.0, 0.0));
+    KDL::RigidBodyInertia inert7(1.0, KDL::Vector(0.0, 0.0, 0.05),
+                                 KDL::RotationalInertia(0.01, 0.01, 0.1, 0.0, 0.0, 0.0));
+
+    motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness),
+                               Frame::DH(0.0, PI_2, 0.36, 0.0),
+                               inert1));
+    motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorA, damping, stiffness),
+                               Frame::DH(0.0, -PI_2, 0.0, 0.0),
+                               inert2));
+    motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness),
+                               Frame::DH(0.0, PI_2, 0.36, 0.0),
+                               inert3));
+    motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorB, damping, stiffness),
+                               Frame::DH(0.0, -PI_2, 0.0, 0.0),
+                               inert4));
+    motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness),
+                               Frame::DH(0.0, PI_2, 0.36, 0.0),
+                               inert5));
+    motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness),
+                               Frame::DH(0.0, -PI_2, 0.0, 0.0),
+                               inert6));
+    motomansia10dyn.addSegment(Segment(Joint(Joint::RotZ, scale, offset, inertiamotorC, damping, stiffness),
+                               Frame::DH(0.0, 0.0, 0.0, 0.0),
+                               inert7));
+    motomansia10dyn.addSegment(Segment(Joint(Joint::None),
+                                       Frame(Rotation::Identity(),Vector(0.0,0.0,0.155))));
 }
 
 void SolverTest::tearDown()
@@ -425,7 +476,7 @@ void SolverTest::IkSingularValueTest()
 	q(0) = 0. ;
 	q(1) = 0.5 ;
 	q(2) = 0.4 ;
-	q(3) = -M_PI_2 ;
+	q(3) = -PI_2 ;
 	q(4) = 0. ;
 	q(5) = 0. ;
 	q(6) = 0. ;
@@ -452,7 +503,7 @@ void SolverTest::IkSingularValueTest()
 	q(0) = 0. ;
 	q(1) = 0.2 ;
 	q(2) = 0.4 ;
-	q(3) = -M_PI_2 ;
+	q(3) = -PI_2 ;
 	q(4) = 0. ;
 	q(5) = 0. ;
 	q(6) = 0. ;
@@ -475,7 +526,7 @@ void SolverTest::IkSingularValueTest()
 	q(0) = 0. ;
 	q(1) = 0.5 ;
 	q(2) = 0.4 ;
-	q(3) = -M_PI_2 ;
+	q(3) = -PI_2 ;
 	q(4) = 0. ;
 	q(5) = 0. ;
 	q(6) = 0. ;
@@ -544,7 +595,7 @@ void SolverTest::IkVelSolverWDLSTest()
 	q(0) = 0. ;
 	q(1) = 0.5 ;
 	q(2) = 0.4 ;
-	q(3) = -M_PI_2 ;
+	q(3) = -PI_2 ;
 	q(4) = 0. ;
 	q(5) = 0. ;
 	q(6) = 0. ;
@@ -771,7 +822,7 @@ void SolverTest::VereshchaginTest()
     // Initial arm position configuration/constraint
     JntArray jointInitialPose(chaindyn.getNrOfJoints());
     jointInitialPose(0) = 0.0; // initial joint0 pose
-    jointInitialPose(1) = M_PI/6.0; //initial joint1 pose, negative in clockwise
+    jointInitialPose(1) = PI/6.0; //initial joint1 pose, negative in clockwise
     //j0=0.0, j1=pi/6.0 correspond to x = 0.2, y = -0.7464
     //j0=2*pi/3.0, j1=pi/4.0 correspond to x = 0.44992, y = 0.58636
 
@@ -789,7 +840,6 @@ void SolverTest::VereshchaginTest()
     double taskTimeConstant = 0.1;
     double simulationTime = 1*taskTimeConstant;
     double timeDelta = 0.01;
-    int status;
 
     const std::string msg = "Assertion failed, check matrix and array sizes";
 
@@ -847,3 +897,308 @@ void SolverTest::FkVelVectTest()
     
     CPPUNIT_ASSERT(Equal(v_out[chain1.getNrOfSegments()-1],f_out,1e-5));
 }
+
+void SolverTest::FdSolverDevelopmentTest()
+{
+    int ret;
+    double eps=1.e-3;
+
+    std::cout<<"KDL FD Solver Development Test for Motoman SIA10"<<std::endl;
+
+    //  NOTE:  This is prototype code for the KDL forward dynamics solver class
+    //         based on the Recurse Newton Euler Method:  ChainFdSolver_RNE
+
+    //  Dynamics Solver
+    Vector gravity(0.0, 0.0, -9.81);  // base frame
+    ChainDynParam DynSolver = KDL::ChainDynParam(motomansia10dyn, gravity);
+
+    unsigned int nj = motomansia10dyn.getNrOfJoints();
+    unsigned int ns = motomansia10dyn.getNrOfSegments();
+
+    // Joint position, velocity, and acceleration
+    JntArray q(nj);
+    JntArray qd(nj);
+    JntArray qdd(nj);
+
+    //  random
+    q(0) = 0.2;
+    q(1) = 0.6;
+    q(2) = 1.;
+    q(3) = 0.5;
+    q(4) = -1.4;
+    q(5) = 0.3;
+    q(6) = -0.8;
+
+    qd(0) = 1.;
+    qd(1) = -2.;
+    qd(2) = 3.;
+    qd(3) = -4.;
+    qd(4) = 5.;
+    qd(5) = -6.;
+    qd(6) = 7.;
+
+    // Validate FK
+    ChainFkSolverPos_recursive fksolver(motomansia10dyn);
+    Frame f_out;
+    fksolver.JntToCart(q,f_out);
+    CPPUNIT_ASSERT(Equal(-0.547, f_out.p(0), eps));
+    CPPUNIT_ASSERT(Equal(-0.301, f_out.p(1), eps));
+    CPPUNIT_ASSERT(Equal(0.924, f_out.p(2), eps));
+    CPPUNIT_ASSERT(Equal(0.503, f_out.M(0,0), eps));
+    CPPUNIT_ASSERT(Equal(0.286, f_out.M(0,1), eps));
+    CPPUNIT_ASSERT(Equal(-0.816, f_out.M(0,2), eps));
+    CPPUNIT_ASSERT(Equal(-0.859, f_out.M(1,0), eps));
+    CPPUNIT_ASSERT(Equal(0.269, f_out.M(1,1), eps));
+    CPPUNIT_ASSERT(Equal(-0.436, f_out.M(1,2), eps));
+    CPPUNIT_ASSERT(Equal(0.095, f_out.M(2,0), eps));
+    CPPUNIT_ASSERT(Equal(0.920, f_out.M(2,1), eps));
+    CPPUNIT_ASSERT(Equal(0.381, f_out.M(2,2), eps));
+
+    // Validate Jacobian
+    ChainJntToJacSolver jacsolver(motomansia10dyn);
+    Jacobian jac(nj);
+    jacsolver.JntToJac(q, jac);
+    double Jac[6][7] =
+        {{0.301,-0.553,0.185,0.019,0.007,-0.086,0.},
+        {-0.547,-0.112,-0.139,-0.376,-0.037,0.063,0.},
+        {0.,-0.596,0.105,-0.342,-0.026,-0.113,0.},
+        {0.,0.199,-0.553,0.788,-0.615,0.162,-0.816},
+        {0.,-0.980,-0.112,-0.392,-0.536,-0.803,-0.436},
+        {1.,0.,0.825,0.475,0.578,-0.573,0.381}};
+    for ( int i=0; i<6; i++ ) {
+        for ( int j=0; j<nj; j++ ) {
+            CPPUNIT_ASSERT(Equal(jac(i,j), Jac[i][j], eps));
+        }
+    }
+
+    // Return values
+    JntArray taugrav(nj);
+    JntArray taucor(nj);
+    JntSpaceInertiaMatrix H(nj), Heff(nj);
+
+    // Compute Dynamics (torque in N-m)
+    ret = DynSolver.JntToGravity(q, taugrav);
+    if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
+    CPPUNIT_ASSERT(Equal(0.000, taugrav(0), eps));
+    CPPUNIT_ASSERT(Equal(-36.672, taugrav(1), eps));
+    CPPUNIT_ASSERT(Equal(4.315, taugrav(2), eps));
+    CPPUNIT_ASSERT(Equal(-11.205, taugrav(3), eps));
+    CPPUNIT_ASSERT(Equal(0.757, taugrav(4), eps));
+    CPPUNIT_ASSERT(Equal(1.780, taugrav(5), eps));
+    CPPUNIT_ASSERT(Equal(0.000, taugrav(6), eps));
+
+    ret = DynSolver.JntToCoriolis(q, qd, taucor);
+    if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
+    CPPUNIT_ASSERT(Equal(-15.523, taucor(0), eps));
+    CPPUNIT_ASSERT(Equal(24.250, taucor(1), eps));
+    CPPUNIT_ASSERT(Equal(-6.862, taucor(2), eps));
+    CPPUNIT_ASSERT(Equal(6.303, taucor(3), eps));
+    CPPUNIT_ASSERT(Equal(0.110, taucor(4), eps));
+    CPPUNIT_ASSERT(Equal(-4.898, taucor(5), eps));
+    CPPUNIT_ASSERT(Equal(-0.249, taucor(6), eps));
+
+    ret = DynSolver.JntToMass(q, H);
+    if (ret < 0) std::cout << "KDL: inverse dynamics ERROR: " << ret << std::endl;
+    double Hexp[7][7] =
+        {{6.8687,-0.4333,0.4599,0.6892,0.0638,-0.0054,0.0381},
+        {-0.4333,8.8324,-0.5922,0.7905,0.0003,-0.0242,0.0265},
+        {0.4599,-0.5922,3.3496,-0.0253,0.1150,-0.0243,0.0814},
+        {0.6892,0.7905,-0.0253,3.9623,-0.0201,0.0087,-0.0291},
+        {0.0638,0.0003,0.1150,-0.0201,1.1234,0.0029,0.0955},
+        {-0.0054,-0.0242,-0.0243,0.0087,0.0029,1.1425,0},
+        {0.0381,0.0265,0.0814,-0.0291,0.0955,0,1.1000}};
+    for ( int i=0; i<nj; i++ ) {
+        for ( int j=0; j<nj; j++ ) {
+            CPPUNIT_ASSERT(Equal(H(i,j), Hexp[i][j], eps));
+        }
+    }
+
+    // Inverse Dynamics:
+    //   T = H * qdd + Tcor + Tgrav - J^T * Fext
+    // Forward Dynamics
+    //   1. Call JntToMass from ChainDynParam to get H
+    //   2. Call ID with qdd=0 to get T=Tcor+Tgrav+J^T*Fext
+    //   3. Calculate qdd = H^-1 * T
+    KDL::ChainIdSolver_RNE IdSolver = KDL::ChainIdSolver_RNE(motomansia10dyn, gravity);
+
+    // In tool coordinates
+    Vector f(10,-20,30) ;
+    Vector n(3,-4,5) ;
+    Wrench f_tool(f,n);
+    // In local link coordinates
+    Wrenches f_ext(ns);
+    for(int i=0;i<ns;i++){
+        SetToZero(f_ext[i]);
+    }
+    f_ext[ns-1]=f_tool;
+
+    JntArray Tnoninertial(nj);
+    JntArray jntarraynull(nj);
+    SetToZero(jntarraynull);
+    IdSolver.CartToJnt(q, qd, jntarraynull, f_ext, Tnoninertial);
+    CPPUNIT_ASSERT(Equal(-21.252, Tnoninertial(0), eps));
+    CPPUNIT_ASSERT(Equal(-37.933, Tnoninertial(1), eps));
+    CPPUNIT_ASSERT(Equal(-2.497, Tnoninertial(2), eps));
+    CPPUNIT_ASSERT(Equal(-15.289, Tnoninertial(3), eps));
+    CPPUNIT_ASSERT(Equal(-4.646, Tnoninertial(4), eps));
+    CPPUNIT_ASSERT(Equal(-9.201, Tnoninertial(5), eps));
+    CPPUNIT_ASSERT(Equal(-5.249, Tnoninertial(6), eps));
+
+    // get acceleration using inverse symmetric matrix times vector
+    Eigen::MatrixXd H_eig(nj,nj), L(nj,nj);
+    Eigen::VectorXd Tnon_eig(nj), D(nj), r(nj), acc_eig(nj);
+    for(int i=0;i<nj;i++){
+        Tnon_eig(i) =  -Tnoninertial(i);
+        for(int j=0;j<nj;j++){
+            H_eig(i,j) =  H(i,j);
+        }
+    }
+    ldl_solver_eigen(H_eig, Tnon_eig, L, D, r, acc_eig);
+    for(int i=0;i<nj;i++){
+        qdd(i) = acc_eig(i);
+    }
+    CPPUNIT_ASSERT(Equal(2.998, qdd(0), eps));
+    CPPUNIT_ASSERT(Equal(4.289, qdd(1), eps));
+    CPPUNIT_ASSERT(Equal(0.946, qdd(2), eps));
+    CPPUNIT_ASSERT(Equal(2.518, qdd(3), eps));
+    CPPUNIT_ASSERT(Equal(3.530, qdd(4), eps));
+    CPPUNIT_ASSERT(Equal(8.150, qdd(5), eps));
+    CPPUNIT_ASSERT(Equal(4.254, qdd(6), eps));
+}
+
+void SolverTest::FdSolverConsistencyTest()
+{
+    int ret;
+    double eps=1.e-3;
+
+    std::cout<<"KDL FD Solver Consistency Test for Motoman SIA10"<<std::endl;
+
+    //  NOTE:  Compute the forward and inverse dynamics and test for consistency
+
+    //  Forward Dynamics Solver
+    Vector gravity(0.0, 0.0, -9.81);  // base frame
+    KDL::ChainFdSolver_RNE FdSolver = KDL::ChainFdSolver_RNE(motomansia10dyn, gravity);
+
+    unsigned int nj = motomansia10dyn.getNrOfJoints();
+    unsigned int ns = motomansia10dyn.getNrOfSegments();
+
+    // Joint position, velocity, and acceleration
+    KDL::JntArray q(nj);
+    KDL::JntArray qd(nj);
+    KDL::JntArray qdd(nj);
+    KDL::JntArray tau(nj);
+
+    //  random
+    q(0) = 0.2;
+    q(1) = 0.6;
+    q(2) = 1.;
+    q(3) = 0.5;
+    q(4) = -1.4;
+    q(5) = 0.3;
+    q(6) = -0.8;
+
+    qd(0) = 1.;
+    qd(1) = -2.;
+    qd(2) = 3.;
+    qd(3) = -4.;
+    qd(4) = 5.;
+    qd(5) = -6.;
+    qd(6) = 7.;
+
+    // actuator torques
+    tau(0) = 50.;
+    tau(1) = -20.;
+    tau(2) = 10.;
+    tau(3) = 40.;
+    tau(4) = -60.;
+    tau(5) = 15.;
+    tau(6) = -10.;
+
+    KDL::Vector f(10,-20,30) ;
+    KDL::Vector n(3,-4,5) ;
+    KDL::Wrench f_tool(f,n);
+    // In local link coordinates
+    KDL::Wrenches f_ext(ns);
+    for(int i=0;i<ns;i++){
+        SetToZero(f_ext[i]);
+    }
+    f_ext[ns-1]=f_tool;
+
+    // Call FD function
+    ret = FdSolver.CartToJnt(q, qd, tau, f_ext, qdd);
+    if (ret < 0) std::cout << "KDL: forward dynamics ERROR: " << ret << std::endl;
+    CPPUNIT_ASSERT(Equal(9.486, qdd(0), eps));
+    CPPUNIT_ASSERT(Equal(1.830, qdd(1), eps));
+    CPPUNIT_ASSERT(Equal(4.726, qdd(2), eps));
+    CPPUNIT_ASSERT(Equal(11.665, qdd(3), eps));
+    CPPUNIT_ASSERT(Equal(-50.108, qdd(4), eps));
+    CPPUNIT_ASSERT(Equal(21.403, qdd(5), eps));
+    CPPUNIT_ASSERT(Equal(-0.381, qdd(6), eps));
+
+    // Check against ID solver for consistency
+    KDL::ChainIdSolver_RNE IdSolver = KDL::ChainIdSolver_RNE(motomansia10dyn, gravity);
+    KDL::JntArray torque(nj);
+    IdSolver.CartToJnt(q, qd, qdd, f_ext, torque);
+    for ( int i=0; i<nj; i++ )
+    {
+        CPPUNIT_ASSERT(Equal(torque(i), tau(i), eps));
+    }
+
+    return;
+}
+
+void SolverTest::LDLdecompTest()
+{
+    std::cout<<"LDL Solver Test"<<std::endl;
+    double eps=1.e-6;
+
+    //  Given A and b, solve Ax=b for x, where A is a symmetric real matrix
+    //  https://en.wikipedia.org/wiki/Cholesky_decomposition
+    Eigen::MatrixXd A(3,3), Aout(3,3);
+    Eigen::VectorXd b(3);
+    Eigen::MatrixXd L(3,3), Lout(3,3);
+    Eigen::VectorXd d(3), dout(3);
+    Eigen::VectorXd x(3), xout(3);
+    Eigen::VectorXd r(3);  // temp variable used internally by ldl solver
+    Eigen::MatrixXd Dout(3,3);  // diagonal matrix
+
+    // Given
+    A <<  4, 12, -16,
+         12, 37, -43,
+        -16, -43, 98;
+    b << 28, 117, 98;
+    // Results to verify
+    L <<  1, 0, 0,
+          3, 1, 0,
+          -4, 5, 1;
+    d << 4, 1, 9;
+    x << 3, 8, 5;
+
+    ldl_solver_eigen(A, b, Lout, dout, r, xout);
+
+    for(int i=0;i<3;i++){
+        for(int j=0;j<3;j++){
+            CPPUNIT_ASSERT(Equal(L(i,j), Lout(i,j), eps));
+        }
+    }
+
+    Dout.setZero();
+    for(int i=0;i<3;i++){
+        Dout(i,i) = dout(i);
+    }
+
+    // Verify solution for x
+    for(int i=0;i<3;i++){
+        CPPUNIT_ASSERT(Equal(xout(i), x(i), eps));
+    }
+
+    // Test reconstruction of A from LDL^T decomposition
+    Aout = Lout * Dout * Lout.transpose();
+    for(int i=0;i<3;i++){
+        for(int j=0;j<3;j++){
+            CPPUNIT_ASSERT(Equal(A(i,j), Aout(i,j), eps));
+        }
+    }
+
+    return;
+}
diff --git a/orocos_kdl/tests/solvertest.hpp b/orocos_kdl/tests/solvertest.hpp
index 26c310636..4beabecf0 100644
--- a/orocos_kdl/tests/solvertest.hpp
+++ b/orocos_kdl/tests/solvertest.hpp
@@ -18,6 +18,9 @@
 #include <chainidsolver_vereshchagin.hpp>
 #include <chainidsolver_recursive_newton_euler.hpp>
 #include <chaindynparam.hpp>
+#include <chainidsolver_recursive_newton_euler.hpp>
+#include <chainfdsolver_recursive_newton_euler.hpp>
+#include <utilities/ldl_solver_eigen.hpp>
 
 
 using namespace KDL;
@@ -34,6 +37,9 @@ class SolverTest : public CppUnit::TestFixture
     CPPUNIT_TEST(IkVelSolverWDLSTest );
     CPPUNIT_TEST(FkPosVectTest );
     CPPUNIT_TEST(FkVelVectTest );
+    CPPUNIT_TEST(FdSolverDevelopmentTest );
+    CPPUNIT_TEST(FdSolverConsistencyTest );
+    CPPUNIT_TEST(LDLdecompTest);
     CPPUNIT_TEST(UpdateChainTest );
     CPPUNIT_TEST_SUITE_END();
 
@@ -50,11 +56,14 @@ class SolverTest : public CppUnit::TestFixture
     void IkVelSolverWDLSTest();
     void FkPosVectTest();
     void FkVelVectTest();
+    void FdSolverDevelopmentTest();
+    void FdSolverConsistencyTest();
+    void LDLdecompTest();
     void UpdateChainTest();
 
 private:
 
-  Chain chain1,chain2,chain3,chain4, chaindyn,motomansia10;
+  Chain chain1, chain2, chain3, chain4, chaindyn, motomansia10, motomansia10dyn;
 
     void FkPosAndJacLocal(Chain& chain,ChainFkSolverPos& fksolverpos,ChainJntToJacSolver& jacsolver);
     void FkVelAndJacLocal(Chain& chain, ChainFkSolverVel& fksolvervel, ChainJntToJacSolver& jacsolver);
diff --git a/orocos_kdl/tests/treeinvdyntest.cpp b/orocos_kdl/tests/treeinvdyntest.cpp
new file mode 100644
index 000000000..c6f5b9cf8
--- /dev/null
+++ b/orocos_kdl/tests/treeinvdyntest.cpp
@@ -0,0 +1,177 @@
+#include "treeinvdyntest.hpp"
+#include <kinfam_io.hpp>
+#include <frames_io.hpp>
+#include <chainidsolver_recursive_newton_euler.hpp>
+#include <treeidsolver_recursive_newton_euler.hpp>
+#include <time.h>
+#include <cmath>
+
+
+CPPUNIT_TEST_SUITE_REGISTRATION( TreeInvDynTest );
+
+using namespace KDL;
+using std::cout;
+using std::endl;
+
+void TreeInvDynTest::setUp()
+{
+    srand( (unsigned)time( NULL ));
+
+    //spatial inertia (just to test dynamics)
+    RigidBodyInertia inertia(0.3, Vector(0.0, 0.1, 0.0), RotationalInertia(0.005, 0.001, 0.001));
+
+    //create chain #1
+    chain1.addSegment(Segment("Segment 11", Joint("Joint 11", Joint::RotZ),
+                              Frame(Vector(0.0,0.0,0.0)), inertia));
+    chain1.addSegment(Segment("Segment 12", Joint("Joint 12", Joint::RotX),
+                              Frame(Vector(0.0,0.0,0.9)), inertia));
+    chain1.addSegment(Segment("Segment 13", Joint("Joint 13", Joint::None),
+                              Frame(Vector(-0.4,0.0,0.0))));
+    chain1.addSegment(Segment("Segment 14", Joint("Joint 14", Joint::RotX),
+                              Frame(Vector(0.0,0.0,1.2)), inertia));
+    chain1.addSegment(Segment("Segment 15", Joint("Joint 15", Joint::None),
+                              Frame(Vector(0.4,0.0,0.0)), inertia));
+    chain1.addSegment(Segment("Segment 16", Joint("Joint 16", Joint::RotZ),
+                              Frame(Vector(0.0,0.0,1.4)), inertia));
+    chain1.addSegment(Segment("Segment 17", Joint("Joint 17", Joint::RotX),
+                              Frame(Vector(0.0,0.0,0.0)), inertia));
+    chain1.addSegment(Segment("Segment 18", Joint("Joint 18", Joint::RotZ),
+                              Frame(Vector(0.0,0.0,0.4)), inertia));
+    chain1.addSegment(Segment("Segment 19", Joint("Joint 19", Joint::None),
+                              Frame(Vector(0.0,0.0,0.0)), inertia));
+
+    //create chain #2
+    chain2.addSegment(Segment("Segment 21", Joint("Joint 21", Joint::RotZ),
+                              Frame(Vector(0.0,0.0,0.5)), inertia));
+    chain2.addSegment(Segment("Segment 22", Joint("Joint 22", Joint::RotX),
+                              Frame(Vector(0.0,0.0,0.4)), inertia));
+    chain2.addSegment(Segment("Segment 23", Joint("Joint 23", Joint::RotX),
+                              Frame(Vector(0.0,0.0,0.3)), inertia));
+    chain2.addSegment(Segment("Segment 24", Joint("Joint 24", Joint::RotX),
+                              Frame(Vector(0.0,0.0,0.2)), inertia));
+    chain2.addSegment(Segment("Segment 25", Joint("Joint 25", Joint::RotZ),
+                              Frame(Vector(0.0,0.0,0.1)), inertia));
+
+    //create tree as two chains attached to the root
+    tree = Tree();
+    tree.addChain(chain1, tree.getRootSegment()->first);
+    tree.addChain(chain2, tree.getRootSegment()->first);
+
+    //create a simple "y-shaped" tree
+    m=1.0, Iz=0.05, L=0.5; //geometric and dynamic parameters
+    Segment s1("S1", Joint("q1", Joint::RotZ), Frame(), RigidBodyInertia(m, Vector(L,0,0), RotationalInertia(0,0,Iz)));
+    Segment s2("S2", Joint("q2", Vector(L,0,0), Vector(0,0,1), Joint::RotAxis), Frame(Vector(L,0,0)), RigidBodyInertia(m, Vector(L/2,0,0), RotationalInertia(0,0,Iz)));
+    Segment s3("S3", Joint("q3", Vector(2*L,0,0), Vector(0,0,1), Joint::RotAxis), Frame(Vector(2*L,0,0)), RigidBodyInertia(m, Vector(L/2,0,0), RotationalInertia(0,0,Iz)));
+    ytree = Tree("root");
+    ytree.addSegment(s1, "root");
+    ytree.addSegment(s2, "S1");
+    ytree.addSegment(s3, "S1");
+}
+
+
+void TreeInvDynTest::tearDown() { }
+
+
+void TreeInvDynTest::UpdateTreeTest() {
+  // std::cout << "Tree: " << endl << tree2str(tree) << endl; //NOTE: tree2str is not available as I implemented it in another "parallel" commit
+  std::cout << "\nTree: " << endl << tree << endl;
+
+  Tree temp_tree(tree);
+  TreeIdSolver_RNE solver(temp_tree, Vector::Zero());
+  JntArray q, qd, qdd, tau;
+
+  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
+  q.resize(temp_tree.getNrOfJoints());
+  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
+  qd.resize(temp_tree.getNrOfJoints());
+  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
+  qdd.resize(temp_tree.getNrOfJoints());
+  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
+  tau.resize(temp_tree.getNrOfJoints());
+  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
+
+  temp_tree.addSegment(Segment("extra"), temp_tree.getRootSegment()->first);
+  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
+  solver.updateInternalDataStructures();
+  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
+}
+
+
+void TreeInvDynTest::TwoChainsTest() {
+  Vector gravity(0,0,-9.8);
+  TreeIdSolver_RNE solver(tree, gravity);
+  ChainIdSolver_RNE solver1(chain1, gravity);
+  ChainIdSolver_RNE solver2(chain2, gravity);
+
+  unsigned int nt = tree.getNrOfJoints();
+  unsigned int n1 = chain1.getNrOfJoints();
+  unsigned int n2 = chain2.getNrOfJoints();
+
+  //Check that sizes are consistent -- otherwise following code does not make sense!
+  CPPUNIT_ASSERT_EQUAL(nt, n1+n2);
+
+  JntArray q(nt), qd(nt), qdd(nt), tau(nt); //data related to tree
+  JntArray q1(n1), qd1(n1), qdd1(n1), tau1(n1); //data related to chain1
+  JntArray q2(n2), qd2(n2), qdd2(n2), tau2(n2); //data related to chain2
+
+  unsigned int iterations = 100;
+  while(iterations-- > 0) {
+    //Randomize joint vectors
+    for(unsigned int i=0; i<nt; i++) random(q(i)), random(qd(i)), random(qdd(i));
+    for(unsigned int i=0; i<n1; i++) q1(i)=q(i), qd1(i)=qd(i), qdd1(i)=qdd(i);
+    for(unsigned int i=0; i<n2; i++) q2(i)=q(i+n1), qd2(i)=qd(i+n1), qdd2(i)=qdd(i+n1);
+
+    //Evaluate efforts
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver1.CartToJnt(q1, qd1, qdd1, Wrenches(chain1.getNrOfSegments()), tau1));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver2.CartToJnt(q2, qd2, qdd2, Wrenches(chain2.getNrOfSegments()), tau2));
+
+    //compare efforts
+    JntArray tau12(tau.rows());
+    for(unsigned int i=0; i<n1; i++) tau12(i) = tau1(i);
+    for(unsigned int i=0; i<n2; i++) tau12(i+n1) = tau2(i);
+    CPPUNIT_ASSERT_EQUAL(tau, tau12);
+  }
+}
+
+
+void TreeInvDynTest::YTreeTest() {
+  std::cout << "\nY-shaped tree: " << endl << ytree << endl;
+
+  double g = 9.8;
+  TreeIdSolver_RNE solver(ytree, Vector(0,-g,0));
+
+  //Following is just a check in case the model is modified. In this case the
+  //analytic model derived using Euler-Lagrange equations would not be valid.
+  unsigned int dof = ytree.getNrOfJoints();
+  CPPUNIT_ASSERT_EQUAL(dof, (unsigned int)3); // The analytic euler-lagrange model was derived for a specific 3-dof model
+
+  JntArray q(dof), qd(dof), qdd(dof), tau(dof), eff(dof);
+
+  double mLL = m*L*L;
+  double Ie = Iz + 0.25*mLL;
+
+  unsigned int iterations = 100;
+  while(iterations-- > 0) {
+    //Randomize joint vectors
+    for(unsigned int i=0; i<dof; i++)
+      random(q(i)), random(qd(i)), random(qdd(i));
+
+    //Evaluate efforts using the numerical solver
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
+
+    //Evaluate efforts using the analytic model derived using Euler-Lagrange equations
+    double s2 = std::sin(q(1)), c2 = std::cos(q(1));
+    double s3 = std::sin(q(2)), c3 = std::cos(q(2));
+    double c1 = std::cos(q(0)), c12 = std::cos(q(0)+q(1)), c13 = std::cos(q(0)+q(2));
+    eff(0) = (3*Iz+6.5*mLL+mLL*c2+2*mLL*c3) * qdd(0) + (Ie+0.5*mLL*c2) * qdd(1)
+            + (Ie+mLL*c3) * qdd(2) - 0.5*mLL*s2*qd(1)*qd(1) - mLL*s3*qd(2)*qd(2)
+            - mLL*s2*qd(0)*qd(1) - 2*mLL*s3*qd(0)*qd(2) + m*g*(4*L*c1+0.5*L*c12+0.5*L*c13);
+    eff(1) = (Ie+0.5*mLL*c2) * qdd(0) + Ie * qdd(1) + 0.5*mLL*s2*qd(0)*qd(0) + 0.5*m*g*L*c12;
+    eff(2) = (Ie+mLL*c3) * qdd(0) + Ie * qdd(2) + mLL*s3*qd(0)*qd(0) + 0.5*m*g*L*c13;
+
+    //compare efforts
+    CPPUNIT_ASSERT_EQUAL(tau, eff);
+  }
+
+}
diff --git a/orocos_kdl/tests/treeinvdyntest.hpp b/orocos_kdl/tests/treeinvdyntest.hpp
new file mode 100644
index 000000000..6fb14f128
--- /dev/null
+++ b/orocos_kdl/tests/treeinvdyntest.hpp
@@ -0,0 +1,33 @@
+#ifndef KDL_TREE_INV_DYN_TEST_HPP
+#define KDL_TREE_INV_DYN_TEST_HPP
+
+#include <cppunit/extensions/HelperMacros.h>
+
+#include <chain.hpp>
+#include <tree.hpp>
+
+
+using namespace KDL;
+
+class TreeInvDynTest : public CppUnit::TestFixture
+{
+    CPPUNIT_TEST_SUITE(TreeInvDynTest);
+    CPPUNIT_TEST(UpdateTreeTest);
+    CPPUNIT_TEST(TwoChainsTest);
+    CPPUNIT_TEST(YTreeTest);
+    CPPUNIT_TEST_SUITE_END();
+
+public:
+    void setUp();
+    void tearDown();
+
+    void UpdateTreeTest();
+    void TwoChainsTest();
+    void YTreeTest();
+
+private:
+    Chain chain1,chain2;
+    Tree tree, ytree;
+    double m, Iz, L;
+};
+#endif
diff --git a/orocos_kdl/tests/velocityprofiletest.cpp b/orocos_kdl/tests/velocityprofiletest.cpp
index 149de6b72..fb52ed9e1 100644
--- a/orocos_kdl/tests/velocityprofiletest.cpp
+++ b/orocos_kdl/tests/velocityprofiletest.cpp
@@ -334,3 +334,45 @@ void VelocityProfileTest::TestTrapHalf_SetDuration_End()
 	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
 
 }
+
+void VelocityProfileTest::TestDirac_SetProfile()
+{
+    double time;
+    double pos1 = 10.0;
+    double pos2 = -1.0;
+
+    VelocityProfile_Dirac v;
+    v.SetProfile(pos1, pos2);
+
+    time = 0;
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(pos1, v.Pos(time), epsilon);
+
+    time = 1.0;
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon);
+
+    time = -1.0;
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(pos1, v.Pos(time), epsilon);
+}
+
+void VelocityProfileTest::TestDirac_SetProfileDuration()
+{
+    double time;
+    double pos1 = 10.0;
+    double pos2 = -1.0;
+    double duration = 5.0;
+    VelocityProfile_Dirac v;
+    v.SetProfileDuration(pos1, pos2, duration);
+
+    time = -1.0;
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(pos1, v.Pos(time), epsilon);
+
+    time = duration/2;
+    CPPUNIT_ASSERT_DOUBLES_EQUAL((pos1 + pos2)/2, v.Pos(time), epsilon);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL((pos2-pos1)/duration, v.Vel(time), epsilon);
+
+    time = duration;
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon);
+
+    time = duration + 1.0;
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon);
+}
diff --git a/orocos_kdl/tests/velocityprofiletest.hpp b/orocos_kdl/tests/velocityprofiletest.hpp
index fb6e2ce24..27b5494b9 100644
--- a/orocos_kdl/tests/velocityprofiletest.hpp
+++ b/orocos_kdl/tests/velocityprofiletest.hpp
@@ -4,6 +4,7 @@
 #include <cppunit/extensions/HelperMacros.h>
 #include <velocityprofile_trap.hpp>
 #include <velocityprofile_traphalf.hpp>
+#include <velocityprofile_dirac.hpp>
 
 class VelocityProfileTest : public CppUnit::TestFixture
 {
@@ -18,6 +19,9 @@ class VelocityProfileTest : public CppUnit::TestFixture
     CPPUNIT_TEST(TestTrapHalf_SetDuration_Start);
     CPPUNIT_TEST(TestTrapHalf_SetDuration_End);
 
+    CPPUNIT_TEST(TestDirac_SetProfile);
+    CPPUNIT_TEST(TestDirac_SetProfileDuration);
+
     CPPUNIT_TEST_SUITE_END();
 
 public:
@@ -33,6 +37,9 @@ class VelocityProfileTest : public CppUnit::TestFixture
     void TestTrapHalf_SetProfile_End();
     void TestTrapHalf_SetDuration_Start();
     void TestTrapHalf_SetDuration_End();
+
+    void TestDirac_SetProfile();
+    void TestDirac_SetProfileDuration();
 };
 
 #endif
diff --git a/orocos_kinematics_dynamics/CMakeLists.txt b/orocos_kinematics_dynamics/CMakeLists.txt
index 2b2e95004..6fea52f0d 100644
--- a/orocos_kinematics_dynamics/CMakeLists.txt
+++ b/orocos_kinematics_dynamics/CMakeLists.txt
@@ -1,4 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
+if(POLICY CMP0048)
+  cmake_policy(SET CMP0048 NEW)
+endif()
 project(orocos_kinematics_dynamics)
 find_package(catkin REQUIRED)
 catkin_metapackage()
diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt
index 35c1c3e29..e8c074e23 100644
--- a/python_orocos_kdl/CMakeLists.txt
+++ b/python_orocos_kdl/CMakeLists.txt
@@ -1,25 +1,62 @@
 cmake_minimum_required(VERSION 2.4.6)
+if(POLICY CMP0048)
+  cmake_policy(SET CMP0048 NEW)
+endif()
 
 project(python_orocos_kdl)
 
-find_package(orocos_kdl)
+find_package(orocos_kdl REQUIRED)
 include_directories(${orocos_kdl_INCLUDE_DIRS})
 link_directories(${orocos_kdl_LIBRARY_DIRS})
 
-SET(PYTHON_VERSION 2 CACHE STRING "Python Version")
+if(DEFINED ENV{ROS_PYTHON_VERSION})
+  SET(PYTHON_VERSION $ENV{ROS_PYTHON_VERSION} CACHE STRING "Python Version")
+else()
+  SET(PYTHON_VERSION 2 CACHE STRING "Python Version")
+endif()
+
 find_package(PythonInterp ${PYTHON_VERSION} REQUIRED)
 find_package(PythonLibs ${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR} REQUIRED)
-execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(plat_specific=True, prefix=''))" OUTPUT_VARIABLE PYTHON_SITE_PACKAGES OUTPUT_STRIP_TRAILING_WHITESPACE)
-list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
-find_package(SIP REQUIRED)
-include(SIPMacros)
+# get_python_lib in python3 produces path which isn't in sys.path: https://bugs.launchpad.net/ubuntu/+source/python3-stdlib-extensions/+bug/1832215
+# execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(plat_specific=True, prefix=''))" OUTPUT_VARIABLE PYTHON_SITE_PACKAGES OUTPUT_STRIP_TRAILING_WHITESPACE)
+set(PYTHON_SITE_PACKAGES_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/dist-packages")
+set(LIBRARY_NAME "PyKDL")
+
+message(DEPRECATION "PyKDL has been moved to PyBin11. PyKDL based on SIP might become depracted in the (near) future. To keep using the SIP based version set 'BUILD_PYKDL_PYBIND11' to OFF.")
+
+option(BUILD_PYKDL_PYBIND11 "Use PyBind11 instead of SIP" ON)
+if (BUILD_PYKDL_PYBIND11)
+  SET(PYTHON_MODULE_EXTENSION ".so")
+  add_subdirectory(pybind11)
+  pybind11_add_module(${LIBRARY_NAME}
+    PyKDL/pybind11/PyKDL.h
+    PyKDL/pybind11/PyKDL.cpp
+    PyKDL/pybind11/frames.cpp
+    PyKDL/pybind11/kinfam.cpp
+    PyKDL/pybind11/framevel.cpp
+    PyKDL/pybind11/dynamics.cpp)
+  target_link_libraries(${LIBRARY_NAME} PRIVATE ${orocos_kdl_LIBRARIES})
+  install(TARGETS ${LIBRARY_NAME} DESTINATION "${PYTHON_SITE_PACKAGES_INSTALL_DIR}")
+else (BUILD_PYKDL_PYBIND11)
+  list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
+  find_package(SIP REQUIRED)
+  include(SIPMacros)
+
+  include_directories(${SIP_INCLUDE_DIR} ${PYTHON_INCLUDE_DIRS})
 
-include_directories(${SIP_INCLUDE_DIR} ${PYTHON_INCLUDE_DIRS})
+  file(GLOB SIP_FILES "${CMAKE_CURRENT_SOURCE_DIR}/${LIBRARY_NAME}/sip/*.sip")
+  set(SIP_INCLUDES ${SIP_FILES})
+  if (${PYTHON_VERSION_MAJOR} GREATER_EQUAL "3")
+    set(SIP_EXTRA_OPTIONS "-o;-x;PYTHON2") # '-x' disables the feature
+  else()
+    set(SIP_EXTRA_OPTIONS "-o")
+  endif()
+  add_sip_python_module(${LIBRARY_NAME} ${LIBRARY_NAME}/sip/${LIBRARY_NAME}.sip ${orocos_kdl_LIBRARIES})
+endif(BUILD_PYKDL_PYBIND11)
 
-file(GLOB SIP_FILES "${CMAKE_CURRENT_SOURCE_DIR}/*.sip")
-set(SIP_INCLUDES ${SIP_FILES})
-set(SIP_EXTRA_OPTIONS "-o")
-set(PYTHON_SITE_PACKAGES_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITE_PACKAGES})
-add_sip_python_module(PyKDL PyKDL/PyKDL.sip ${orocos_kdl_LIBRARIES})
+install(FILES package.xml DESTINATION share/python_orocos_kdl)
 
-install(FILES package.xml DESTINATION share/python_orocos_kdl)
\ No newline at end of file
+find_package(catkin QUIET)
+if(catkin_FOUND)
+  catkin_add_env_hooks(python_orocos_kdl_site_packages SHELLS bash tcsh zsh DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks)
+endif()
diff --git a/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp b/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp
new file mode 100644
index 000000000..113981910
--- /dev/null
+++ b/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp
@@ -0,0 +1,39 @@
+//Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
+//
+//Version: 1.0
+//Author: Ruben Smits Ruben Smits <ruben dot smits at intermodalics dot eu>
+//Author: Zihan Chen <zihan dot chen dot jhu at gmail dot com>
+//Author: Matthijs van der Burgh <MatthijsBurgh at outlook dot com>
+//Maintainer: Ruben Smits Ruben Smits <ruben dot smits at intermodalics dot eu>
+//URL: http://www.orocos.org/kdl
+//
+//This library is free software; you can redistribute it and/or
+//modify it under the terms of the GNU Lesser General Public
+//License as published by the Free Software Foundation; either
+//version 2.1 of the License, or (at your option) any later version.
+//
+//This library is distributed in the hope that it will be useful,
+//but WITHOUT ANY WARRANTY; without even the implied warranty of
+//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+//Lesser General Public License for more details.
+//
+//You should have received a copy of the GNU Lesser General Public
+//License along with this library; if not, write to the Free Software
+//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+
+#include <kdl/config.h>
+#include "PyKDL.h"
+
+namespace py = pybind11;
+
+
+PYBIND11_MODULE(PyKDL, m)
+{
+    m.doc() = "Orocos KDL Python wrapper"; // optional module docstring
+    m.attr("__version__") = KDL_VERSION_STRING;
+    init_frames(m);
+    init_framevel(m);
+    init_kinfam(m);
+    init_dynamics(m);
+}
diff --git a/python_orocos_kdl/PyKDL/pybind11/PyKDL.h b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h
new file mode 100644
index 000000000..17256935d
--- /dev/null
+++ b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h
@@ -0,0 +1,34 @@
+//Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
+//
+//Version: 1.0
+//Author: Ruben Smits Ruben Smits <ruben dot smits at intermodalics dot eu>
+//Author: Zihan Chen <zihan dot chen dot jhu at gmail dot com>
+//Author: Matthijs van der Burgh <MatthijsBurgh at outlook dot com>
+//Maintainer: Ruben Smits Ruben Smits <ruben dot smits at intermodalics dot eu>
+//URL: http://www.orocos.org/kdl
+//
+//This library is free software; you can redistribute it and/or
+//modify it under the terms of the GNU Lesser General Public
+//License as published by the Free Software Foundation; either
+//version 2.1 of the License, or (at your option) any later version.
+//
+//This library is distributed in the hope that it will be useful,
+//but WITHOUT ANY WARRANTY; without even the implied warranty of
+//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+//Lesser General Public License for more details.
+//
+//You should have received a copy of the GNU Lesser General Public
+//License along with this library; if not, write to the Free Software
+//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+
+#include <pybind11/pybind11.h>
+#include <pybind11/eigen.h>
+#include <pybind11/operators.h>
+#include <pybind11/stl.h>
+
+
+void init_frames(pybind11::module &m);
+void init_framevel(pybind11::module &m);
+void init_kinfam(pybind11::module &m);
+void init_dynamics(pybind11::module &m);
diff --git a/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp
new file mode 100644
index 000000000..b63b08763
--- /dev/null
+++ b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp
@@ -0,0 +1,92 @@
+//Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
+//
+//Version: 1.0
+//Author: Ruben Smits Ruben Smits <ruben dot smits at intermodalics dot eu>
+//Author: Zihan Chen <zihan dot chen dot jhu at gmail dot com>
+//Author: Matthijs van der Burgh <MatthijsBurgh at outlook dot com>
+//Maintainer: Ruben Smits Ruben Smits <ruben dot smits at intermodalics dot eu>
+//URL: http://www.orocos.org/kdl
+//
+//This library is free software; you can redistribute it and/or
+//modify it under the terms of the GNU Lesser General Public
+//License as published by the Free Software Foundation; either
+//version 2.1 of the License, or (at your option) any later version.
+//
+//This library is distributed in the hope that it will be useful,
+//but WITHOUT ANY WARRANTY; without even the implied warranty of
+//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+//Lesser General Public License for more details.
+//
+//You should have received a copy of the GNU Lesser General Public
+//License along with this library; if not, write to the Free Software
+//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+
+#include <iostream>
+#include <iomanip>
+#include <kdl/chaindynparam.hpp>
+#include <kdl/jntspaceinertiamatrix.hpp>
+#include <kdl/kinfam_io.hpp>
+#include "PyKDL.h"
+
+namespace py = pybind11;
+using namespace KDL;
+
+
+void init_dynamics(pybind11::module &m)
+{
+    // --------------------
+    // JntSpaceInertiaMatrix
+    // --------------------
+    py::class_<JntSpaceInertiaMatrix> jnt_space_inertia_matrix(m, "JntSpaceInertiaMatrix");
+    jnt_space_inertia_matrix.def(py::init<>());
+    jnt_space_inertia_matrix.def(py::init<int>());
+    jnt_space_inertia_matrix.def(py::init<const JntSpaceInertiaMatrix&>());
+    jnt_space_inertia_matrix.def("resize", &JntSpaceInertiaMatrix::resize);
+    jnt_space_inertia_matrix.def("rows", &JntSpaceInertiaMatrix::rows);
+    jnt_space_inertia_matrix.def("columns", &JntSpaceInertiaMatrix::columns);
+    jnt_space_inertia_matrix.def("__getitem__", [](const JntSpaceInertiaMatrix &jm, std::tuple<int, int> idx)
+    {
+        int i = std::get<0>(idx);
+        int j = std::get<1>(idx);
+        if (i < 0 || i >= jm.rows() || j < 0 || j >= jm.columns())
+            throw py::index_error("Inertia index out of range");
+
+        return jm((unsigned int)i, (unsigned int)j);
+    });
+    jnt_space_inertia_matrix.def("__setitem__", [](JntSpaceInertiaMatrix &jm, std::tuple<int, int> idx, double value)
+    {
+        int i = std::get<0>(idx);
+        int j = std::get<1>(idx);
+        if (i < 0 || i >= jm.rows() || j < 0 || j >= jm.columns())
+            throw py::index_error("Inertia index out of range");
+
+        jm((unsigned int)i, (unsigned int)j) = value;
+    });
+    jnt_space_inertia_matrix.def("__repr__", [](const JntSpaceInertiaMatrix &jm)
+    {
+        std::ostringstream oss;
+        oss << jm;
+        return oss.str();
+    });
+    jnt_space_inertia_matrix.def(py::self == py::self);
+
+    m.def("Add", (void (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&, JntSpaceInertiaMatrix&)) &KDL::Add);
+    m.def("Subtract", (void (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&,JntSpaceInertiaMatrix&)) &KDL::Subtract);
+    m.def("Multiply", (void (*)(const JntSpaceInertiaMatrix&, const double&, JntSpaceInertiaMatrix&)) &KDL::Multiply);
+    m.def("Divide", (void (*)(const JntSpaceInertiaMatrix&, const double&, JntSpaceInertiaMatrix&)) &KDL::Divide);
+    m.def("Multiply", (void (*)(const JntSpaceInertiaMatrix&, const JntArray&, JntArray&)) &KDL::Multiply);
+    m.def("SetToZero", (void (*)(JntSpaceInertiaMatrix&)) &KDL::SetToZero);
+    m.def("Equal", (bool (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&, double)) &KDL::Equal,
+          py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon);
+
+
+    // --------------------
+    // ChainDynParam
+    // --------------------
+    py::class_<ChainDynParam> chain_dyn_param(m, "ChainDynParam");
+    chain_dyn_param.def(py::init<const Chain&, Vector>());
+    chain_dyn_param.def("JntToCoriolis", &ChainDynParam::JntToCoriolis);
+    chain_dyn_param.def("JntToMass", &ChainDynParam::JntToMass);
+    chain_dyn_param.def("JntToGravity", &ChainDynParam::JntToGravity);
+}
diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp
new file mode 100644
index 000000000..09ce6dd68
--- /dev/null
+++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp
@@ -0,0 +1,493 @@
+//Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
+//
+//Version: 1.0
+//Author: Ruben Smits Ruben Smits <ruben dot smits at intermodalics dot eu>
+//Author: Zihan Chen <zihan dot chen dot jhu at gmail dot com>
+//Author: Matthijs van der Burgh <MatthijsBurgh at outlook dot com>
+//Maintainer: Ruben Smits Ruben Smits <ruben dot smits at intermodalics dot eu>
+//URL: http://www.orocos.org/kdl
+//
+//This library is free software; you can redistribute it and/or
+//modify it under the terms of the GNU Lesser General Public
+//License as published by the Free Software Foundation; either
+//version 2.1 of the License, or (at your option) any later version.
+//
+//This library is distributed in the hope that it will be useful,
+//but WITHOUT ANY WARRANTY; without even the implied warranty of
+//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+//Lesser General Public License for more details.
+//
+//You should have received a copy of the GNU Lesser General Public
+//License along with this library; if not, write to the Free Software
+//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+
+#include <kdl/frames.hpp>
+#include <kdl/frames_io.hpp>
+#include "PyKDL.h"
+
+namespace py = pybind11;
+using namespace KDL;
+
+
+void init_frames(py::module &m)
+{
+    // --------------------
+    // Vector
+    // --------------------
+    py::class_<Vector> vector(m, "Vector");
+    vector.def(py::init<>());
+    vector.def(py::init<double, double, double>());
+    vector.def(py::init<const Vector&>());
+    vector.def("x", (void (Vector::*)(double)) &Vector::x);
+    vector.def("y", (void (Vector::*)(double)) &Vector::y);
+    vector.def("z", (void (Vector::*)(double)) &Vector::z);
+    vector.def("x", (double (Vector::*)(void) const) &Vector::x);
+    vector.def("y", (double (Vector::*)(void) const) &Vector::y);
+    vector.def("z", (double (Vector::*)(void) const) &Vector::z);
+    vector.def("__getitem__", [](const Vector &v, int i)
+    {
+        if (i < 0 || i > 2)
+            throw py::index_error("Vector index out of range");
+
+        return v(i);
+    });
+    vector.def("__setitem__", [](Vector &v, int i, double value)
+    {
+        if (i < 0 || i > 2)
+            throw py::index_error("Vector index out of range");
+
+        v(i) = value;
+    });
+    vector.def("__repr__", [](const Vector &v)
+    {
+        std::ostringstream oss;
+        oss << v;
+        return oss.str();
+    });
+    vector.def("ReverseSign", &Vector::ReverseSign);
+    vector.def(py::self -= py::self);
+    vector.def(py::self += py::self);
+    vector.def(py::self + py::self);
+    vector.def(py::self - py::self);
+    vector.def(py::self * double());
+    vector.def(double() * py::self);
+    vector.def(py::self / double());
+    vector.def(py::self * py::self);
+    vector.def(py::self == py::self);
+    vector.def(py::self != py::self);
+    vector.def("__neg__", [](const Vector &a)
+    {
+        return operator-(a);
+    }, py::is_operator());
+    vector.def("__copy__", [](const Vector& self)
+    {
+        return Vector(self);
+    });
+    vector.def("__deepcopy__", [](const Vector& self, py::dict)
+    {
+        return Vector(self);
+    }, py::arg("memo"));
+    vector.def_static("Zero", &Vector::Zero);
+    vector.def("Norm", static_cast<double (Vector::*)() const>(&Vector::Norm));
+    vector.def("Norm", static_cast<double (Vector::*)(double) const>(&Vector::Norm), py::arg("eps"));
+    // vector.def("Norm", static_cast<aaa>(&Vector::Norm));
+    vector.def("Normalize", &Vector::Normalize, py::arg("eps")=epsilon);
+    vector.def(py::pickle(
+            [](const Vector &v)
+            { // __getstate__
+                /* Return a tuple that fully encodes the state of the object */
+                return py::make_tuple(v.x(), v.y(), v.z());
+            },
+            [](py::tuple t)
+            { // __setstate__
+                if (t.size() != 3)
+                    throw std::runtime_error("Invalid state!");
+
+                /* Create a new C++ instance */
+                Vector v(t[0].cast<double>(), t[1].cast<double>(), t[2].cast<double>());
+                return v;
+            }));
+
+    m.def("SetToZero", (void (*)(Vector&)) &KDL::SetToZero);
+    m.def("dot", (double (*)(const Vector&, const Vector&)) &KDL::dot);
+    m.def("Equal", (bool (*)(const Vector&, const Vector&, double)) &KDL::Equal,
+          py::arg("a"), py::arg("b"), py::arg("eps")=epsilon);
+
+
+    // --------------------
+    // Wrench
+    // --------------------
+    py::class_<Wrench> wrench(m, "Wrench");
+    wrench.def(py::init<>());
+    wrench.def(py::init<const Vector&, const Vector&>());
+    wrench.def(py::init<const Wrench&>());
+    wrench.def_readwrite("force", &Wrench::force);
+    wrench.def_readwrite("torque", &Wrench::torque);
+    wrench.def("__getitem__", [](const Wrench &t, int i)
+    {
+        if (i < 0 || i > 5)
+            throw py::index_error("Wrench index out of range");
+
+        return t(i);
+    });
+    wrench.def("__setitem__", [](Wrench &t, int i, double value)
+    {
+        if (i < 0 || i > 5)
+            throw py::index_error("Wrench index out of range");
+
+        t(i) = value;
+    });
+    wrench.def("__repr__", [](const Wrench &t)
+    {
+        std::ostringstream oss;
+        oss << t;
+        return oss.str();
+    });
+    wrench.def("__copy__", [](const Wrench& self)
+    {
+        return Wrench(self);
+    });
+    wrench.def("__deepcopy__", [](const Wrench& self, py::dict)
+    {
+        return Wrench(self);
+    }, py::arg("memo"));
+    wrench.def_static("Zero", &Wrench::Zero);
+    wrench.def("ReverseSign", &Wrench::ReverseSign);
+    wrench.def("RefPoint", &Wrench::RefPoint);
+    wrench.def(py::self -= py::self);
+    wrench.def(py::self += py::self);
+    wrench.def(py::self * double());
+    wrench.def(double() * py::self);
+    wrench.def(py::self / double());
+    wrench.def(py::self + py::self);
+    wrench.def(py::self - py::self);
+    wrench.def(py::self == py::self);
+    wrench.def(py::self != py::self);
+    wrench.def("__neg__", [](const Wrench &w)
+    {
+        return operator-(w);
+    }, py::is_operator());
+    wrench.def(py::pickle(
+            [](const Wrench &wr)
+            { // __getstate__
+                /* Return a tuple that fully encodes the state of the object */
+                return py::make_tuple(wr.force, wr.torque);
+            },
+            [](py::tuple t)
+            { // __setstate__
+                if (t.size() != 2)
+                    throw std::runtime_error("Invalid state!");
+
+                /* Create a new C++ instance */
+                Wrench wr(t[0].cast<Vector>(), t[1].cast<Vector>());
+                return wr;
+            }));
+
+    m.def("SetToZero", (void (*)(Wrench&)) &KDL::SetToZero);
+    m.def("Equal", (bool (*)(const Wrench&, const Wrench&, double eps)) &KDL::Equal,
+          py::arg("a"), py::arg("b"), py::arg("eps")=epsilon);
+
+
+    // --------------------
+    // Twist
+    // --------------------
+    py::class_<Twist> twist(m, "Twist");
+    twist.def(py::init<>());
+    twist.def(py::init<const Vector&, const Vector&>());
+    twist.def(py::init<const Twist&>());
+    twist.def_readwrite("vel", &Twist::vel);
+    twist.def_readwrite("rot", &Twist::rot);
+    twist.def("__getitem__", [](const Twist &t, int i)
+    {
+        if (i < 0 || i > 5)
+            throw py::index_error("Twist index out of range");
+
+        return t(i);
+    });
+    twist.def("__setitem__", [](Twist &t, int i, double value)
+    {
+        if (i < 0 || i > 5)
+            throw py::index_error("Twist index out of range");
+
+        t(i) = value;
+    });
+    twist.def("__repr__", [](const Twist &t)
+    {
+        std::ostringstream oss;
+        oss << t;
+        return oss.str();
+    });
+    twist.def("__copy__", [](const Twist& self)
+    {
+        return Twist(self);
+    });
+    twist.def("__deepcopy__", [](const Twist& self, py::dict)
+    {
+        return Twist(self);
+    }, py::arg("memo"));
+    twist.def_static("Zero", &Twist::Zero);
+    twist.def("ReverseSign", &Twist::ReverseSign);
+    twist.def("RefPoint", &Twist::RefPoint);
+    twist.def(py::self -= py::self);
+    twist.def(py::self += py::self);
+    twist.def(py::self * double());
+    twist.def(double() * py::self);
+    twist.def(py::self / double());
+    twist.def(py::self + py::self);
+    twist.def(py::self - py::self);
+    twist.def(py::self == py::self);
+    twist.def(py::self != py::self);
+    twist.def("__neg__", [](const Twist &a)
+    {
+        return operator-(a);
+    }, py::is_operator());
+    twist.def(py::pickle(
+            [](const Twist &tt)
+            { // __getstate__
+                /* Return a tuple that fully encodes the state of the object */
+                return py::make_tuple(tt.vel, tt.rot);
+            },
+            [](py::tuple t)
+            { // __setstate__
+                if (t.size() != 2)
+                    throw std::runtime_error("Invalid state!");
+
+                /* Create a new C++ instance */
+                Twist tt(t[0].cast<Vector>(), t[1].cast<Vector>());
+                return tt;
+            }));
+
+    m.def("dot", (double (*)(const Twist&, const Wrench&)) &KDL::dot);
+    m.def("dot", (double (*)(const Wrench&, const Twist&)) &KDL::dot);
+    m.def("SetToZero", (void (*)(Twist&)) &KDL::SetToZero);
+    m.def("Equal", (bool (*)(const Twist&, const Twist&, double eps)) &KDL::Equal,
+          py::arg("a"), py::arg("b"), py::arg("eps")=epsilon);
+
+
+    // --------------------
+    // Rotation
+    // --------------------
+    py::class_<Rotation> rotation(m, "Rotation");
+    rotation.def(py::init<>());
+    rotation.def(py::init<double, double, double, double, double, double, double, double, double>());
+    rotation.def(py::init<const Vector&, const Vector&, const Vector&>());
+    rotation.def(py::init<const Rotation&>());
+    rotation.def("__getitem__", [](const Rotation &r, std::tuple<int, int> idx)
+    {
+        int i = std::get<0>(idx);
+        int j = std::get<1>(idx);
+        if (i < 0 || i > 2 || j < 0 || j > 2)
+            throw py::index_error("Rotation index out of range");
+
+        return r(i, j);
+    });
+    rotation.def("__setitem__", [](Rotation &r, std::tuple<int, int> idx, double value)
+    {
+        int i = std::get<0>(idx);
+        int j = std::get<1>(idx);
+        if (i < 0 || i > 2 || j < 0 || j > 2)
+            throw py::index_error("Rotation index out of range");
+
+        r(i, j) = value;
+    });
+    rotation.def("__repr__", [](const Rotation &r)
+    {
+            std::ostringstream oss;
+            oss << r;
+            return oss.str();
+    });
+    rotation.def("__copy__", [](const Rotation& self)
+    {
+        return Rotation(self);
+    });
+    rotation.def("__deepcopy__", [](const Rotation& self, py::dict)
+    {
+        return Rotation(self);
+    }, py::arg("memo"));
+    rotation.def("SetInverse", &Rotation::SetInverse);
+    rotation.def("Inverse", (Rotation (Rotation::*)(void) const) &Rotation::Inverse);
+    rotation.def("Inverse", (Vector (Rotation::*)(const Vector&) const) &Rotation::Inverse);
+    rotation.def("Inverse", (Wrench (Rotation::*)(const Wrench&) const) &Rotation::Inverse);
+    rotation.def("Inverse", (Twist (Rotation::*)(const Twist&) const) &Rotation::Inverse);
+    rotation.def_static("Identity", &Rotation::Identity);
+    rotation.def_static("RotX", &Rotation::RotX);
+    rotation.def_static("RotY", &Rotation::RotY);
+    rotation.def_static("RotZ", &Rotation::RotZ);
+    rotation.def_static("Rot", &Rotation::Rot);
+    rotation.def_static("Rot2", &Rotation::Rot2);
+    rotation.def_static("EulerZYZ", &Rotation::EulerZYZ);
+    rotation.def_static("RPY", &Rotation::RPY);
+    rotation.def_static("EulerZYX", &Rotation::EulerZYX);
+    rotation.def_static("Quaternion", &Rotation::Quaternion);
+    rotation.def("DoRotX", &Rotation::DoRotX);
+    rotation.def("DoRotY", &Rotation::DoRotY);
+    rotation.def("DoRotZ", &Rotation::DoRotZ);
+    rotation.def("GetRot", &Rotation::GetRot);
+    rotation.def("GetRotAngle", [](const Rotation &r, double eps)
+    {
+        Vector axis;
+        double ret = r.GetRotAngle(axis, eps);
+        return py::make_tuple(ret, axis);
+    }, py::arg("eps") = epsilon);
+    rotation.def("GetEulerZYZ", [](const Rotation &r)
+    {
+        double Alfa, Beta, Gamma;
+        r.GetEulerZYZ(Alfa, Beta, Gamma);
+        return py::make_tuple(Alfa, Beta, Gamma);
+    });
+    rotation.def("GetRPY", [](const Rotation &r)
+    {
+        double roll, pitch, yaw;
+        r.GetRPY(roll, pitch, yaw);
+        return py::make_tuple(roll, pitch, yaw);
+    });
+    rotation.def("GetEulerZYX", [](const Rotation &r)
+    {
+        double Alfa, Beta, Gamma;
+        r.GetEulerZYX(Alfa, Beta, Gamma);
+        return py::make_tuple(Alfa, Beta, Gamma);
+    });
+    rotation.def("GetQuaternion", [](const Rotation &r)
+    {
+        double x, y, z, w;
+        r.GetQuaternion(x, y, z, w);
+        return py::make_tuple(x, y, z, w);
+    });
+    rotation.def("UnitX", (Vector (Rotation::*)() const) &Rotation::UnitX);
+    rotation.def("UnitY", (Vector (Rotation::*)() const) &Rotation::UnitY);
+    rotation.def("UnitZ", (Vector (Rotation::*)() const) &Rotation::UnitZ);
+    rotation.def("UnitX", (void (Rotation::*)(const Vector&)) &Rotation::UnitX);
+    rotation.def("UnitY", (void (Rotation::*)(const Vector&)) &Rotation::UnitY);
+    rotation.def("UnitZ", (void (Rotation::*)(const Vector&)) &Rotation::UnitZ);
+    rotation.def(py::self * Vector());
+    rotation.def(py::self * Twist());
+    rotation.def(py::self * Wrench());
+    rotation.def(py::self == py::self);
+    rotation.def(py::self != py::self);
+    rotation.def(py::self * py::self);
+    rotation.def(py::pickle(
+            [](const Rotation &rot)
+            { // __getstate__
+                /* Return a tuple that fully encodes the state of the object */
+                double roll{0}, pitch{0}, yaw{0};
+                rot.GetRPY(roll, pitch, yaw);
+                return py::make_tuple(roll, pitch, yaw);
+            },
+            [](py::tuple t)
+            { // __setstate__
+                if (t.size() != 3)
+                    throw std::runtime_error("Invalid state!");
+
+                /* Create a new C++ instance */
+                return Rotation::RPY(t[0].cast<double>(), t[1].cast<double>(), t[2].cast<double>());
+            }));
+
+    m.def("Equal", (bool (*)(const Rotation&, const Rotation&, double eps)) &KDL::Equal,
+          py::arg("a"), py::arg("b"), py::arg("eps")=epsilon);
+
+
+    // --------------------
+    // Frame
+    // --------------------
+    py::class_<Frame> frame(m, "Frame");
+    frame.def(py::init<const Rotation&, const Vector&>());
+    frame.def(py::init<const Vector&>());
+    frame.def(py::init<const Rotation&>());
+    frame.def(py::init<const Frame&>());
+    frame.def(py::init<>());
+    frame.def_readwrite("M", &Frame::M);
+    frame.def_readwrite("p", &Frame::p);
+    frame.def("__getitem__", [](const Frame &frm, std::tuple<int, int> idx)
+    {
+        int i = std::get<0>(idx);
+        int j = std::get<1>(idx);
+        if (i < 0 || i > 2 || j < 0 || j > 3)
+            throw py::index_error("Frame index out of range");
+
+        return frm(i, j);
+    });
+    frame.def("__setitem__", [](Frame &frm, std::tuple<int, int> idx, double value)
+    {
+        int i = std::get<0>(idx);
+        int j = std::get<1>(idx);
+        if (i < 0 || i > 2 || j < 0 || j > 3)
+            throw py::index_error("Frame index out of range");
+
+        if (j == 3)
+            frm.p(i) = value;
+        else
+            frm.M(i, j) = value;
+    });
+    frame.def("__repr__", [](const Frame &frm)
+    {
+        std::ostringstream oss;
+        oss << frm;
+        return oss.str();
+    });
+    frame.def("__copy__", [](const Frame& self)
+    {
+        return Frame(self);
+    });
+    frame.def("__deepcopy__", [](const Frame& self, py::dict)
+    {
+        return Frame(self);
+    }, py::arg("memo"));
+    frame.def("DH_Craig1989", &Frame::DH_Craig1989);
+    frame.def("DH", &Frame::DH);
+    frame.def("Inverse", (Frame (Frame::*)() const) &Frame::Inverse);
+    frame.def("Inverse", (Vector (Frame::*)(const Vector&) const) &Frame::Inverse);
+    frame.def("Inverse", (Wrench (Frame::*)(const Wrench&) const) &Frame::Inverse);
+    frame.def("Inverse", (Twist (Frame::*)(const Twist&) const) &Frame::Inverse);
+    frame.def_static("Identity", &Frame::Identity);
+    frame.def("Integrate", &Frame::Integrate);
+    frame.def(py::self * Vector());
+    frame.def(py::self * Wrench());
+    frame.def(py::self * Twist());
+    frame.def(py::self * py::self);
+    frame.def(py::self == py::self);
+    frame.def(py::self != py::self);
+    frame.def(py::pickle(
+            [](const Frame &frm)
+            { // __getstate__
+                /* Return a tuple that fully encodes the state of the object */
+                return py::make_tuple(frm.M, frm.p);
+            },
+            [](py::tuple t)
+            { // __setstate__
+                if (t.size() != 2)
+                    throw std::runtime_error("Invalid state!");
+
+                /* Create a new C++ instance */
+                Frame frm(t[0].cast<Rotation>(), t[1].cast<Vector>());
+                return frm;
+            }));
+
+    m.def("Equal", (bool (*)(const Frame&, const Frame&, double eps)) &KDL::Equal,
+          py::arg("a"), py::arg("b"), py::arg("eps")=epsilon);
+
+
+    // --------------------
+    // Global
+    // --------------------
+    m.def("diff", (Vector (*)(const Vector&, const Vector&, double dt)) &KDL::diff,
+          py::arg("a"), py::arg("b"), py::arg("dt") = 1);
+    m.def("diff", (Vector (*)(const Rotation&, const Rotation&, double dt)) &KDL::diff,
+          py::arg("a"), py::arg("b"), py::arg("dt") = 1);
+    m.def("diff", (Twist (*)(const Frame&, const Frame&, double dt)) &KDL::diff,
+          py::arg("a"), py::arg("b"), py::arg("dt") = 1);
+    m.def("diff", (Twist (*)(const Twist&, const Twist&, double dt)) &KDL::diff,
+          py::arg("a"), py::arg("b"), py::arg("dt") = 1);
+    m.def("diff", (Wrench (*)(const Wrench&, const Wrench&, double dt)) &KDL::diff,
+          py::arg("a"), py::arg("b"), py::arg("dt") = 1);
+    m.def("addDelta", (Vector (*)(const Vector&, const Vector&, double dt)) &KDL::addDelta,
+          py::arg("a"), py::arg("da"), py::arg("dt") = 1);
+    m.def("addDelta", (Rotation (*)(const Rotation&, const Vector&, double dt)) &KDL::addDelta,
+          py::arg("a"), py::arg("da"), py::arg("dt") = 1);
+    m.def("addDelta", (Frame (*)(const Frame&, const Twist&, double dt)) &KDL::addDelta,
+          py::arg("a"), py::arg("da"), py::arg("dt") = 1);
+    m.def("addDelta", (Twist (*)(const Twist&, const Twist&, double dt)) &KDL::addDelta,
+          py::arg("a"), py::arg("da"), py::arg("dt") = 1);
+    m.def("addDelta", (Wrench (*)(const Wrench&, const Wrench&, double dt)) &KDL::addDelta,
+          py::arg("a"), py::arg("da"), py::arg("dt") = 1);
+}
diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp
new file mode 100644
index 000000000..635b4512f
--- /dev/null
+++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp
@@ -0,0 +1,420 @@
+//Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
+//
+//Version: 1.0
+//Author: Ruben Smits Ruben Smits <ruben dot smits at intermodalics dot eu>
+//Author: Zihan Chen <zihan dot chen dot jhu at gmail dot com>
+//Author: Matthijs van der Burgh <MatthijsBurgh at outlook dot com>
+//Maintainer: Ruben Smits Ruben Smits <ruben dot smits at intermodalics dot eu>
+//URL: http://www.orocos.org/kdl
+//
+//This library is free software; you can redistribute it and/or
+//modify it under the terms of the GNU Lesser General Public
+//License as published by the Free Software Foundation; either
+//version 2.1 of the License, or (at your option) any later version.
+//
+//This library is distributed in the hope that it will be useful,
+//but WITHOUT ANY WARRANTY; without even the implied warranty of
+//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+//Lesser General Public License for more details.
+//
+//You should have received a copy of the GNU Lesser General Public
+//License along with this library; if not, write to the Free Software
+//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+
+#include <kdl/framevel.hpp>
+#include <kdl/framevel_io.hpp>
+#include "PyKDL.h"
+
+namespace py = pybind11;
+using namespace KDL;
+
+
+void init_framevel(pybind11::module &m)
+{
+    // --------------------
+    // doubleVel
+    // --------------------
+    py::class_<doubleVel > double_vel(m, "doubleVel");
+    double_vel.def(py::init<>());
+    double_vel.def(py::init<const double>());
+    double_vel.def(py::init<const double, const double>());
+    double_vel.def(py::init<const doubleVel&>());
+    double_vel.def_readwrite("t", &doubleVel::t);
+    double_vel.def_readwrite("grad", &doubleVel::grad);
+    double_vel.def("value", &doubleVel::value);
+    double_vel.def("deriv", &doubleVel::deriv);
+    double_vel.def("__repr__", [](const doubleVel &d)
+    {
+        std::ostringstream oss;
+        oss << d;
+        return oss.str();
+    });
+    double_vel.def("__copy__", [](const doubleVel& self)
+    {
+        return doubleVel(self);
+    });
+    double_vel.def("__deepcopy__", [](const doubleVel& self, py::dict)
+    {
+        return doubleVel(self);
+    }, py::arg("memo"));
+
+    double_vel.def(py::self == py::self);
+    double_vel.def(py::self != py::self);
+
+    double_vel.def("__neg__", [](const doubleVel &a)
+    {
+        return operator-(a);
+    }, py::is_operator());
+
+    m.def("diff", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::diff,
+          py::arg("a"), py::arg("b"), py::arg("dt")=1.0);
+    m.def("addDelta", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::addDelta,
+          py::arg("a"), py::arg("da"), py::arg("dt")=1.0);
+    m.def("Equal", (bool (*)(const doubleVel&, const doubleVel&, double)) &KDL::Equal,
+          py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon);
+
+
+    // --------------------
+    // VectorVel
+    // --------------------
+    py::class_<VectorVel> vector_vel(m, "VectorVel");
+    vector_vel.def_readwrite("p", &VectorVel::p);
+    vector_vel.def_readwrite("v", &VectorVel::v);
+    vector_vel.def(py::init<>());
+    vector_vel.def(py::init<const Vector&, const Vector&>());
+    vector_vel.def(py::init<const Vector&>());
+    vector_vel.def(py::init<const VectorVel&>());
+    vector_vel.def("value", &VectorVel::value);
+    vector_vel.def("deriv", &VectorVel::deriv);
+    vector_vel.def("__repr__", [](const VectorVel &vv)
+    {
+        std::ostringstream oss;
+        oss << vv;
+        return oss.str();
+    });
+    vector_vel.def("__copy__", [](const VectorVel& self)
+    {
+        return VectorVel(self);
+    });
+    vector_vel.def("__deepcopy__", [](const VectorVel& self, py::dict)
+    {
+        return VectorVel(self);
+    }, py::arg("memo"));
+    vector_vel.def_static("Zero", &VectorVel::Zero);
+    vector_vel.def("ReverseSign", &VectorVel::ReverseSign);
+    vector_vel.def("Norm", &VectorVel::Norm, py::arg("eps")=epsilon);
+    vector_vel.def(py::self += py::self);
+    vector_vel.def(py::self -= py::self);
+    vector_vel.def(py::self + py::self);
+    vector_vel.def(py::self - py::self);
+    vector_vel.def(Vector() + py::self);
+    vector_vel.def(Vector() - py::self);
+    vector_vel.def(py::self + Vector());
+    vector_vel.def(py::self - Vector());
+
+    vector_vel.def(py::self * py::self);
+    vector_vel.def(py::self * Vector());
+    vector_vel.def(Vector() * py::self);
+    vector_vel.def(double() * py::self);
+    vector_vel.def(py::self * double());
+    vector_vel.def(doubleVel() * py::self);
+    vector_vel.def(py::self * doubleVel());
+    vector_vel.def(Rotation() * py::self);
+
+    vector_vel.def(py::self / double());
+    vector_vel.def(py::self / doubleVel());
+
+    vector_vel.def(py::self == py::self);
+    vector_vel.def(py::self != py::self);
+    vector_vel.def(Vector() == py::self);
+    vector_vel.def(Vector() != py::self);
+    vector_vel.def(py::self == Vector());
+    vector_vel.def(py::self != Vector());
+    vector_vel.def("__neg__", [](const VectorVel &a)
+    {
+        return operator-(a);
+    }, py::is_operator());
+    vector_vel.def(py::pickle(
+            [](const VectorVel &vv)
+            { // __getstate__
+                /* Return a tuple that fully encodes the state of the object */
+                return py::make_tuple(vv.p, vv.v);
+            },
+            [](py::tuple t)
+            { // __setstate__
+                if (t.size() != 2)
+                    throw std::runtime_error("Invalid state!");
+
+                /* Create a new C++ instance */
+                VectorVel vv(t[0].cast<Vector>(), t[1].cast<Vector>());
+                return vv;
+            }));
+
+    m.def("SetToZero", (void (*)(VectorVel&)) &KDL::SetToZero);
+    m.def("Equal", (bool (*)(const VectorVel&, const VectorVel&, double)) &KDL::Equal,
+          py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon);
+    m.def("Equal", (bool (*)(const Vector&, const VectorVel&, double)) &KDL::Equal,
+          py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon);
+    m.def("Equal", (bool (*)(const VectorVel&, const Vector&, double)) &KDL::Equal,
+          py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon);
+
+    m.def("dot", (doubleVel (*)(const VectorVel&, const VectorVel&)) &KDL::dot);
+    m.def("dot", (doubleVel (*)(const VectorVel&, const Vector&)) &KDL::dot);
+    m.def("dot", (doubleVel (*)(const Vector&, const VectorVel&)) &KDL::dot);
+
+
+    // --------------------
+    // TwistVel
+    // --------------------
+    py::class_<TwistVel> twist_vel(m, "TwistVel");
+    twist_vel.def_readwrite("vel", &TwistVel::vel);
+    twist_vel.def_readwrite("rot", &TwistVel::rot);
+    twist_vel.def(py::init<>());
+    twist_vel.def(py::init<const VectorVel&, const VectorVel&>());
+    twist_vel.def(py::init<const Twist&, const Twist&>());
+    twist_vel.def(py::init<const Twist&>());
+    twist_vel.def(py::init<const TwistVel&>());
+    twist_vel.def("value", &TwistVel::value);
+    twist_vel.def("deriv", &TwistVel::deriv);
+    twist_vel.def("__repr__", [](const TwistVel &tv)
+    {
+        std::ostringstream oss;
+        oss << tv;
+        return oss.str();
+    });
+    twist_vel.def("__copy__", [](const TwistVel& self)
+    {
+        return TwistVel(self);
+    });
+    twist_vel.def("__deepcopy__", [](const TwistVel& self, py::dict)
+    {
+        return TwistVel(self);
+    }, py::arg("memo"));
+    twist_vel.def_static("Zero", &TwistVel::Zero);
+    twist_vel.def("ReverseSign", &TwistVel::ReverseSign);
+    twist_vel.def("RefPoint", &TwistVel::RefPoint);
+    twist_vel.def("GetTwist", &TwistVel::GetTwist);
+    twist_vel.def("GetTwistDot", &TwistVel::GetTwistDot);
+
+    twist_vel.def(py::self -= py::self);
+    twist_vel.def(py::self += py::self);
+    twist_vel.def(py::self * double());
+    twist_vel.def(double() * py::self);
+    twist_vel.def(py::self / double());
+
+    twist_vel.def(py::self * doubleVel());
+    twist_vel.def(doubleVel() * py::self);
+    twist_vel.def(py::self / doubleVel());
+
+    twist_vel.def(py::self + py::self);
+    twist_vel.def(py::self - py::self);
+
+    twist_vel.def(py::self == py::self);
+    twist_vel.def(py::self != py::self);
+    twist_vel.def(Twist() == py::self);
+    twist_vel.def(Twist() != py::self);
+    twist_vel.def(py::self == Twist());
+    twist_vel.def(py::self != Twist());
+    twist_vel.def("__neg__", [](const TwistVel &a)
+    {
+        return operator-(a);
+    }, py::is_operator());
+    twist_vel.def(py::pickle(
+            [](const TwistVel &tv)
+            { // __getstate__
+                /* Return a tuple that fully encodes the state of the object */
+                return py::make_tuple(tv.vel, tv.rot);
+            },
+            [](py::tuple t)
+            { // __setstate__
+                if (t.size() != 2)
+                    throw std::runtime_error("Invalid state!");
+
+                /* Create a new C++ instance */
+                TwistVel tv(t[0].cast<VectorVel>(), t[1].cast<VectorVel>());
+                return tv;
+            }));
+
+    m.def("SetToZero", (void (*)(TwistVel&)) &KDL::SetToZero);
+    m.def("Equal", (bool (*)(const TwistVel&, const TwistVel&, double)) &KDL::Equal,
+          py::arg("a"), py::arg("b"), py::arg("eps")=epsilon);
+    m.def("Equal", (bool (*)(const Twist&, const TwistVel&, double)) &KDL::Equal,
+          py::arg("a"), py::arg("b"), py::arg("eps")=epsilon);
+    m.def("Equal", (bool (*)(const TwistVel&, const Twist&, double)) &KDL::Equal,
+          py::arg("a"), py::arg("b"), py::arg("eps")=epsilon);
+
+
+    // --------------------
+    // RotationVel
+    // --------------------
+    py::class_<RotationVel> rotation_vel(m, "RotationVel");
+    rotation_vel.def_readwrite("R", &RotationVel::R);
+    rotation_vel.def_readwrite("w", &RotationVel::w);
+    rotation_vel.def(py::init<>());
+    rotation_vel.def(py::init<const Rotation&>());
+    rotation_vel.def(py::init<const Rotation&, const Vector&>());
+    rotation_vel.def(py::init<const RotationVel&>());
+    rotation_vel.def("value", &RotationVel::value);
+    rotation_vel.def("deriv", &RotationVel::deriv);
+    rotation_vel.def("__repr__", [](const RotationVel &rv)
+    {
+        std::ostringstream oss;
+        oss << rv;
+        return oss.str();
+    });
+    rotation_vel.def("__copy__", [](const RotationVel& self)
+    {
+        return RotationVel(self);
+    });
+    rotation_vel.def("__deepcopy__", [](const RotationVel& self, py::dict)
+    {
+        return RotationVel(self);
+    }, py::arg("memo"));
+    rotation_vel.def("UnitX", &RotationVel::UnitX);
+    rotation_vel.def("UnitY", &RotationVel::UnitY);
+    rotation_vel.def("UnitZ", &RotationVel::UnitZ);
+    rotation_vel.def_static("Identity", &RotationVel::Identity);
+    rotation_vel.def("Inverse", (RotationVel (RotationVel::*)(void) const) &RotationVel::Inverse);
+    rotation_vel.def("Inverse", (VectorVel (RotationVel::*)(const VectorVel&) const) &RotationVel::Inverse);
+    rotation_vel.def("Inverse", (VectorVel (RotationVel::*)(const Vector&) const) &RotationVel::Inverse);
+    rotation_vel.def("DoRotX", &RotationVel::DoRotX);
+    rotation_vel.def("DoRotY", &RotationVel::DoRotY);
+    rotation_vel.def("DoRotZ", &RotationVel::DoRotZ);
+    rotation_vel.def_static("RotX", &RotationVel::RotX);
+    rotation_vel.def_static("RotY", &RotationVel::RotY);
+    rotation_vel.def_static("RotZ", &RotationVel::RotZ);
+    rotation_vel.def_static("Rot", &RotationVel::Rot);
+    rotation_vel.def_static("Rot2", &RotationVel::Rot2);
+
+    rotation_vel.def("Inverse", (TwistVel (RotationVel::*)(const TwistVel&) const) &RotationVel::Inverse);
+    rotation_vel.def("Inverse", (TwistVel (RotationVel::*)(const Twist&) const) &RotationVel::Inverse);
+
+    rotation_vel.def(py::self * VectorVel());
+    rotation_vel.def(py::self * Vector());
+    rotation_vel.def(py::self * TwistVel());
+    rotation_vel.def(py::self * Twist());
+    rotation_vel.def(py::self * py::self);
+    rotation_vel.def(Rotation() * py::self);
+    rotation_vel.def(py::self * Rotation());
+
+    rotation_vel.def(py::self == py::self);
+    rotation_vel.def(py::self != py::self);
+    rotation_vel.def(Rotation() == py::self);
+    rotation_vel.def(Rotation() != py::self);
+    rotation_vel.def(py::self == Rotation());
+    rotation_vel.def(py::self != Rotation());
+    rotation_vel.def(py::pickle(
+            [](const RotationVel &rv)
+            { // __getstate__
+                /* Return a tuple that fully encodes the state of the object */
+                return py::make_tuple(rv.R, rv.w);
+            },
+            [](py::tuple t)
+            { // __setstate__
+                if (t.size() != 2)
+                    throw std::runtime_error("Invalid state!");
+
+                /* Create a new C++ instance */
+                RotationVel rv(t[0].cast<Rotation>(), t[1].cast<Vector>());
+                return rv;
+            }));
+
+    m.def("Equal", (bool (*)(const RotationVel&, const RotationVel&, double)) &KDL::Equal,
+          py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon);
+    m.def("Equal", (bool (*)(const Rotation&, const RotationVel&, double)) &KDL::Equal,
+          py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon);
+    m.def("Equal", (bool (*)(const RotationVel&, const Rotation&, double)) &KDL::Equal,
+          py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon);
+
+
+    // --------------------
+    // FrameVel
+    // --------------------
+    py::class_<FrameVel> frame_vel(m, "FrameVel");
+    frame_vel.def_readwrite("M", &FrameVel::M);
+    frame_vel.def_readwrite("p", &FrameVel::p);
+    frame_vel.def(py::init<>());
+    frame_vel.def(py::init<const Frame&>());
+    frame_vel.def(py::init<const Frame&, const Twist&>());
+    frame_vel.def(py::init<const RotationVel&, const VectorVel&>());
+    frame_vel.def(py::init<const FrameVel&>());
+    frame_vel.def("value", &FrameVel::value);
+    frame_vel.def("deriv", &FrameVel::deriv);
+    frame_vel.def("__repr__", [](const FrameVel &fv)
+    {
+        std::ostringstream oss;
+        oss << fv;
+        return oss.str();
+    });
+    frame_vel.def("__copy__", [](const FrameVel& self)
+    {
+        return FrameVel(self);
+    });
+    frame_vel.def("__deepcopy__", [](const FrameVel& self, py::dict)
+    {
+        return FrameVel(self);
+    }, py::arg("memo"));
+    frame_vel.def_static("Identity", &FrameVel::Identity);
+    frame_vel.def("Inverse", (FrameVel (FrameVel::*)() const) &FrameVel::Inverse);
+    frame_vel.def("Inverse", (VectorVel (FrameVel::*)(const VectorVel&) const) &FrameVel::Inverse);
+    frame_vel.def("Inverse", (VectorVel (FrameVel::*)(const Vector&) const) &FrameVel::Inverse);
+    frame_vel.def(py::self * VectorVel());
+    frame_vel.def(py::self * Vector());
+    frame_vel.def("GetFrame", &FrameVel::GetFrame);
+    frame_vel.def("GetTwist", &FrameVel::GetTwist);
+    frame_vel.def("Inverse", (TwistVel (FrameVel::*)(const TwistVel&) const) &FrameVel::Inverse);
+    frame_vel.def("Inverse", (TwistVel (FrameVel::*)(const Twist&) const) &FrameVel::Inverse);
+    frame_vel.def(py::self * TwistVel());
+    frame_vel.def(py::self * Twist());
+    frame_vel.def(py::self * py::self);
+    frame_vel.def(Frame() * py::self);
+    frame_vel.def(py::self * Frame());
+
+    frame_vel.def(py::self == py::self);
+    frame_vel.def(py::self != py::self);
+    frame_vel.def(Frame() == py::self);
+    frame_vel.def(Frame() != py::self);
+    frame_vel.def(py::self == Frame());
+    frame_vel.def(py::self != Frame());
+    frame_vel.def(py::pickle(
+            [](const FrameVel &fv)
+            { // __getstate__
+                /* Return a tuple that fully encodes the state of the object */
+                return py::make_tuple(fv.M, fv.p);
+            },
+            [](py::tuple t)
+            { // __setstate__
+                if (t.size() != 2)
+                    throw std::runtime_error("Invalid state!");
+
+                /* Create a new C++ instance */
+                FrameVel rv(t[0].cast<RotationVel>(), t[1].cast<VectorVel>());
+                return rv;
+            }));
+
+    m.def("Equal", (bool (*)(const FrameVel&, const FrameVel&, double)) &KDL::Equal,
+          py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon);
+    m.def("Equal", (bool (*)(const Frame&, const FrameVel&, double)) &KDL::Equal,
+          py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon);
+    m.def("Equal", (bool (*)(const FrameVel&, const Frame&, double)) &KDL::Equal,
+          py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon);
+
+
+    // --------------------
+    // Global
+    // --------------------
+    m.def("diff", (VectorVel (*)(const VectorVel&, const VectorVel&, double)) &KDL::diff,
+          py::arg("a"), py::arg("b"), py::arg("dt")=1.0);
+    m.def("diff", (VectorVel (*)(const RotationVel&, const RotationVel&, double)) &KDL::diff,
+          py::arg("a"), py::arg("b"), py::arg("dt")=1.0);
+    m.def("diff", (TwistVel (*)(const FrameVel&, const FrameVel&, double)) &KDL::diff,
+          py::arg("a"), py::arg("b"), py::arg("dt")=1.0);
+
+    m.def("addDelta", (VectorVel (*)(const VectorVel&, const VectorVel&, double)) &KDL::addDelta,
+          py::arg("a"), py::arg("da"), py::arg("dt")=1.0);
+    m.def("addDelta", (RotationVel (*)(const RotationVel&, const VectorVel&, double)) &KDL::addDelta,
+          py::arg("a"), py::arg("da"), py::arg("dt")=1.0);
+    m.def("addDelta", (FrameVel (*)(const FrameVel&, const TwistVel&, double)) &KDL::addDelta,
+          py::arg("a"), py::arg("da"), py::arg("dt")=1.0);
+}
diff --git a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp
new file mode 100644
index 000000000..8eaee9c5a
--- /dev/null
+++ b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp
@@ -0,0 +1,486 @@
+//Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
+//
+//Version: 1.0
+//Author: Ruben Smits Ruben Smits <ruben dot smits at intermodalics dot eu>
+//Author: Zihan Chen <zihan dot chen dot jhu at gmail dot com>
+//Author: Matthijs van der Burgh <MatthijsBurgh at outlook dot com>
+//Maintainer: Ruben Smits Ruben Smits <ruben dot smits at intermodalics dot eu>
+//URL: http://www.orocos.org/kdl
+//
+//This library is free software; you can redistribute it and/or
+//modify it under the terms of the GNU Lesser General Public
+//License as published by the Free Software Foundation; either
+//version 2.1 of the License, or (at your option) any later version.
+//
+//This library is distributed in the hope that it will be useful,
+//but WITHOUT ANY WARRANTY; without even the implied warranty of
+//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+//Lesser General Public License for more details.
+//
+//You should have received a copy of the GNU Lesser General Public
+//License along with this library; if not, write to the Free Software
+//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+
+#include <kdl/joint.hpp>
+#include <kdl/rotationalinertia.hpp>
+#include <kdl/rigidbodyinertia.hpp>
+#include <kdl/chain.hpp>
+#include <kdl/tree.hpp>
+#include <kdl/jntarrayvel.hpp>
+#include <kdl/chainiksolver.hpp>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainfksolvervel_recursive.hpp>
+#include <kdl/chainiksolverpos_nr.hpp>
+#include <kdl/chainiksolverpos_nr_jl.hpp>
+#include <kdl/chainiksolvervel_pinv.hpp>
+#include <kdl/chainiksolvervel_wdls.hpp>
+#include <kdl/chainiksolverpos_lma.hpp>
+#include <kdl/chainiksolvervel_pinv_nso.hpp>
+#include <kdl/chainiksolvervel_pinv_givens.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
+#include <kdl/kinfam_io.hpp>
+#include "PyKDL.h"
+
+namespace py = pybind11;
+using namespace KDL;
+
+
+void init_kinfam(pybind11::module &m)
+{
+    // --------------------
+    // Joint
+    // --------------------
+    py::class_<Joint> joint(m, "Joint");
+    py::enum_<Joint::JointType> joint_type(joint, "JointType");
+        joint_type.value("RotAxis", Joint::JointType::RotAxis);
+        joint_type.value("RotX", Joint::JointType::RotX);
+        joint_type.value("RotY", Joint::JointType::RotY);
+        joint_type.value("RotZ", Joint::JointType::RotZ);
+        joint_type.value("TransAxis", Joint::JointType::TransAxis);
+        joint_type.value("TransX", Joint::JointType::TransX);
+        joint_type.value("TransY", Joint::JointType::TransY);
+        joint_type.value("TransZ", Joint::JointType::TransZ);
+        joint_type.value("Fixed", Joint::JointType::Fixed);
+#if PY_VERSION_HEX < 0x03000000
+        joint_type.value("None", Joint::JointType::None);
+#endif
+        joint_type.export_values();
+
+    joint.def(py::init<>());
+    joint.def(py::init<std::string, Joint::JointType, double, double, double, double, double>(),
+              py::arg("name"), py::arg("type")=Joint::JointType::Fixed, py::arg("scale")=1, py::arg("offset")=0,
+              py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0);
+    joint.def(py::init<Joint::JointType, double, double, double, double, double>(),
+              py::arg("type")=Joint::JointType::Fixed, py::arg("scale")=1, py::arg("offset")=0,
+              py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0);
+    joint.def(py::init<std::string, Vector, Vector, Joint::JointType, double, double, double, double, double>(),
+              py::arg("name"), py::arg("origin"), py::arg("axis"), py::arg("type"), py::arg("scale")=1, py::arg("offset")=0,
+              py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0);
+    joint.def(py::init<Vector, Vector, Joint::JointType, double, double, double, double, double>(),
+              py::arg("origin"), py::arg("axis"), py::arg("type"), py::arg("scale")=1, py::arg("offset")=0,
+              py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0);
+    joint.def(py::init<const Joint&>());
+    joint.def("pose", &Joint::pose);
+    joint.def("twist", &Joint::twist);
+    joint.def("JointAxis", &Joint::JointAxis);
+    joint.def("JointOrigin", &Joint::JointOrigin);
+    joint.def("getName", &Joint::getName);
+    joint.def("getType", &Joint::getType);
+    joint.def("getTypeName", &Joint::getTypeName);
+    joint.def("__repr__", [](const Joint &j)
+    {
+        std::ostringstream oss;
+        oss << j;
+        return oss.str();
+    });
+
+
+    // --------------------
+    // RotationalInertia
+    // --------------------
+    py::class_<RotationalInertia> rotational_inertia(m, "RotationalInertia");
+    rotational_inertia.def(py::init<double, double, double, double, double, double>(),
+                           py::arg("Ixx")=0, py::arg("Iyy")=0, py::arg("Izz")=0,
+                           py::arg("Ixy")=0, py::arg("Ixz")=0, py::arg("Iyz")=0);
+    rotational_inertia.def_static("Zero", &RotationalInertia::Zero);
+    rotational_inertia.def("__getitem__", [](const RotationalInertia &inertia, int i)
+    {
+        if (i < 0 || i > 8)
+            throw py::index_error("RotationalInertia index out of range");
+
+        return inertia.data[i];
+    });
+    rotational_inertia.def("__setitem__", [](RotationalInertia &inertia, int i, double value)
+    {
+        if (i < 0 || i > 8)
+            throw py::index_error("RotationalInertia index out of range");
+
+        inertia.data[i] = value;
+    });
+    rotational_inertia.def(double() * py::self);
+    rotational_inertia.def(py::self + py::self);
+    rotational_inertia.def(py::self * Vector());
+
+
+    // --------------------
+    // RigidBodyInertia
+    // --------------------
+    py::class_<RigidBodyInertia> rigid_body_inertia(m, "RigidBodyInertia");
+    rigid_body_inertia.def(py::init<double, const Vector&, const RotationalInertia&>(),
+                           py::arg("m")=0, py::arg_v("oc", Vector::Zero(), "Vector.Zero"),
+                           py::arg_v("Ic", RotationalInertia::Zero(), "RigidBodyInertia.Zero"));
+    rigid_body_inertia.def_static("Zero", &RigidBodyInertia::Zero);
+    rigid_body_inertia.def("RefPoint", &RigidBodyInertia::RefPoint);
+    rigid_body_inertia.def("getMass", &RigidBodyInertia::getMass);
+    rigid_body_inertia.def("getCOG", &RigidBodyInertia::getCOG);
+    rigid_body_inertia.def("getRotationalInertia", &RigidBodyInertia::getRotationalInertia);
+    rigid_body_inertia.def(double() * py::self);
+    rigid_body_inertia.def(py::self + py::self);
+    rigid_body_inertia.def(py::self * Twist());
+    rigid_body_inertia.def(Frame() * py::self);
+    rigid_body_inertia.def(Rotation() * py::self);
+
+
+    // --------------------
+    // Segment
+    // --------------------
+    py::class_<Segment> segment(m, "Segment");
+    segment.def(py::init<const std::string&, const Joint&, const Frame&, const RigidBodyInertia&>(),
+                py::arg("name"), py::arg_v("joint", Joint(), "Joint"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity"),
+                py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero"));
+    segment.def(py::init<const Joint&, const Frame&, const RigidBodyInertia&>(),
+                py::arg_v("joint", Joint(), "Joint"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity"),
+                py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero"));
+    segment.def(py::init<const Segment&>());
+    segment.def("getFrameToTip", &Segment::getFrameToTip);
+    segment.def("pose", &Segment::pose);
+    segment.def("twist", &Segment::twist);
+    segment.def("getName", &Segment::getName);
+    segment.def("getJoint", &Segment::getJoint);
+    segment.def("getInertia", &Segment::getInertia);
+    segment.def("setInertia", &Segment::setInertia);
+
+
+    // --------------------
+    // Chain
+    // --------------------
+    py::class_<Chain> chain(m, "Chain");
+    chain.def(py::init<>());
+    chain.def(py::init<const Chain&>());
+    chain.def("addSegment", &Chain::addSegment);
+    chain.def("addChain", &Chain::addChain);
+    chain.def("getNrOfJoints", &Chain::getNrOfJoints);
+    chain.def("getNrOfSegments", &Chain::getNrOfSegments);
+    chain.def("getSegment", (Segment& (Chain::*)(unsigned int)) &Chain::getSegment);
+    chain.def("getSegment", (const Segment& (Chain::*)(unsigned int) const) &Chain::getSegment);
+    chain.def("__repr__", [](const Chain &c)
+    {
+        std::ostringstream oss;
+        oss << c;
+        return oss.str();
+    });
+
+
+    // --------------------
+    // Tree
+    // --------------------
+    py::class_<Tree> tree(m, "Tree");
+    tree.def(py::init<const std::string&>(), py::arg("root_name")="root");
+    tree.def("addSegment", &Tree::addSegment);
+    tree.def("addChain", &Tree::addChain);
+    tree.def("addTree", &Tree::addTree);
+    tree.def("getNrOfJoints", &Tree::getNrOfJoints);
+    tree.def("getNrOfSegments", &Tree::getNrOfSegments);
+    tree.def("getChain", [](const Tree &tree, const std::string& chain_root, const std::string& chain_tip)
+    {
+        Chain* chain = new Chain();
+        tree.getChain(chain_root, chain_tip, *chain);
+        return chain;
+    });
+    tree.def("__repr__", [](const Tree &t)
+    {
+        std::ostringstream oss;
+        oss << t;
+        return oss.str();
+    });
+
+
+    // --------------------
+    // Jacobian
+    // --------------------
+    py::class_<Jacobian> jacobian(m, "Jacobian");
+    jacobian.def(py::init<>());
+    jacobian.def(py::init<unsigned int>());
+    jacobian.def(py::init<const Jacobian&>());
+    jacobian.def("rows", &Jacobian::rows);
+    jacobian.def("columns", &Jacobian::columns);
+    jacobian.def("resize", &Jacobian::resize);
+    jacobian.def("getColumn", &Jacobian::getColumn);
+    jacobian.def("setColumn", &Jacobian::setColumn);
+    jacobian.def("changeRefPoint", &Jacobian::changeRefPoint);
+    jacobian.def("changeBase", &Jacobian::changeBase);
+    jacobian.def("changeRefFrame", &Jacobian::changeRefFrame);
+    jacobian.def("__getitem__", [](const Jacobian &jac, std::tuple<int, int> idx)
+    {
+        int i = std::get<0>(idx);
+        int j = std::get<1>(idx);
+        if (i < 0 || i > 5 || j < 0 || j >= jac.columns())
+            throw py::index_error("Jacobian index out of range");
+        return jac((unsigned int)i, (unsigned int)j);
+    });
+    jacobian.def("__setitem__", [](Jacobian &jac, std::tuple<int, int> idx, double value)
+    {
+        int i = std::get<0>(idx);
+        int j = std::get<1>(idx);
+        if (i < 0 || i > 5 || j < 0 || j >= jac.columns())
+            throw py::index_error("Jacobian index out of range");
+
+        jac((unsigned int)i, (unsigned int)j) = value;
+    });
+    jacobian.def("__repr__", [](const Jacobian &jac)
+    {
+        std::ostringstream oss;
+        oss << jac;
+        return oss.str();
+    });
+
+    m.def("SetToZero", (void (*)(Jacobian&)) &KDL::SetToZero);
+    m.def("changeRefPoint", (void (*)(const Jacobian&, const Vector&, Jacobian&)) &KDL::changeRefPoint);
+    m.def("changeBase", (void (*)(const Jacobian&, const Rotation&, Jacobian&)) &KDL::changeBase);
+    m.def("SetToZero", (void (*)(const Jacobian&, const Frame&, Jacobian&)) &KDL::changeRefFrame);
+
+
+    // --------------------
+    // JntArray
+    // --------------------
+    py::class_<JntArray> jnt_array(m, "JntArray");
+    jnt_array.def(py::init<>());
+    jnt_array.def(py::init<unsigned int>());
+    jnt_array.def(py::init<const JntArray&>());
+    jnt_array.def("rows", &JntArray::rows);
+    jnt_array.def("columns", &JntArray::columns);
+    jnt_array.def("resize", &JntArray::resize);
+    jnt_array.def("__getitem__", [](const JntArray &ja, int i)
+    {
+        if (i < 0 || i >= ja.rows())
+            throw py::index_error("JntArray index out of range");
+
+        return ja(i);
+    });
+    jnt_array.def("__setitem__", [](JntArray &ja, int i, double value)
+    {
+        if (i < 0 || i >= ja.rows())
+            throw py::index_error("JntArray index out of range");
+
+        ja(i) = value;
+    });
+    jnt_array.def("__repr__", [](const JntArray &ja)
+    {
+        std::ostringstream oss;
+        oss << ja;
+        return oss.str();
+    });
+    jnt_array.def(py::self == py::self);
+
+    m.def("Add", (void (*)(const JntArray&, const JntArray&, JntArray&)) &KDL::Add);
+    m.def("Subtract", (void (*)(const JntArray&, const JntArray&, JntArray&)) &KDL::Subtract);
+    m.def("Multiply", (void (*)(const JntArray&, const double&, JntArray&)) &KDL::Multiply);
+    m.def("Divide", (void (*)(const JntArray&, const double&, JntArray&)) &KDL::Divide);
+    m.def("MultiplyJacobian", (void (*)(const Jacobian&, const JntArray&, Twist&)) &KDL::MultiplyJacobian);
+    m.def("SetToZero", (void (*)(JntArray&)) &KDL::SetToZero);
+    m.def("Equal", (bool (*)(const JntArray&, const JntArray&, double)) &KDL::Equal,
+          py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon);
+
+
+    // --------------------
+    // JntArrayVel
+    // --------------------
+    py::class_<JntArrayVel> jnt_array_vel(m, "JntArrayVel");
+    jnt_array_vel.def_readwrite("q", &JntArrayVel::q);
+    jnt_array_vel.def_readwrite("qdot", &JntArrayVel::qdot);
+    jnt_array_vel.def(py::init<unsigned int>());
+    jnt_array_vel.def(py::init<const JntArray&, const JntArray&>());
+    jnt_array_vel.def(py::init<const JntArray&>());
+    jnt_array_vel.def("resize", &JntArrayVel::resize);
+    jnt_array_vel.def("value", &JntArrayVel::value);
+    jnt_array_vel.def("deriv", &JntArrayVel::deriv);
+
+    m.def("Add", (void (*)(const JntArrayVel&, const JntArrayVel&, JntArrayVel&)) &KDL::Add);
+    m.def("Add", (void (*)(const JntArrayVel&, const JntArray&, JntArrayVel&)) &KDL::Add);
+    m.def("Subtract", (void (*)(const JntArrayVel&, const JntArrayVel&, JntArrayVel&)) &KDL::Subtract);
+    m.def("Subtract", (void (*)(const JntArrayVel&, const JntArray&, JntArrayVel&)) &KDL::Subtract);
+    m.def("Multiply", (void (*)(const JntArrayVel&, const double&, JntArrayVel&)) &KDL::Multiply);
+    m.def("Multiply", (void (*)(const JntArrayVel&, const doubleVel&, JntArrayVel&)) &KDL::Multiply);
+    m.def("Divide", (void (*)(const JntArrayVel&, const double&, JntArrayVel&)) &KDL::Divide);
+    m.def("Divide", (void (*)(const JntArrayVel&, const doubleVel&, JntArrayVel&)) &KDL::Divide);
+    m.def("SetToZero", (void (*)(JntArrayVel&)) &KDL::SetToZero);
+    m.def("Equal", (bool (*)(const JntArrayVel&, const JntArrayVel&, double)) &KDL::Equal,
+          py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon);
+
+
+    // --------------------
+    // SolverI
+    // --------------------
+    py::class_<SolverI> solver_i(m, "SolverI");
+    solver_i.def("getError", &SolverI::getError);
+    solver_i.def("strError", &SolverI::strError);
+    solver_i.def("updateInternalDataStructures", &SolverI::updateInternalDataStructures);
+
+
+    // --------------------
+    // ChainFkSolverPos
+    // --------------------
+    py::class_<ChainFkSolverPos, SolverI> chain_fk_solver_pos(m, "ChainFkSolverPos");
+    chain_fk_solver_pos.def("JntToCart", (int (ChainFkSolverPos::*)(const JntArray&, Frame&, int)) &ChainFkSolverPos::JntToCart,
+                            py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1);
+    chain_fk_solver_pos.def("JntToCart", (int (ChainFkSolverPos::*)(const JntArray&, std::vector<Frame>&, int)) &ChainFkSolverPos::JntToCart,
+                            py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1);
+
+
+    // --------------------
+    // ChainFkSolverVel
+    // --------------------
+    py::class_<ChainFkSolverVel, SolverI> chain_fk_solver_vel(m, "ChainFkSolverVel");
+    chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, FrameVel&, int)) &ChainFkSolverVel::JntToCart,
+                            py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1);
+//    Argument by reference doesn't work for container types
+//    chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, std::vector<FrameVel>&, int)) &ChainFkSolverVel::JntToCart,
+//                            py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1);
+
+
+    // ------------------------------
+    // ChainFkSolverPos_recursive
+    // ------------------------------
+    py::class_<ChainFkSolverPos_recursive, ChainFkSolverPos> chain_fk_solver_pos_recursive(m, "ChainFkSolverPos_recursive");
+    chain_fk_solver_pos_recursive.def(py::init<const Chain&>());
+
+
+    // ------------------------------
+    // ChainFkSolverVel_recursive
+    // ------------------------------
+    py::class_<ChainFkSolverVel_recursive, ChainFkSolverVel> chain_fk_solver_vel_recursive(m, "ChainFkSolverVel_recursive");
+    chain_fk_solver_vel_recursive.def(py::init<const Chain&>());
+
+
+    // --------------------
+    // ChainIkSolverPos
+    // --------------------
+    py::class_<ChainIkSolverPos, SolverI> chain_ik_solver_pos(m, "ChainIkSolverPos");
+    chain_ik_solver_pos.def("CartToJnt", (int (ChainIkSolverPos::*)(const JntArray&, const Frame&, JntArray&)) &ChainIkSolverPos::CartToJnt,
+                            py::arg("q_init"), py::arg("p_in"), py::arg("q_out"));
+
+
+    // --------------------
+    // ChainIkSolverVel
+    // --------------------
+    py::class_<ChainIkSolverVel, SolverI> chain_ik_solver_vel(m, "ChainIkSolverVel");
+    chain_ik_solver_vel.def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const Twist&, JntArray&)) &ChainIkSolverVel::CartToJnt,
+                            py::arg("q_in"), py::arg("v_in"), py::arg("qdot_out"));
+//    Argument by reference doesn't work for container types
+//    chain_ik_solver_vel.def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const FrameVel&, JntArrayVel&)) &ChainIkSolverVel::CartToJnt,
+//                            py::arg("q_init"), py::arg("v_in"), py::arg("q_out"));
+
+
+    // ----------------------
+    // ChainIkSolverPos_NR
+    // ----------------------
+    py::class_<ChainIkSolverPos_NR, ChainIkSolverPos> chain_ik_solver_pos_NR(m, "ChainIkSolverPos_NR");
+    chain_ik_solver_pos_NR.def(py::init<const Chain&, ChainFkSolverPos&, ChainIkSolverVel&, unsigned int, double>(),
+                               py::arg("chain"), py::arg("fksolver"), py::arg("iksolver"),
+                               py::arg("maxiter")=100, py::arg("eps")=epsilon);
+
+
+    // ---------------------------
+    // ChainIkSolverPos_NR_JL
+    // ---------------------------
+    py::class_<ChainIkSolverPos_NR_JL, ChainIkSolverPos> chain_ik_solver_pos_NR_JL(m, "ChainIkSolverPos_NR_JL");
+    chain_ik_solver_pos_NR_JL.def(py::init<const Chain&, const JntArray&, const JntArray&, ChainFkSolverPos&,
+                                  ChainIkSolverVel&, unsigned int, double>(),
+                                  py::arg("chain"), py::arg("q_min"), py::arg("q_max"), py::arg("fksolver"),
+                                  py::arg("iksolver"), py::arg("maxiter")=100, py::arg("eps")=epsilon);
+
+
+    // -------------------------
+    // ChainIkSolverVel_pinv
+    // -------------------------
+    py::class_<ChainIkSolverVel_pinv, ChainIkSolverVel> chain_ik_solver_vel_pinv(m, "ChainIkSolverVel_pinv");
+    chain_ik_solver_vel_pinv.def(py::init<const Chain&, double, int>(),
+                                 py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150);
+
+
+    // -------------------------
+    // ChainIkSolverVel_wdls
+    // -------------------------
+    py::class_<ChainIkSolverVel_wdls, ChainIkSolverVel> chain_ik_solver_vel_wdls(m, "ChainIkSolverVel_wdls");
+    chain_ik_solver_vel_wdls.def(py::init<const Chain&, double, int>(),
+                                 py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150);
+    chain_ik_solver_vel_wdls.def("setWeightTS", &ChainIkSolverVel_wdls::setWeightTS);
+    chain_ik_solver_vel_wdls.def("setWeightJS", &ChainIkSolverVel_wdls::setWeightJS);
+    chain_ik_solver_vel_wdls.def("setLambda", &ChainIkSolverVel_wdls::setLambda);
+    chain_ik_solver_vel_wdls.def("setEps", &ChainIkSolverVel_wdls::setEps);
+    chain_ik_solver_vel_wdls.def("setMaxIter", &ChainIkSolverVel_wdls::setMaxIter);
+    chain_ik_solver_vel_wdls.def("getNrZeroSigmas", &ChainIkSolverVel_wdls::getNrZeroSigmas);
+    chain_ik_solver_vel_wdls.def("getSigmaMin", &ChainIkSolverVel_wdls::getSigmaMin);
+    chain_ik_solver_vel_wdls.def("getSigma", &ChainIkSolverVel_wdls::getSigma);
+    chain_ik_solver_vel_wdls.def("getEps", &ChainIkSolverVel_wdls::getEps);
+    chain_ik_solver_vel_wdls.def("getLambda", &ChainIkSolverVel_wdls::getLambda);
+    chain_ik_solver_vel_wdls.def("getLambdaScaled", &ChainIkSolverVel_wdls::getLambdaScaled);
+    chain_ik_solver_vel_wdls.def("getSVDResult", &ChainIkSolverVel_wdls::getSVDResult);
+
+
+    // -------------------------
+    // ChainIkSolverPos_LMA
+    // -------------------------
+    py::class_<ChainIkSolverPos_LMA, ChainIkSolverPos> chain_ik_solver_pos_LMA(m, "ChainIkSolverPos_LMA");
+    chain_ik_solver_pos_LMA.def(py::init<const Chain&, double, int, double>(),
+                                py::arg("chain"), py::arg("eps")=1e-5, py::arg("maxiter")=500,
+                                py::arg("eps_joints")=1e-15);
+
+
+    // ----------------------------
+    // ChainIkSolverVel_pinv_nso
+    // ----------------------------
+    py::class_<ChainIkSolverVel_pinv_nso, ChainIkSolverVel> chain_ik_solver_vel_pinv_nso(m, "ChainIkSolverVel_pinv_nso");
+    chain_ik_solver_vel_pinv_nso.def(py::init<const Chain&, double, int, double>(),
+                                     py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150, py::arg("alpha")=0.25);
+    chain_ik_solver_vel_pinv_nso.def("setWeights", &ChainIkSolverVel_pinv_nso::setWeights);
+    chain_ik_solver_vel_pinv_nso.def("setOptPos", &ChainIkSolverVel_pinv_nso::setOptPos);
+    chain_ik_solver_vel_pinv_nso.def("setAlpha", &ChainIkSolverVel_pinv_nso::setAlpha);
+    chain_ik_solver_vel_pinv_nso.def("getWeights", &ChainIkSolverVel_pinv_nso::getWeights);
+    chain_ik_solver_vel_pinv_nso.def("getOptPos", &ChainIkSolverVel_pinv_nso::getOptPos);
+    chain_ik_solver_vel_pinv_nso.def("getAlpha", &ChainIkSolverVel_pinv_nso::getAlpha);
+
+
+    // -------------------------------
+    // ChainIkSolverVel_pinv_givens
+    // -------------------------------
+    py::class_<ChainIkSolverVel_pinv_givens, ChainIkSolverVel> chain_ik_solver_vel_pinv_givens(m, "ChainIkSolverVel_pinv_givens");
+    chain_ik_solver_vel_pinv_givens.def(py::init<const Chain&>());
+
+
+    // ------------------------------
+    // ChainJntToJacSolver
+    // ------------------------------
+    py::class_<ChainJntToJacSolver, SolverI> chain_jnt_to_jac_solver(m, "ChainJntToJacSolver");
+    chain_jnt_to_jac_solver.def(py::init<const Chain&>());
+    chain_jnt_to_jac_solver.def("JntToJac", &ChainJntToJacSolver::JntToJac,
+                                py::arg("q_in"), py::arg("jac"), py::arg("segmentNR")=-1);
+    chain_jnt_to_jac_solver.def("setLockedJoints", &ChainJntToJacSolver::setLockedJoints);
+
+
+    // ------------------------------
+    // ChainIdSolver
+    // ------------------------------
+    py::class_<ChainIdSolver, SolverI> chain_id_solver(m, "ChainIdSolver");
+    chain_id_solver.def("CartToJnt", &ChainIdSolver::CartToJnt);
+
+
+    // ------------------------------
+    // ChainIdSolver_RNE
+    // ------------------------------
+    py::class_<ChainIdSolver_RNE, ChainIdSolver> chain_id_solver_RNE(m, "ChainIdSolver_RNE");
+    chain_id_solver_RNE.def(py::init<const Chain&, Vector>());
+}
diff --git a/python_orocos_kdl/PyKDL/PyKDL.sip b/python_orocos_kdl/PyKDL/sip/PyKDL.sip
similarity index 63%
rename from python_orocos_kdl/PyKDL/PyKDL.sip
rename to python_orocos_kdl/PyKDL/sip/PyKDL.sip
index 1de04528c..d5d60756c 100644
--- a/python_orocos_kdl/PyKDL/PyKDL.sip
+++ b/python_orocos_kdl/PyKDL/sip/PyKDL.sip
@@ -1,8 +1,8 @@
-//Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+//Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 //
 //Version: 1.0
-//Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-//Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+//Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+//Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 //URL: http://www.orocos.org/kdl
 //
 //This library is free software; you can redistribute it and/or
@@ -20,13 +20,21 @@
 //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
 
-%Module(name = PyKDL,version=0,keyword_arguments="All")
+%Module(name = PyKDL, keyword_arguments="All")
 
-%License(type="LGPL",licensee="Ruben Smits",signature="ruben@intermodalics.eu",timestamp="2014")
+%License(type="LGPL", licensee="Ruben Smits", signature="ruben@intermodalics.eu", timestamp="2020")
 
 %Include std_string.sip
+%Include std_vector.sip
+
 %Include frames.sip
-%Include kinfam.sip
 %Include framevel.sip
-
+%Include kinfam.sip
 %Include dynamics.sip
+
+const std::string __version__;
+
+%ModuleCode
+#include <kdl/config.h>
+const std::string __version__ = KDL_VERSION_STRING;
+%End
diff --git a/python_orocos_kdl/PyKDL/dynamics.sip b/python_orocos_kdl/PyKDL/sip/dynamics.sip
similarity index 70%
rename from python_orocos_kdl/PyKDL/dynamics.sip
rename to python_orocos_kdl/PyKDL/sip/dynamics.sip
index e0096ddc8..2f2e4c2e1 100644
--- a/python_orocos_kdl/PyKDL/dynamics.sip
+++ b/python_orocos_kdl/PyKDL/sip/dynamics.sip
@@ -1,8 +1,8 @@
-//Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+//Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 //
 //Version: 1.0
-//Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-//Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+//Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+//Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 //URL: http://www.orocos.org/kdl
 //
 //This library is free software; you can redistribute it and/or
@@ -19,11 +19,11 @@
 //License along with this library; if not, write to the Free Software
 //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
-%Include std_string.sip
 
 class JntSpaceInertiaMatrix {
 %TypeHeaderCode
 #include <kdl/jntspaceinertiamatrix.hpp>
+#include <kdl/kinfam_io.hpp>
 using namespace KDL;
 %End
 public:
@@ -33,17 +33,38 @@ public:
     void resize(unsigned int newSize);
     unsigned int rows()const;
     unsigned int columns()const;
-    //JntSpaceInertiaMatrix& operator = ( const JntSpaceInertiaMatrix& arg);
     double __getitem__(SIP_PYTUPLE);
 %MethodCode
     int i,j;
     PyArg_ParseTuple(a0,"ii",&i,&j);
-    if (i < 0 || j < 0 || i > (int)sipCpp->rows() || j >= (int)sipCpp->columns()) {
+    if (i < 0 || j < 0 || i >= (int)sipCpp->rows() || j >= (int)sipCpp->columns()) {
+        PyErr_SetString(PyExc_IndexError, "Inertia index out of range");
+        sipError = sipErrorFail;
+    }
+    else {
+        sipRes=(*sipCpp)(i,j);
+    }
+%End
+    void __setitem__(SIP_PYTUPLE,double value);
+%MethodCode
+    int i,j;
+    PyArg_ParseTuple(a0,"ii",&i,&j);
+    if (i < 0 || j < 0 || i >= (int)sipCpp->rows() || j >= (int)sipCpp->columns()) {
         PyErr_SetString(PyExc_IndexError, "Inertia index out of range");
-        return NULL;
+        sipError = sipErrorFail;
+    }
+    else {
+        (*sipCpp)(i,j)=a1;
     }
-    sipRes=(*sipCpp)(i,j);
 %End
+    const std::string* __repr__() const;
+%MethodCode
+    std::ostringstream oss;
+    oss<<(*sipCpp);
+    std::string s(oss.str());
+    sipRes=&s;
+%End
+    //JntSpaceInertiaMatrix& operator = ( const JntSpaceInertiaMatrix& arg);
     //double operator()(unsigned int i,unsigned int j)const;
     //double& operator()(unsigned int i,unsigned int j);
     //bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2);
@@ -60,7 +81,7 @@ bool Equal(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,d
 bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2);
 
 class ChainDynParam {
-    
+
 %TypeHeaderCode
 #include <kdl/chaindynparam.hpp>
 using namespace KDL;
@@ -70,6 +91,9 @@ public:
     ChainDynParam(const Chain& chain, Vector _grav);
 
     int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis);
-	int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H);
-	int JntToGravity(const JntArray &q,JntArray &gravity);
+    int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H);
+    int JntToGravity(const JntArray &q,JntArray &gravity);
+
+private:
+    ChainDynParam& operator=(const ChainDynParam&);
 };
diff --git a/python_orocos_kdl/PyKDL/frames.sip b/python_orocos_kdl/PyKDL/sip/frames.sip
similarity index 80%
rename from python_orocos_kdl/PyKDL/frames.sip
rename to python_orocos_kdl/PyKDL/sip/frames.sip
index 07a9d24c4..96feacf75 100644
--- a/python_orocos_kdl/PyKDL/frames.sip
+++ b/python_orocos_kdl/PyKDL/sip/frames.sip
@@ -1,8 +1,8 @@
-//Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+//Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 //
 //Version: 1.0
-//Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-//Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+//Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+//Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 //URL: http://www.orocos.org/kdl
 //
 //This library is free software; you can redistribute it and/or
@@ -45,19 +45,23 @@ public:
     double __getitem__ (int index) const;
 %MethodCode
     if (a0 < 0 || a0 > 2) {
-	    PyErr_SetString(PyExc_IndexError, "Vector index out of range");
-	    return 0;
+        PyErr_SetString(PyExc_IndexError, "Vector index out of range");
+        sipError = sipErrorFail;
+    }
+    else {
+        sipRes=(*sipCpp)(a0);
     }
-    sipRes=(*sipCpp)(a0);
 %End
 
     void __setitem__(int index, double value);
 %MethodCode
     if (a0 < 0 || a0 > 2) {
         PyErr_SetString(PyExc_IndexError, "Vector index out of range");
-        return 0;
+        sipError = sipErrorFail;
+    }
+    else {
+        (*sipCpp)(a0)=a1;
     }
-    (*sipCpp)(a0)=a1;
 %End
 
     const std::string* __repr__() const;
@@ -68,6 +72,16 @@ public:
     sipRes=&s;
 %End
 
+    SIP_PYOBJECT __copy__() const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new Vector(*sipCpp), sipFindType("Vector"), NULL);
+%End
+
+    SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new Vector(*sipCpp), sipFindType("Vector"), NULL);
+%End
+
     void ReverseSign();
 
     Vector& operator-=(const Vector& arg);
@@ -75,7 +89,7 @@ public:
 
     static Vector Zero()/Factory/;
 
-    double Norm();
+    double Norm(double eps=epsilon) const;
     double Normalize(double eps=epsilon);
 
 %PickleCode
@@ -111,16 +125,18 @@ public:
              double Xy,double Yy,double Zy,
              double Xz,double Yz,double Zz);
     Rotation(const Vector& x,const Vector& y,const Vector& z);
-    
+
     double __getitem__(SIP_PYTUPLE) const;
 %MethodCode
     int i,j;
     PyArg_ParseTuple(a0, "ii", &i, &j);
     if (i < 0 || j < 0 || i > 2 || j > 2) {
         PyErr_SetString(PyExc_IndexError, "Rotation index out of range");
-        return 0;
+        sipError = sipErrorFail;
+    }
+    else {
+        sipRes=((const Rotation)(*sipCpp))(i,j);
     }
-    sipRes=((const Rotation)(*sipCpp))(i,j);
 %End
 
     void __setitem__(SIP_PYTUPLE,double value);
@@ -129,9 +145,11 @@ public:
     PyArg_ParseTuple(a0,"ii",&i,&j);
     if (i < 0 || j < 0 || i > 2 || j > 2) {
         PyErr_SetString(PyExc_IndexError, "Rotation index out of range");
-        return 0;
+        sipError = sipErrorFail;
+    }
+    else {
+        (*sipCpp)(i,j)=a1;
     }
-    (*sipCpp)(i,j)=a1;
 %End
 
     const std::string* __repr__() const;
@@ -141,13 +159,23 @@ public:
     std::string s(oss.str());
     sipRes=&s;
 %End
-    
+
+    SIP_PYOBJECT __copy__() const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new Rotation(*sipCpp), sipFindType("Rotation"), NULL);
+%End
+
+    SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new Rotation(*sipCpp), sipFindType("Rotation"), NULL);
+%End
+
     void SetInverse();
     Rotation Inverse() const /Factory/;
     Vector Inverse(const Vector& v) const /Factory/;
     Wrench Inverse(const Wrench& w) const /Factory/;
     Twist Inverse(const Twist& t) const /Factory/;
-    
+
     static Rotation Identity()/Factory/;
     static Rotation RotX(double angle)/Factory/;
     static Rotation RotY(double angle)/Factory/;
@@ -158,12 +186,12 @@ public:
     static Rotation RPY(double roll,double pitch,double yaw)/Factory/;
     static Rotation EulerZYX(double Alfa,double Beta,double Gamma)/Factory/;
     static Rotation Quaternion(double x, double y, double z, double w)/Factory/;
-    
+
 
     void DoRotX(double angle);
     void DoRotY(double angle);
     void DoRotZ(double angle);
-    
+
     Vector GetRot() const /Factory/;
     double GetRotAngle(Vector& axis /Out/,double eps=epsilon) const;
     void GetEulerZYZ(double& alfa /Out/,double& beta /Out/,double& gamma /Out/) const;
@@ -171,11 +199,11 @@ public:
     void GetEulerZYX(double& Alfa /Out/,double& Beta /Out/,double& Gamma /Out/) const;
     void GetQuaternion(double& x /Out/,double& y /Out/,double& z /Out/, double& w) const;
 
-    
+
     Vector operator*(const Vector& v) const /Numeric,Factory/;
     Twist operator*(const Twist& arg) const /Numeric,Factory/;
     Wrench operator*(const Wrench& arg) const /Numeric,Factory/;
-    
+
     Vector UnitX() const /Factory/;
     Vector UnitY() const /Factory/;
     Vector UnitZ() const /Factory/;
@@ -209,19 +237,21 @@ public:
     Frame(const Vector& V);
     Frame(const Rotation& R);
     Frame();
-    
+
     Vector p;
     Rotation M;
-    
+
     double __getitem__ (SIP_PYTUPLE) const;
 %MethodCode
     int i,j;
     PyArg_ParseTuple(a0,"ii",&i,&j);
     if (i < 0 || j < 0 || i > 2 || j > 3) {
         PyErr_SetString(PyExc_IndexError, "Frame index out of range");
-        return 0;
+        sipError = sipErrorFail;
+    }
+    else {
+        sipRes=(*sipCpp)(i,j);
     }
-    sipRes=(*sipCpp)(i,j);
 %End
 
     void __setitem__(SIP_PYTUPLE,double value);
@@ -230,12 +260,14 @@ public:
     PyArg_ParseTuple(a0,"ii",&i,&j);
     if (i < 0 || j < 0 || i > 2 || j > 3) {
         PyErr_SetString(PyExc_IndexError, "Frame index out of range");
-        return 0;
+        sipError = sipErrorFail;
+    }
+    else {
+        if(j==3)
+            (*sipCpp).p(i)=a1;
+        else
+            (*sipCpp).M(i,j)=a1;
     }
-    if(j==3)
-        (*sipCpp).p(i)=a1;
-    else
-        (*sipCpp).M(i,j)=a1;
 %End
 
     const std::string* __repr__() const;
@@ -246,6 +278,16 @@ public:
     sipRes=&s;
 %End
 
+    SIP_PYOBJECT __copy__() const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new Frame(*sipCpp), sipFindType("Frame"), NULL);
+%End
+
+    SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new Frame(*sipCpp), sipFindType("Frame"), NULL);
+%End
+
     Frame DH_Craig1989(double a,double alpha,double d,double theta);
     Frame DH(double a,double alpha,double d,double theta);
     Frame Inverse()/Factory/;
@@ -256,9 +298,8 @@ public:
     Vector operator*(const Vector& arg) const /Numeric,Factory/;
     Wrench operator * (const Wrench& arg) const /Numeric,Factory/;
     Twist operator * (const Twist& arg) const /Numeric,Factory/;
-    
+
     static Frame Identity() /Factory/;
-    
     void Integrate(const Twist& t_this,double frequency);
 
 %PickleCode
@@ -274,7 +315,7 @@ bool Equal(const Frame& a,const Frame& b,double eps=epsilon);
 bool operator==(const Frame& a,const Frame& b);
 bool operator!=(const Frame& a,const Frame& b);
 
-class Twist 
+class Twist
 {
 
 %TypeHeaderCode
@@ -286,10 +327,10 @@ using namespace KDL;
 public:
     Vector vel;
     Vector rot;
-    
+
     Twist();
     Twist(const Vector& _vel,const Vector& _rot);
-    
+
     Twist& operator-=(const Twist& arg);
     Twist& operator+=(const Twist& arg);
 
@@ -297,18 +338,22 @@ public:
 %MethodCode
     if (a0 < 0 || a0 > 5) {
         PyErr_SetString(PyExc_IndexError, "Twist index out of range");
-        return 0;
+        sipError = sipErrorFail;
+    }
+    else {
+        sipRes=(*sipCpp)(a0);
     }
-    sipRes=(*sipCpp)(a0);
 %End
 
     void __setitem__(int i, double value);
 %MethodCode
     if (a0 < 0 || a0 > 5) {
         PyErr_SetString(PyExc_IndexError, "Twist index out of range");
-        return 0;
+        sipError = sipErrorFail;
+    }
+    else {
+        (*sipCpp)(a0)=a1;
     }
-    (*sipCpp)(a0)=a1;
 %End
 
     const std::string* __repr__() const;
@@ -319,11 +364,21 @@ public:
     sipRes=&s;
 %End
 
+    SIP_PYOBJECT __copy__() const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new Twist(*sipCpp), sipFindType("Twist"), NULL);
+%End
+
+    SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new Twist(*sipCpp), sipFindType("Twist"), NULL);
+%End
+
     static Twist Zero() /Factory/;
     void ReverseSign();
-    
+
     Twist RefPoint(const Vector& v_base_AB) const /Factory/;
-    
+
 %PickleCode
     const sipTypeDef *vector_type = sipFindType("Vector");
     sipRes = Py_BuildValue("OO", sipConvertFromType(&(sipCpp->vel), vector_type, Py_None),
@@ -347,7 +402,7 @@ bool operator!=(const Twist& a,const Twist& b);
 
 class Wrench
 {
-    
+
 %TypeHeaderCode
 #include <kdl/frames.hpp>
 #include <kdl/frames_io.hpp>
@@ -358,10 +413,10 @@ using namespace KDL;
 public:
     Vector force;
     Vector torque;
-    
+
     Wrench();
     Wrench(const Vector& force,const Vector& torque);
-    
+
     Wrench& operator-=(const Wrench& arg);
     Wrench& operator+=(const Wrench& arg);
 
@@ -369,19 +424,23 @@ public:
     double __getitem__ (int i) const;
 %MethodCode
     if (a0 < 0 || a0 > 5) {
-        PyErr_SetString(PyExc_IndexError, "Twist index out of range");
-        return 0;
+        PyErr_SetString(PyExc_IndexError, "Wrench index out of range");
+        sipError = sipErrorFail;
+    }
+    else {
+        sipRes=(*sipCpp)(a0);
     }
-    sipRes=(*sipCpp)(a0);
 %End
 
     void __setitem__(int i, double value);
 %MethodCode
     if (a0 < 0 || a0 > 5) {
-        PyErr_SetString(PyExc_IndexError, "Twist index out of range");
-        return 0;
+        PyErr_SetString(PyExc_IndexError, "Wrench index out of range");
+        sipError = sipErrorFail;
+    }
+    else {
+        (*sipCpp)(a0)=a1;
     }
-    (*sipCpp)(a0)=a1;
 %End
 
     const std::string* __repr__() const;
@@ -392,6 +451,16 @@ public:
     sipRes=&s;
 %End
 
+    SIP_PYOBJECT __copy__() const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new Wrench(*sipCpp), sipFindType("Wrench"), NULL);
+%End
+
+    SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new Wrench(*sipCpp), sipFindType("Wrench"), NULL);
+%End
+
     static Wrench Zero() /Factory/;
     void ReverseSign();
     Wrench RefPoint(const Vector& v_base_AB) const /Factory/;
diff --git a/python_orocos_kdl/PyKDL/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip
similarity index 65%
rename from python_orocos_kdl/PyKDL/framevel.sip
rename to python_orocos_kdl/PyKDL/sip/framevel.sip
index a3e0ad0ac..b435e3f7a 100644
--- a/python_orocos_kdl/PyKDL/framevel.sip
+++ b/python_orocos_kdl/PyKDL/sip/framevel.sip
@@ -1,8 +1,8 @@
-//Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+//Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 //
 //Version: 1.0
-//Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-//Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+//Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+//Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 //URL: http://www.orocos.org/kdl
 //
 //This library is free software; you can redistribute it and/or
@@ -19,46 +19,95 @@
 //License along with this library; if not, write to the Free Software
 //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
+
 class doubleVel
 {
-        
+
 %TypeHeaderCode
 #include <kdl/framevel.hpp>
+#include <kdl/framevel_io.hpp>
 using namespace KDL;
 %End
 public:
-    typedef Rall1d<double> doubleVel;
+    doubleVel();
+    doubleVel(const double c);
+    doubleVel(const double tn, const double afg);
+    doubleVel(const doubleVel& r);
     double t;
     double grad;
+    double value();
+    double deriv();
+
+    const std::string* __repr__() const;
+%MethodCode
+    std::ostringstream oss;
+    oss<<(*sipCpp);
+    std::string s(oss.str());
+    sipRes=&s;
+%End
+
+    SIP_PYOBJECT __copy__() const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new doubleVel(*sipCpp), sipFindType("doubleVel"), NULL);
+%End
+
+    SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new doubleVel(*sipCpp), sipFindType("doubleVel"), NULL);
+%End
 };
 
+doubleVel operator - (const doubleVel& r);
+
 doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0);
 doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0);
+
 bool Equal(const doubleVel& r1,const doubleVel& r2,double eps=epsilon);
-//bool Equal(const double& r1,const doubleVel& r2,double eps=epsilon);
-//bool Equal(const doubleVel& r1,const double& r2,double eps=epsilon);
+
+bool operator==(const doubleVel& r1,const doubleVel& r2);
+bool operator!=(const doubleVel& r1,const doubleVel& r2);
+
 
 class VectorVel
 {
 %TypeHeaderCode
 #include <kdl/framevel.hpp>
+#include <kdl/framevel_io.hpp>
 using namespace KDL;
 %End
 public:
     Vector p;
-	Vector v;
-    
+    Vector v;
+
     VectorVel();
     VectorVel(const Vector& _p,const Vector& _v);
     VectorVel(const Vector& _p);
     Vector value() const;
     Vector deriv() const;
 
+    const std::string* __repr__() const;
+%MethodCode
+    std::ostringstream oss;
+    oss<<(*sipCpp);
+    std::string s(oss.str());
+    sipRes=&s;
+%End
+
+    SIP_PYOBJECT __copy__() const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new VectorVel(*sipCpp), sipFindType("VectorVel"), NULL);
+%End
+
+    SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new VectorVel(*sipCpp), sipFindType("VectorVel"), NULL);
+%End
+
     VectorVel& operator += (const VectorVel& arg);
     VectorVel& operator -= (const VectorVel& arg);
     static VectorVel Zero();
     void ReverseSign();
-    doubleVel Norm() const;
+    doubleVel Norm(double eps=epsilon) const;
 
 %PickleCode
     const sipTypeDef *vector_type = sipFindType("Vector");
@@ -73,6 +122,7 @@ VectorVel operator + (const Vector& r1,const VectorVel& r2);
 VectorVel operator - (const Vector& r1,const VectorVel& r2);
 VectorVel operator + (const VectorVel& r1,const Vector& r2);
 VectorVel operator - (const VectorVel& r1,const Vector& r2);
+
 VectorVel operator * (const VectorVel& r1,const VectorVel& r2);
 VectorVel operator * (const VectorVel& r1,const Vector& r2);
 VectorVel operator * (const Vector& r1,const VectorVel& r2);
@@ -80,24 +130,33 @@ VectorVel operator * (const VectorVel& r1,double r2);
 VectorVel operator * (double r1,const VectorVel& r2);
 VectorVel operator * (const doubleVel& r1,const VectorVel& r2);
 VectorVel operator * (const VectorVel& r2,const doubleVel& r1);
-VectorVel operator*(const Rotation& R,const VectorVel& x);
+VectorVel operator * (const Rotation& R,const VectorVel& x);
 
 VectorVel operator / (const VectorVel& r1,double r2);
 VectorVel operator / (const VectorVel& r2,const doubleVel& r1);
-
+void SetToZero(VectorVel& v);
 bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon);
 bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon);
 bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon);
 
+bool operator==(const VectorVel& r1,const VectorVel& r2);
+bool operator!=(const VectorVel& r1,const VectorVel& r2);
+bool operator==(const Vector& r1,const VectorVel& r2);
+bool operator!=(const Vector& r1,const VectorVel& r2);
+bool operator==(const VectorVel& r1,const Vector& r2);
+bool operator!=(const VectorVel& r1,const Vector& r2);
+
 VectorVel operator - (const VectorVel& r);
 doubleVel dot(const VectorVel& lhs,const VectorVel& rhs);
 doubleVel dot(const VectorVel& lhs,const Vector& rhs);
 doubleVel dot(const Vector& lhs,const VectorVel& rhs);
 
+
 class RotationVel
 {
 %TypeHeaderCode
 #include <kdl/framevel.hpp>
+#include <kdl/framevel_io.hpp>
 using namespace KDL;
 %End
 
@@ -111,10 +170,27 @@ public:
     Rotation value() const;
     Vector   deriv() const;
 
+    const std::string* __repr__() const;
+%MethodCode
+    std::ostringstream oss;
+    oss<<(*sipCpp);
+    std::string s(oss.str());
+    sipRes=&s;
+%End
 
-	VectorVel UnitX() const;
-	VectorVel UnitY() const;
-	VectorVel UnitZ() const;
+    SIP_PYOBJECT __copy__() const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new RotationVel(*sipCpp), sipFindType("RotationVel"), NULL);
+%End
+
+    SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new RotationVel(*sipCpp), sipFindType("RotationVel"), NULL);
+%End
+
+    VectorVel UnitX() const;
+    VectorVel UnitY() const;
+    VectorVel UnitZ() const;
     static RotationVel Identity();
     RotationVel Inverse() const;
     VectorVel Inverse(const VectorVel& arg) const;
@@ -125,7 +201,7 @@ public:
     void DoRotY(const doubleVel& angle);
     void DoRotZ(const doubleVel& angle);
     static RotationVel RotX(const doubleVel& angle);
-    static RotationVel RotY(const doubleVel& angle);    
+    static RotationVel RotY(const doubleVel& angle);
     static RotationVel RotZ(const doubleVel& angle);
     static RotationVel Rot(const Vector& rotvec,const doubleVel& angle);
     static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle);
@@ -149,14 +225,20 @@ bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon);
 bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon);
 bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon);
 
-
+bool operator==(const RotationVel& r1,const RotationVel& r2);
+bool operator!=(const RotationVel& r1,const RotationVel& r2);
+bool operator==(const Rotation& r1,const RotationVel& r2);
+bool operator!=(const Rotation& r1,const RotationVel& r2);
+bool operator==(const RotationVel& r1,const Rotation& r2);
+bool operator!=(const RotationVel& r1,const Rotation& r2);
 
 
 class FrameVel
 {
-        
+
 %TypeHeaderCode
 #include <kdl/framevel.hpp>
+#include <kdl/framevel_io.hpp>
 using namespace KDL;
 %End
 
@@ -168,10 +250,27 @@ public:
     FrameVel(const Frame& _T);
     FrameVel(const Frame& _T,const Twist& _t);
     FrameVel(const RotationVel& _M,const VectorVel& _p);
-    
+
     Frame value() const;
     Twist deriv() const;
 
+    const std::string* __repr__() const;
+%MethodCode
+    std::ostringstream oss;
+    oss<<(*sipCpp);
+    std::string s(oss.str());
+    sipRes=&s;
+%End
+
+    SIP_PYOBJECT __copy__() const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new FrameVel(*sipCpp), sipFindType("FrameVel"), NULL);
+%End
+
+    SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new FrameVel(*sipCpp), sipFindType("FrameVel"), NULL);
+%End
 
     static FrameVel Identity();
     FrameVel Inverse() const;
@@ -201,12 +300,21 @@ bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon);
 bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon);
 bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon);
 
+bool operator==(const FrameVel& r1,const FrameVel& r2);
+bool operator!=(const FrameVel& r1,const FrameVel& r2);
+bool operator==(const Frame& r1,const FrameVel& r2);
+bool operator!=(const Frame& r1,const FrameVel& r2);
+bool operator==(const FrameVel& r1,const Frame& r2);
+bool operator!=(const FrameVel& r1,const Frame& r2);
+
+
 class TwistVel
 {
 %TypeHeaderCode
 #include <kdl/framevel.hpp>
+#include <kdl/framevel_io.hpp>
 using namespace KDL;
-%End    
+%End
 
 public:
     VectorVel vel;
@@ -219,11 +327,30 @@ public:
 
     Twist value() const;
     Twist deriv() const;
+
+    const std::string* __repr__() const;
+%MethodCode
+    std::ostringstream oss;
+    oss<<(*sipCpp);
+    std::string s(oss.str());
+    sipRes=&s;
+%End
+
+    SIP_PYOBJECT __copy__() const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new TwistVel(*sipCpp), sipFindType("TwistVel"), NULL);
+%End
+
+    SIP_PYOBJECT __deepcopy__(SIP_PYOBJECT) const;
+%MethodCode
+    sipRes = sipConvertFromNewType(new TwistVel(*sipCpp), sipFindType("TwistVel"), NULL);
+%End
+
     TwistVel& operator-=(const TwistVel& arg);
     TwistVel& operator+=(const TwistVel& arg);
 
     static TwistVel Zero();
-    
+
     void ReverseSign();
 
     TwistVel RefPoint(const VectorVel& v_base_AB);
@@ -251,6 +378,13 @@ bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon);
 bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon);
 bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon);
 
+bool operator==(const TwistVel& r1,const TwistVel& r2);
+bool operator!=(const TwistVel& r1,const TwistVel& r2);
+bool operator==(const Twist& r1,const TwistVel& r2);
+bool operator!=(const Twist& r1,const TwistVel& r2);
+bool operator==(const TwistVel& r1,const Twist& r2);
+bool operator!=(const TwistVel& r1,const Twist& r2);
+
 
 VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0);
 VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0);
diff --git a/python_orocos_kdl/PyKDL/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip
similarity index 61%
rename from python_orocos_kdl/PyKDL/kinfam.sip
rename to python_orocos_kdl/PyKDL/sip/kinfam.sip
index d87fd00d6..0302a09a9 100644
--- a/python_orocos_kdl/PyKDL/kinfam.sip
+++ b/python_orocos_kdl/PyKDL/sip/kinfam.sip
@@ -1,8 +1,8 @@
-//Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+//Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 //
 //Version: 1.0
-//Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-//Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+//Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+//Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 //URL: http://www.orocos.org/kdl
 //
 //This library is free software; you can redistribute it and/or
@@ -19,6 +19,9 @@
 //License along with this library; if not, write to the Free Software
 //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
+
+%Feature PYTHON2
+
 class Joint{
 
 %TypeHeaderCode
@@ -29,18 +32,23 @@ using namespace KDL;
 
 
 public:
-    enum JointType {RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,None};
-    Joint(std::string name, JointType type=None,double scale=1,double offset=0,
+%If (!PYTHON2)
+    enum JointType {RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,Fixed};
+%End
+%If (PYTHON2)
+    enum JointType {RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,Fixed,None};
+%End
+    Joint(std::string name, JointType type=Fixed,double scale=1,double offset=0,
               double inertia=0,double damping=0,double stiffness=0);
-    Joint(JointType type=None,double scale=1,double offset=0,
+    Joint(JointType type=Fixed,double scale=1,double offset=0,
           double inertia=0,const double damping=0,double stiffness=0);
     Joint(std::string name, Vector origin, Vector axis, JointType type, double scale=1, double offset=0,
-	      double inertia=0, double damping=0, double stiffness=0);
+          double inertia=0, double damping=0, double stiffness=0);
     Joint(Vector origin, Vector axis, JointType type, double scale=1, double offset=0,
-	      double inertia=0, double damping=0, double stiffness=0);
-    
+          double inertia=0, double damping=0, double stiffness=0);
+
     Joint(const Joint& in);
-    
+
     Frame pose(const double& q)const /Factory/ ;
     Twist twist(const double& qdot)const /Factory/ ;
     Vector JointAxis() const /Factory/;
@@ -49,28 +57,50 @@ public:
 
     JointType getType() const;
     std::string getTypeName() const;
-    const std::string* __repr__();
-    %MethodCode
-        std::ostringstream oss;
-        oss<<(*sipCpp);
-        std::string s(oss.str());
-        sipRes=&s;
-    %End
+    const std::string* __repr__() const;
+%MethodCode
+    std::ostringstream oss;
+    oss<<(*sipCpp);
+    std::string s(oss.str());
+    sipRes=&s;
+%End
 };
 
 class RotationalInertia
 {
 %TypeHeaderCode
 #include <kdl/rotationalinertia.hpp>
-#include <kdl/kinfam_io.hpp>
 using namespace KDL;
 %End
 public:
     RotationalInertia(double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0);
-        
+
     static RotationalInertia Zero()/Factory/;
-    Vector operator*(Vector omega) const /Factory/;
+
+    double __getitem__(int index);
+%MethodCode
+    if (a0 < 0 || a0 >= 9) {
+        PyErr_SetString(PyExc_IndexError, "RotationalInertia index out of range");
+        sipError = sipErrorFail;
+    }
+    else {
+        sipRes=(*sipCpp).data[a0];
+    }
+%End
+
+    void __setitem__(int i, double value);
+%MethodCode
+    if (a0 < 0 || a0 >= 9) {
+        PyErr_SetString(PyExc_IndexError, "RotationalInertia index out of range");
+        sipError = sipErrorFail;
+    }
+    else {
+        (*sipCpp).data[a0]=a1;
+    }
+%End
+
 };
+Vector operator*(RotationalInertia& Ia, Vector omega) const /Factory/;
 RotationalInertia operator*(double a, const RotationalInertia& I)/Factory/;
 RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib)/Factory/;
 
@@ -79,14 +109,13 @@ class RigidBodyInertia
 {
 %TypeHeaderCode
 #include <kdl/rigidbodyinertia.hpp>
-#include <kdl/kinfam_io.hpp>
 using namespace KDL;
 %End
 public:
     RigidBodyInertia(double m=0, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero());
-    static RigidBodyInertia Zero() /Factory/;        
+    static RigidBodyInertia Zero() /Factory/;
     RigidBodyInertia RefPoint(const Vector& p) /Factory/;
-    double getMass()const /Factory/;        
+    double getMass()const /Factory/;
     Vector getCOG() const /Factory/;
     RotationalInertia getRotationalInertia() const /Factory/;
 };
@@ -98,25 +127,25 @@ RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I) /Factory
 
 class Segment
 {
-    
+
 %TypeHeaderCode
 #include <kdl/segment.hpp>
 #include <kdl/kinfam_io.hpp>
 using namespace KDL;
 %End
 public:
-    Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
-    Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
+    Segment(const std::string& name, const Joint& joint=Joint(Joint::Fixed), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
+    Segment(const Joint& joint=Joint(Joint::Fixed), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
     Segment(const Segment& in);
 
-    const std::string* __repr__();
+    const std::string* __repr__() const;
 %MethodCode
-    std::stringstream ss;
-    ss<<(*sipCpp);
-    std::string s(ss.str());
+    std::ostringstream oss;
+    oss<<(*sipCpp);
+    std::string s(oss.str());
     sipRes=&s;
 %End
-    
+
     const Frame& getFrameToTip()const /Factory/;
     Frame pose(const double& q)const /Factory/ ;
     Twist twist(const double& q,const double& qdot)const /Factory/ ;
@@ -128,22 +157,31 @@ public:
 
 class Chain
 {
-    
+
 %TypeHeaderCode
 #include <kdl/chain.hpp>
+#include <kdl/kinfam_io.hpp>
 using namespace KDL;
 %End
 
 public:
     Chain();
     Chain(const Chain& in);
-    
+
     void addSegment(const Segment& segment);
     void addChain(const Chain& chain);
-        
+
     unsigned int getNrOfJoints()const;
     unsigned int getNrOfSegments()const;
-        
+
+    const std::string* __repr__() const;
+%MethodCode
+    std::ostringstream oss;
+    oss<<(*sipCpp);
+    std::string s(oss.str());
+    sipRes=&s;
+%End
+
     const Segment& getSegment(unsigned int nr)const /Factory/;
 
 };
@@ -151,6 +189,7 @@ public:
 class Tree {
 %TypeHeaderCode
 #include <kdl/tree.hpp>
+#include <kdl/kinfam_io.hpp>
 using namespace KDL;
 %End
 public:
@@ -158,7 +197,16 @@ public:
     bool addSegment(const Segment& segment, const std::string& hook_name);
     unsigned int getNrOfJoints()const;
     unsigned int getNrOfSegments()const;
-    Chain* getChain(const std::string& chain_root, const std::string& chain_tip)const;
+
+    const std::string* __repr__() const;
+%MethodCode
+    std::ostringstream oss;
+    oss<<(*sipCpp);
+    std::string s(oss.str());
+    sipRes=&s;
+%End
+
+    Chain* getChain(const std::string& chain_root, const std::string& chain_tip)const /Factory/;
 %MethodCode
     Chain* chain = new Chain();
     sipCpp->getChain(*a0, *a1, *chain);
@@ -170,10 +218,12 @@ class JntArray{
 
 %TypeHeaderCode
 #include <kdl/jntarray.hpp>
+#include <kdl/kinfam_io.hpp>
 using namespace KDL;
 %End
 
 public:
+    JntArray();
     JntArray(unsigned int size);
     JntArray(const JntArray& arg);
     unsigned int rows()const;
@@ -184,25 +234,29 @@ public:
 %MethodCode
     if (a0 < 0 || a0 >= (int)sipCpp->rows()) {
         PyErr_SetString(PyExc_IndexError, "JntArray index out of range");
-        return 0;
+        sipError = sipErrorFail;
+    }
+    else {
+        sipRes=(*sipCpp)(a0);
     }
-    sipRes=(*sipCpp)(a0);
 %End
-    
+
     void __setitem__(int index, double value);
 %MethodCode
     if (a0 < 0 || a0 >= (int)sipCpp->rows()) {
         PyErr_SetString(PyExc_IndexError, "JntArray index out of range");
-        return 0;
+        sipError = sipErrorFail;
+    }
+    else {
+        (*sipCpp)(a0)=a1;
     }
-    (*sipCpp)(a0)=a1;
 %End
 
-    const std::string* __repr__();
+    const std::string* __repr__() const;
 %MethodCode
-    std::stringstream ss;
-    ss<<sipCpp->data;
-    std::string s(ss.str());
+    std::ostringstream oss;
+    oss<<(*sipCpp);
+    std::string s(oss.str());
     sipRes=&s;
 %End
 };
@@ -231,7 +285,7 @@ public:
     JntArrayVel(const JntArray& q,const JntArray& qdot);
     JntArrayVel(const JntArray& q);
     void resize(unsigned int newSize);
-    
+
     JntArray value()const /Factory/;
     JntArray deriv()const /Factory/;
 };
@@ -250,10 +304,12 @@ bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps=epsilon);
 class Jacobian
 {
 %TypeHeaderCode
-#include <kdl/jntarray.hpp>
+#include <kdl/jacobian.hpp>
+#include <kdl/kinfam_io.hpp>
 using namespace KDL;
 %End
 public:
+    Jacobian();
     Jacobian(unsigned int size);
     Jacobian(const Jacobian& arg);
     unsigned int rows()const;
@@ -266,39 +322,43 @@ public:
     PyArg_ParseTuple(a0,"ii",&i,&j);
     if (i < 0 || j < 0 || i > 5 || j >= (int)sipCpp->columns()) {
         PyErr_SetString(PyExc_IndexError, "Jacobian index out of range");
-        return 0;
+        sipError = sipErrorFail;
+    }
+    else {
+        sipRes=(*sipCpp)(i,j);
     }
-    sipRes=(*sipCpp)(i,j);
 %End
-    
+
     void __setitem__(SIP_PYTUPLE,double value);
 %MethodCode
     int i,j;
     PyArg_ParseTuple(a0,"ii",&i,&j);
     if (i < 0 || j < 0 || i > 5 || j >= (int)sipCpp->columns()) {
         PyErr_SetString(PyExc_IndexError, "Jacobian index out of range");
-        return 0;
+        sipError = sipErrorFail;
+    }
+    else {
+        (*sipCpp)(i,j)=a1;
     }
-    (*sipCpp)(i,j)=a1;
 %End
 
-    const std::string* __repr__();
+    const std::string* __repr__() const;
 %MethodCode
-    std::stringstream ss;
-    ss<<sipCpp->data;
-    std::string s(ss.str());
+    std::ostringstream oss;
+    oss<<(*sipCpp);
+    std::string s(oss.str());
     sipRes=&s;
 %End
     Twist getColumn(unsigned int i) const /Factory/;
     void setColumn(unsigned int i,const Twist& t);
-    
+
     void changeRefPoint(const Vector& base_AB);
     void changeBase(const Rotation& rot);
     void changeRefFrame(const Frame& frame);
 
 };
 void SetToZero(Jacobian& jac);
-    
+
 void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest);
 void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest);
 void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest);
@@ -320,7 +380,9 @@ class ChainFkSolverPos : SolverI
 #include <kdl/chainfksolver.hpp>
 using namespace KDL;
 %End
-    virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1)=0;
+    virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1)=0;
+//    Argument by reference doesn't work for container types
+//    virtual int JntToCart(const JntArray& q_in, std::vector<Frame>& p_out, int segmentNr=-1)=0;
 };
 
 class ChainFkSolverVel : SolverI
@@ -329,8 +391,9 @@ class ChainFkSolverVel : SolverI
 #include <kdl/chainfksolver.hpp>
 using namespace KDL;
 %End
-    virtual int JntToCart(const JntArrayVel& q_in, FrameVel& p_out,int
-    segmentNr=-1)=0;
+    virtual int JntToCart(const JntArrayVel& q_in, FrameVel& p_out, int segmentNr=-1)=0;
+//    Argument by reference doesn't work for container types
+//    virtual int JntToCart(const JntArrayVel& q_in, std::vector<FrameVel>& p_out, int segmentNr=-1)=0;
 };
 
 class ChainFkSolverPos_recursive : ChainFkSolverPos
@@ -342,8 +405,13 @@ using namespace KDL;
 
 public:
     ChainFkSolverPos_recursive(const Chain& chain);
-    virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1);
+    virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1);
+//    Argument by reference doesn't work for container types
+//    virtual int JntToCart(const JntArray& q_in, std::vector<Frame>& p_out, int segmentNr=-1);
     virtual void updateInternalDataStructures();
+
+private:
+    ChainFkSolverPos_recursive& operator=(const ChainFkSolverPos_recursive&);
 };
 
 class ChainFkSolverVel_recursive : ChainFkSolverVel
@@ -354,9 +422,13 @@ using namespace KDL;
 %End
 public:
     ChainFkSolverVel_recursive(const Chain& chain);
-    virtual int JntToCart(const JntArrayVel& q_in ,FrameVel& out,int
-    segmentNr=-1 );
+    virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out, int segmentNr=-1);
+//    Argument by reference doesn't work for container types
+//    virtual int JntToCart(const JntArrayVel& q_in, std::vector<FrameVel>& out, int segmentNr=-1 );
     virtual void updateInternalDataStructures();
+
+private:
+    ChainFkSolverVel_recursive& operator=(const ChainFkSolverVel_recursive&);
 };
 
 class ChainIkSolverPos : SolverI {
@@ -389,9 +461,12 @@ using namespace KDL;
 public:
     ChainIkSolverPos_NR(const Chain& chain,ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver,
                         unsigned int maxiter=100,double eps=epsilon);
-    
+
     virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out);
     virtual void updateInternalDataStructures();
+
+private:
+    ChainIkSolverPos_NR& operator=(const ChainIkSolverPos_NR&);
 };
 
 class ChainIkSolverPos_NR_JL : ChainIkSolverPos
@@ -401,12 +476,15 @@ class ChainIkSolverPos_NR_JL : ChainIkSolverPos
 using namespace KDL;
 %End
 public:
-    ChainIkSolverPos_NR_JL(const Chain& chain,const JntArray &q_min,const JntArray &q_max, 
+    ChainIkSolverPos_NR_JL(const Chain& chain,const JntArray &q_min,const JntArray &q_max,
                         ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver,
                         unsigned int maxiter=100,double eps=epsilon);
-    
+
     virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out);
     virtual void updateInternalDataStructures();
+
+private:
+    ChainIkSolverPos_NR_JL& operator=(const ChainIkSolverPos_NR_JL&);
 };
 
 class ChainIkSolverVel_pinv : ChainIkSolverVel
@@ -417,9 +495,12 @@ using namespace KDL;
 %End
 public:
     ChainIkSolverVel_pinv(const Chain& chain,double eps=0.00001,int maxiter=150);
-        
+
     virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
     virtual void updateInternalDataStructures();
+
+private:
+    ChainIkSolverVel_pinv& operator=(const ChainIkSolverVel_pinv&);
 };
 
 class ChainIkSolverVel_wdls : ChainIkSolverVel
@@ -430,7 +511,7 @@ using namespace KDL;
 %End
 public:
     ChainIkSolverVel_wdls(const Chain& chain,double eps=0.00001,int maxiter=150);
-        
+
     virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
     virtual void updateInternalDataStructures();
 
@@ -447,28 +528,41 @@ public:
     temp1=PyList_GetItem(list,0);
     numCols=PyList_Size(temp1);
     if (numRows!=numCols) {
-       sipIsErr=1; //todo: raise exception
+        std::ostringstream oss;
+        oss << "Number of rows(" << numRows << ") and columns(" << numCols << ") don't match" << std::endl;
+        PyErr_SetString(PyExc_ValueError, oss.str().c_str());
+        sipError = sipErrorFail;
     }
-    if (numRows!=6) {
-       sipIsErr=1; //todo: raise exception
+    if (sipError==sipErrorNone && numRows!=6) {
+        std::ostringstream oss;
+        oss << "Number of rows != 6, but " << numRows << std::endl;
+        PyErr_SetString(PyExc_ValueError, oss.str().c_str());
+        sipError = sipErrorFail;
     }
-    Eigen::MatrixXd Mx;
-    Mx=Eigen::MatrixXd::Identity(numRows,numCols);
-    
-    for (Py_ssize_t r=0;r<numRows;r++) {
-        PyObject *row;
-        row=PyList_GetItem(list,r);
-        if (numCols!=PyList_Size(row)) {
-           sipIsErr=1; //todo: raise exception
-        }
-        for (Py_ssize_t c=0;c<numCols;c++) {
-            PyObject *item;
-            item=PyList_GetItem(row,c);
-            c_item=PyFloat_AsDouble(item);
-            Mx(r,c)= c_item;
+    if (sipError==sipErrorNone) {
+        Eigen::MatrixXd Mx;
+        Mx=Eigen::MatrixXd::Identity(numRows,numCols);
+
+        for (Py_ssize_t r=0;r<numRows;r++) {
+            PyObject *row;
+            row=PyList_GetItem(list,r);
+            if (numCols!=PyList_Size(row)) {
+                std::ostringstream oss;
+                oss << "Number of colums(" << PyList_Size(row) << ") of column(" << r << ") doesn't match with the expected number of columns(" << numCols << ")" << std::endl;
+                PyErr_SetString(PyExc_ValueError, oss.str().c_str());
+                sipError = sipErrorFail;
+                break;
+            }
+            for (Py_ssize_t c=0;c<numCols;c++) {
+                PyObject *item;
+                item=PyList_GetItem(row,c);
+                c_item=PyFloat_AsDouble(item);
+                Mx(r,c)= c_item;
+            }
         }
+        if (sipError==sipErrorNone)
+            sipCpp->setWeightTS(Mx);
     }
-    sipCpp->setWeightTS(Mx);
 %End
 
     void setWeightJS(SIP_PYLIST);
@@ -484,28 +578,40 @@ public:
     temp1=PyList_GetItem(list,0);
     numCols=PyList_Size(temp1);
     if (numRows!=numCols) {
-       sipIsErr=1; //todo: raise exception
+        std::ostringstream oss;
+        oss << "Number of rows(" << numRows << ") and columns(" << numCols << ") don't match" << std::endl;
+        PyErr_SetString(PyExc_ValueError, oss.str().c_str());
+        sipError = sipErrorFail;
     }
-    Eigen::MatrixXd Mq;
-    Mq=Eigen::MatrixXd::Identity(numRows,numCols);
-    for (Py_ssize_t r=0;r<numRows;r++) {
-        PyObject *row;
-        row=PyList_GetItem(list,r);
-        if (numCols!=PyList_Size(row)) {
-           sipIsErr=1; //todo: raise exception
-        }
-        for (Py_ssize_t c=0;c<numCols;c++) {
-            PyObject *item;
-            item=PyList_GetItem(row,c);
-            c_item=PyFloat_AsDouble(item);
-            Mq(r,c)= c_item;
+    if (sipError==sipErrorNone) {
+        Eigen::MatrixXd Mq;
+        Mq=Eigen::MatrixXd::Identity(numRows,numCols);
+        for (Py_ssize_t r=0;r<numRows;r++) {
+            PyObject *row;
+            row=PyList_GetItem(list,r);
+            if (numCols!=PyList_Size(row)) {
+                std::ostringstream oss;
+                oss << "Number of colums(" << PyList_Size(row) << ") of column(" << r << ") doesn't match with the expected number of columns(" << numCols << ")" << std::endl;
+                PyErr_SetString(PyExc_ValueError, oss.str().c_str());
+                sipError = sipErrorFail;
+                break;
+            }
+            for (Py_ssize_t c=0;c<numCols;c++) {
+                PyObject *item;
+                item=PyList_GetItem(row,c);
+                c_item=PyFloat_AsDouble(item);
+                Mq(r,c)= c_item;
+            }
         }
+        if (sipError==sipErrorNone)
+            sipCpp->setWeightJS(Mq);
     }
-    sipCpp->setWeightJS(Mq);
 %End
 
     void setLambda(const double& lambda);
 
+private:
+    ChainIkSolverVel_wdls& operator=(const ChainIkSolverVel_wdls&);
 };
 
 
@@ -520,6 +626,9 @@ public:
 
     virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out);
     virtual void updateInternalDataStructures();
+
+private:
+    ChainIkSolverPos_LMA& operator=(const ChainIkSolverPos_LMA&);
 };
 
 
@@ -531,7 +640,7 @@ using namespace KDL;
 %End
 public:
     ChainIkSolverVel_pinv_nso(const Chain& chain,double eps=0.00001,int maxiter=150, double alpha=0.25);
-        
+
     virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
     virtual void updateInternalDataStructures();
 
@@ -546,6 +655,9 @@ public:
     const JntArray& getOptPos()const /Factory/;
 
     const double& getAlpha()const /Factory/;
+
+private:
+    ChainIkSolverVel_pinv_nso& operator=(const ChainIkSolverVel_pinv_nso&);
 };
 
 class ChainIkSolverVel_pinv_givens : ChainIkSolverVel
@@ -556,9 +668,12 @@ using namespace KDL;
 %End
 public:
     ChainIkSolverVel_pinv_givens(const Chain& chain);
-        
+
     virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
     virtual void updateInternalDataStructures();
+
+private:
+    ChainIkSolverVel_pinv_givens& operator=(const ChainIkSolverVel_pinv_givens&);
 };
 
 class ChainJntToJacSolver : SolverI
@@ -569,6 +684,63 @@ using namespace KDL;
 %End
 public:
     ChainJntToJacSolver(const Chain& chain);
-    int JntToJac(const JntArray& q_in,Jacobian& jac);
+    int JntToJac(const JntArray& q_in,Jacobian& jac, int seg_nr=-1);
+    virtual void updateInternalDataStructures();
+
+private:
+    ChainJntToJacSolver& operator=(const ChainJntToJacSolver&);
+};
+
+class ChainJntToJacDotSolver : SolverI
+{
+%TypeHeaderCode
+#include <kdl/chainjnttojacdotsolver.hpp>
+using namespace KDL;
+%End
+public:
+    ChainJntToJacDotSolver(const Chain& chain);
+    int JntToJacDot(const JntArrayVel& q_in,Jacobian& jac, int seg_nr=-1);
     virtual void updateInternalDataStructures();
+    int setLockedJoints(const std::vector<bool>& locked_joints);
+
+    static const int E_JAC_DOT_FAILED;
+    static const int E_JACSOLVER_FAILED;
+    static const int E_FKSOLVERPOS_FAILED;
+
+    static const int HYBRID;
+    static const int BODYFIXED;
+    static const int INERTIAL;
+
+    void setHybridRepresentation();
+    void setBodyFixedRepresentation();
+    void setInertialRepresentation();
+    void setRepresentation(const int& representation);
+
+private:
+    ChainJntToJacDotSolver& operator=(const ChainJntToJacDotSolver&);
+};
+
+class ChainIdSolver : SolverI
+{
+%TypeHeaderCode
+#include <kdl/chainidsolver.hpp>
+using namespace KDL;
+%End
+    virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const std::vector<Wrench>& f_ext,JntArray &torques)=0;
+};
+
+class ChainIdSolver_RNE : ChainIdSolver
+{
+%TypeHeaderCode
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
+using namespace KDL;
+%End
+
+public:
+    ChainIdSolver_RNE(const Chain& chain,Vector grav);
+    int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const std::vector<Wrench>& f_ext,JntArray &torques);
+    virtual void updateInternalDataStructures();
+
+private:
+    ChainIdSolver_RNE& operator=(const KDL::ChainIdSolver_RNE&);
 };
diff --git a/python_orocos_kdl/PyKDL/std_string.sip b/python_orocos_kdl/PyKDL/sip/std_string.sip
similarity index 74%
rename from python_orocos_kdl/PyKDL/std_string.sip
rename to python_orocos_kdl/PyKDL/sip/std_string.sip
index e31324add..33b24e0ba 100644
--- a/python_orocos_kdl/PyKDL/std_string.sip
+++ b/python_orocos_kdl/PyKDL/sip/std_string.sip
@@ -15,6 +15,13 @@
 //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
 
+%ModuleHeaderCode
+#if PY_VERSION_HEX < 0x03000000
+#define PyBytes_AS_STRING PyString_AS_STRING
+#define PyBytes_FromString PyString_FromString
+#define PyBytes_Check PyString_Check
+#endif
+%End
 %MappedType std::string
 {
 %TypeHeaderCode
@@ -39,7 +46,7 @@
     // If argument is a Python string, assume it's UTF-8
      if (sipIsErr == NULL)
 #if PY_MAJOR_VERSION < 3
-        return (PyString_Check(sipPy) || PyUnicode_Check(sipPy));
+        return (PyBytes_Check(sipPy) || PyUnicode_Check(sipPy));
 #else
         return PyUnicode_Check(sipPy);
 #endif
@@ -47,15 +54,20 @@
         *sipCppPtr = new std::string;
          return 1;
      }
+#if PY_MAJOR_VERSION < 3
      if (PyUnicode_Check(sipPy)) {
-        PyObject* s = PyUnicode_AsEncodedString(sipPy, "UTF-8", "");
-        *sipCppPtr = new std::string(PyUnicode_AS_DATA(s));
+        PyObject* s = PyUnicode_AsUTF8String(sipPy);
+        *sipCppPtr = new std::string(PyBytes_AS_STRING(s));
         Py_DECREF(s);
         return 1;
      }
-#if PY_MAJOR_VERSION < 3
-     if (PyString_Check(sipPy)) {
-        *sipCppPtr = new std::string(PyString_AS_STRING(sipPy));
+     else if (PyBytes_Check(sipPy)) {
+        *sipCppPtr = new std::string(PyBytes_AS_STRING(sipPy));
+        return 1;
+     }
+#else
+     if (PyUnicode_Check(sipPy)) {
+        *sipCppPtr = new std::string(PyUnicode_AsUTF8(sipPy));
         return 1;
      }
 #endif
diff --git a/python_orocos_kdl/PyKDL/sip/std_vector.sip b/python_orocos_kdl/PyKDL/sip/std_vector.sip
new file mode 100644
index 000000000..c1545f3bc
--- /dev/null
+++ b/python_orocos_kdl/PyKDL/sip/std_vector.sip
@@ -0,0 +1,353 @@
+// SIP support for std::vector
+// by Giovanni Bajo <rasky <at> develer.com>
+// Public domain
+
+// ****************************************************
+// SIP generic implementation for std::vector<>
+// ****************************************************
+// ALas, this template-based generic implementation is valid only
+// if the element type is a SIP-wrapped type. For basic types (int, double, etc.)
+// we are forced to cut & paste to provide a specialization.
+
+template<TYPE>
+%MappedType std::vector<TYPE>
+{
+%TypeHeaderCode
+#include <vector>
+%End
+
+%ConvertFromTypeCode
+    PyObject *l = PyList_New(sipCpp -> size());
+
+    // Create the Python list of the correct length.
+    if (!l)
+        return NULL;
+
+    // Go through each element in the C++ instance and convert it to a
+    // wrapped P2d.
+    for (int i = 0; i < (int)sipCpp->size(); ++i) {
+        TYPE *cpp = new TYPE(sipCpp->at(i));
+        PyObject *pobj = sipConvertFromInstance(cpp, sipClass_TYPE, sipTransferObj);
+
+        // Get the Python wrapper for the Type instance, creating a new
+        // one if necessary, and handle any ownership transfer.
+        if (!pobj) {
+            // There was an error so garbage collect the Python list.
+            Py_DECREF(l);
+            return NULL;
+        }
+
+        // Add the wrapper to the list.
+        PyList_SET_ITEM(l, i, pobj);
+    }
+
+    // Return the Python list.
+    return l;
+%End
+
+%ConvertToTypeCode
+    // Check if type is compatible
+    if (!sipIsErr) {
+        // Must be any iterable
+        PyObject *i = PyObject_GetIter(sipPy);
+        bool iterable = (i != NULL);
+        Py_XDECREF(i);
+        return iterable;
+    }
+
+    // Iterate over the object
+    PyObject *iterator = PyObject_GetIter(sipPy);
+    PyObject *item;
+
+    std::vector<TYPE> *V = new std::vector<TYPE>();
+
+    while ((item = PyIter_Next(iterator)))
+    {
+        if (!sipCanConvertToInstance(item, sipClass_TYPE, SIP_NOT_NONE)) {
+            PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to TYPE");
+            *sipIsErr = 1;
+            break;
+        }
+
+        int state;
+        TYPE* p = reinterpret_cast<TYPE*>(
+             sipConvertToInstance(item, sipClass_TYPE, 0, SIP_NOT_NONE, &state, sipIsErr));
+
+        if (!*sipIsErr)
+            V->push_back(*p);
+
+        sipReleaseInstance(p, sipClass_TYPE, state);
+        Py_DECREF(item);
+    }
+
+    Py_DECREF(iterator);
+
+    if (*sipIsErr) {
+        delete V;
+        return 0;
+    }
+
+    *sipCppPtr = V;
+    return sipGetState(sipTransferObj);
+%End
+};
+
+// ****************************************************
+// Specialization for std::vector<double>
+// ****************************************************
+
+%MappedType std::vector<double>
+{
+%TypeHeaderCode
+#include <vector>
+%End
+
+%ConvertFromTypeCode
+    PyObject *l;
+
+    // Create the Python list of the correct length.
+    if ((l = PyList_New(sipCpp -> size())) == NULL)
+        return NULL;
+
+    // Go through each element in the C++ instance and convert it to a
+    // wrapped object.
+    for (int i = 0; i < (int)sipCpp -> size(); ++i)
+    {
+        // Add the wrapper to the list.
+        PyList_SET_ITEM(l, i, PyFloat_FromDouble(sipCpp -> at(i)));
+    }
+
+    // Return the Python list.
+    return l;
+%End
+
+%ConvertToTypeCode
+    // Check if type is compatible
+    if (sipIsErr == NULL)
+    {
+        // Must be any iterable
+        PyObject *i = PyObject_GetIter(sipPy);
+        bool iterable = (i != NULL);
+        Py_XDECREF(i);
+        return iterable;
+    }
+
+    // Iterate over the object
+    PyObject *iterator = PyObject_GetIter(sipPy);
+    PyObject *item;
+
+    // Maximum number of elements
+    int len = PyObject_Size(sipPy);
+    std::vector<double> *V = new std::vector<double>();
+    V->reserve(len);
+
+    if (len)
+    {
+        while ((item = PyIter_Next(iterator)))
+        {
+            if (!PyNumber_Check(item))
+            {
+                PyErr_Format(PyExc_TypeError, "object in iterable is not a number");
+                *sipIsErr = 1;
+                break;
+            }
+
+            PyObject *f = PyNumber_Float(item);
+            V->push_back(PyFloat_AsDouble(f));
+
+            Py_DECREF(f);
+            Py_DECREF(item);
+        }
+
+        Py_DECREF(iterator);
+
+        if (*sipIsErr)
+        {
+            delete V;
+            return 0;
+        }
+    }
+
+    *sipCppPtr = V;
+    return sipGetState(sipTransferObj);
+%End
+};
+
+// ****************************************************
+// Specialization for std::vector<int>
+// ****************************************************
+
+%MappedType std::vector<int>
+{
+%TypeHeaderCode
+#include <vector>
+
+#if PY_VERSION_HEX < 0x03000000
+#define PyLong_AsLong PyInt_AsLong
+#define PyLong_FromLong PyInt_FromLong
+#define PyLong_Check PyInt_Check
+#endif
+%End
+
+%ConvertFromTypeCode
+    PyObject *l;
+
+    // Create the Python list of the correct length.
+    if ((l = PyList_New((SIP_SSIZE_T)sipCpp -> size())) == NULL)
+        return NULL;
+
+    // Go through each element in the C++ instance and convert it to a
+    // wrapped object.
+    for (int i = 0; i < (int)sipCpp -> size(); ++i)
+    {
+        // Add the wrapper to the list.
+        PyList_SET_ITEM(l, i, PyLong_FromLong(sipCpp -> at(i)));
+    }
+
+    // Return the Python list.
+    return l;
+%End
+
+%ConvertToTypeCode
+    // Check if type is compatible
+    if (sipIsErr == NULL)
+    {
+        // Must be any iterable
+        PyObject *i = PyObject_GetIter(sipPy);
+        bool iterable = (i != NULL);
+        Py_XDECREF(i);
+        return iterable;
+    }
+
+    // Iterate over the object
+    PyObject *iterator = PyObject_GetIter(sipPy);
+    PyObject *item;
+
+    // Maximum number of elements
+    int len = PyObject_Size(sipPy);
+    std::vector<int> *V = new std::vector<int>();
+    V->reserve(len);
+
+    if (len)
+    {
+        while ((item = PyIter_Next(iterator)))
+        {
+            if (!PyLong_Check(item))
+            {
+                PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to float");
+                *sipIsErr = 1;
+                break;
+            }
+
+            int val = PyLong_AsLong(item);
+            V->push_back(val);
+
+            Py_DECREF(item);
+        }
+
+        Py_DECREF(iterator);
+
+        if (*sipIsErr)
+        {
+            delete V;
+            return 0;
+        }
+    }
+
+    *sipCppPtr = V;
+    return sipGetState(sipTransferObj);
+%End
+};
+
+
+// ****************************************************
+// Specialization for std::vector<unsigned int>
+// ****************************************************
+
+%MappedType std::vector<unsigned int>
+{
+%TypeHeaderCode
+#include <vector>
+
+#if PY_VERSION_HEX < 0x03000000
+#define PyLong_AsLong PyInt_AsLong
+#define PyLong_FromLong PyInt_FromLong
+#define PyLong_Check PyInt_Check
+#endif
+%End
+
+%ConvertFromTypeCode
+    PyObject *l;
+
+    // Create the Python list of the correct length.
+    if ((l = PyList_New(sipCpp -> size())) == NULL)
+        return NULL;
+
+    // Go through each element in the C++ instance and convert it to a
+    // wrapped object.
+    for (int i = 0; i < (int)sipCpp -> size(); ++i)
+    {
+        // Add the wrapper to the list.
+        PyList_SET_ITEM(l, i, PyLong_FromLong(sipCpp -> at(i)));
+    }
+
+    // Return the Python list.
+    return l;
+%End
+
+%ConvertToTypeCode
+    // Check if type is compatible
+    if (sipIsErr == NULL)
+    {
+        // Must be any iterable
+        PyObject *i = PyObject_GetIter(sipPy);
+        bool iterable = (i != NULL);
+        Py_XDECREF(i);
+        return iterable;
+    }
+
+    // Iterate over the object
+    PyObject *iterator = PyObject_GetIter(sipPy);
+    PyObject *item;
+
+    // Maximum number of elements
+    Py_ssize_t size = PyObject_Size(sipPy);
+    if (size == -1) {
+        Py_DECREF(iterator);
+        return 0;
+    }
+
+    unsigned int len = size;
+    std::vector<unsigned int> *V = new std::vector<unsigned int>();
+    V->reserve(len);
+
+    if (len)
+    {
+        while ((item = PyIter_Next(iterator)))
+        {
+            if (!PyLong_Check(item))
+            {
+                PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to float");
+                *sipIsErr = 1;
+                break;
+            }
+
+            unsigned int val = PyLong_AsLong(item);
+            V->push_back(val);
+
+            Py_DECREF(item);
+        }
+
+        Py_DECREF(iterator);
+
+        if (*sipIsErr)
+        {
+            delete V;
+            return 0;
+        }
+    }
+
+    *sipCppPtr = V;
+    return sipGetState(sipTransferObj);
+%End
+};
diff --git a/python_orocos_kdl/cmake/SIPMacros.cmake b/python_orocos_kdl/cmake/SIPMacros.cmake
index 4464a9267..836387ad3 100644
--- a/python_orocos_kdl/cmake/SIPMacros.cmake
+++ b/python_orocos_kdl/cmake/SIPMacros.cmake
@@ -92,22 +92,12 @@ MACRO(ADD_SIP_PYTHON_MODULE MODULE_NAME MODULE_SIP)
         ENDIF( ${CONCAT_NUM} LESS ${SIP_CONCAT_PARTS} )
     ENDFOREACH(CONCAT_NUM RANGE 0 ${SIP_CONCAT_PARTS} )
 
-    IF(NOT WIN32)
-        SET(TOUCH_COMMAND touch)
-    ELSE(NOT WIN32)
-        SET(TOUCH_COMMAND echo)
-        # instead of a touch command, give out the name and append to the files
-        # this is basically what the touch command does.
-        FOREACH(filename ${_sip_output_files})
-            FILE(APPEND filename "")
-        ENDFOREACH(filename ${_sip_output_files})
-    ENDIF(NOT WIN32)
     ADD_CUSTOM_COMMAND(
-        OUTPUT ${_sip_output_files} 
+        OUTPUT ${_sip_output_files}
         COMMAND ${CMAKE_COMMAND} -E echo ${message}
-        COMMAND ${TOUCH_COMMAND} ${_sip_output_files} 
+        COMMAND ${CMAKE_COMMAND} -E touch ${_sip_output_files}
         COMMAND ${SIP_EXECUTABLE} ${_sip_tags} ${_sip_x} ${SIP_EXTRA_OPTIONS} -j ${SIP_CONCAT_PARTS} -c ${CMAKE_CURRENT_SIP_OUTPUT_DIR} ${_sip_includes} ${_abs_module_sip}
-        DEPENDS ${_abs_module_sip} ${SIP_EXTRA_FILES_DEPEND}
+        DEPENDS ${SIP_INCLUDES} ${SIP_EXTRA_FILES_DEPEND}
     )
     # not sure if type MODULE could be uses anywhere, limit to cygwin for now
     IF (CYGWIN OR APPLE)
@@ -118,6 +108,9 @@ MACRO(ADD_SIP_PYTHON_MODULE MODULE_NAME MODULE_SIP)
     TARGET_LINK_LIBRARIES(${_logical_name} ${PYTHON_LIBRARY})
     TARGET_LINK_LIBRARIES(${_logical_name} ${EXTRA_LINK_LIBRARIES})
     SET_TARGET_PROPERTIES(${_logical_name} PROPERTIES PREFIX "" OUTPUT_NAME ${_child_module_name})
+    IF (MSVC)
+        SET_TARGET_PROPERTIES(${_logical_name} PROPERTIES SUFFIX ".pyd")
+    ENDIF(MSVC)
 
     INSTALL(TARGETS ${_logical_name} DESTINATION "${PYTHON_SITE_PACKAGES_INSTALL_DIR}/${_parent_module_path}")
 
diff --git a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.bash.in b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.bash.in
new file mode 120000
index 000000000..47a63df22
--- /dev/null
+++ b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.bash.in
@@ -0,0 +1 @@
+python_orocos_kdl_site_packages.sh.in
\ No newline at end of file
diff --git a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.sh.in b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.sh.in
new file mode 100644
index 000000000..13d938d3c
--- /dev/null
+++ b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.sh.in
@@ -0,0 +1 @@
+export PYTHONPATH=@PYTHON_SITE_PACKAGES_INSTALL_DIR@:$PYTHONPATH
diff --git a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.tcsh.in b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.tcsh.in
new file mode 120000
index 000000000..47a63df22
--- /dev/null
+++ b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.tcsh.in
@@ -0,0 +1 @@
+python_orocos_kdl_site_packages.sh.in
\ No newline at end of file
diff --git a/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.zsh.in b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.zsh.in
new file mode 120000
index 000000000..47a63df22
--- /dev/null
+++ b/python_orocos_kdl/env-hooks/python_orocos_kdl_site_packages.zsh.in
@@ -0,0 +1 @@
+python_orocos_kdl_site_packages.sh.in
\ No newline at end of file
diff --git a/python_orocos_kdl/mainpage.dox b/python_orocos_kdl/mainpage.dox
deleted file mode 100644
index 0be6e3112..000000000
--- a/python_orocos_kdl/mainpage.dox
+++ /dev/null
@@ -1,26 +0,0 @@
-/**
-\mainpage
-\htmlinclude manifest.html
-
-\b python_orocos_kdl is ... 
-
-<!-- 
-Provide an overview of your package.
--->
-
-
-\section codeapi Code API
-
-<!--
-Provide links to specific auto-generated API documentation within your
-package that is of particular interest to a reader. Doxygen will
-document pretty much every part of your code, so do your best here to
-point the reader to the actual API.
-
-If your codebase is fairly large or has different sets of APIs, you
-should use the doxygen 'group' tag to keep these APIs together. For
-example, the roscpp documentation has 'libros' group.
--->
-
-
-*/
diff --git a/python_orocos_kdl/manifest.xml b/python_orocos_kdl/manifest.xml
deleted file mode 100644
index ae3cd5ac7..000000000
--- a/python_orocos_kdl/manifest.xml
+++ /dev/null
@@ -1,22 +0,0 @@
-<package>
-  <description brief="python_orocos_kdl">
-
-     python_orocos_kdl
-
-  </description>
-  <author>Ruben Smits</author>
-  <license>LGPL</license>
-  <review status="unreviewed" notes=""/>
-  <url>http://ros.org/wiki/python_orocos_kdl</url>
-
-  <depend package="orocos_kdl"/>
-  <rosdep name="python-sip"/>
-
-  <export>
-    <python path="${prefix}/lib"  />
-    <rosdoc config="rosdoc.yaml"/>
-  </export>
-
-</package>
-
-
diff --git a/python_orocos_kdl/package.xml b/python_orocos_kdl/package.xml
index 3fe99b324..03c11530e 100644
--- a/python_orocos_kdl/package.xml
+++ b/python_orocos_kdl/package.xml
@@ -1,9 +1,10 @@
-<package>
+<?xml version="1.0"?>
+<package format="3">
   <name>python_orocos_kdl</name>
   <version>1.4.0</version>
   <description>
     This package contains the python bindings PyKDL for the Kinematics and Dynamics
-    Library (KDL), distributed by the Orocos Project. 
+    Library (KDL), distributed by the Orocos Project.
   </description>
   <maintainer email="ruben@intermodalics.eu">Ruben Smits</maintainer>
   <url>http://wiki.ros.org/python_orocos_kdl</url>
@@ -14,12 +15,21 @@
   <build_depend>orocos_kdl</build_depend>
   <build_depend>python-sip</build_depend>
 
-  <run_depend>catkin</run_depend>
-  <run_depend>orocos_kdl</run_depend>
-  <run_depend>python-sip</run_depend>
+  <exec_depend>catkin</exec_depend>
+  <exec_depend>orocos_kdl</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-sip</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-sip</exec_depend>
+
+  <test_depend condition="$ROS_PYTHON_VERSION == 2">python-future</test_depend>
+  <test_depend condition="$ROS_PYTHON_VERSION == 3">python3-future</test_depend>
+  <test_depend condition="$ROS_PYTHON_VERSION == 2">python-psutil</test_depend>
+  <test_depend condition="$ROS_PYTHON_VERSION == 3">python3-psutil</test_depend>
+
+  <doc_depend>python-sphinx</doc_depend>
 
   <export>
     <build_type>cmake</build_type>
+    <rosdoc config="rosdoc.yaml"/>
   </export>
 
 </package>
diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11
new file mode 160000
index 000000000..59a2ac274
--- /dev/null
+++ b/python_orocos_kdl/pybind11
@@ -0,0 +1 @@
+Subproject commit 59a2ac2745d8a57ac94c6accced73620d59fb844
diff --git a/python_orocos_kdl/rosdoc.yaml b/python_orocos_kdl/rosdoc.yaml
index daa8282ec..b2f4d1f9e 100644
--- a/python_orocos_kdl/rosdoc.yaml
+++ b/python_orocos_kdl/rosdoc.yaml
@@ -1,3 +1,4 @@
 - builder: sphinx
   output_dir: doc
-  sphinx_root_dir: doc
\ No newline at end of file
+  sphinx_root_dir: doc
+  name: Python API
diff --git a/python_orocos_kdl/tests/PyKDLtest.py b/python_orocos_kdl/tests/PyKDLtest.py
index 37375da10..47a1595c0 100644
--- a/python_orocos_kdl/tests/PyKDLtest.py
+++ b/python_orocos_kdl/tests/PyKDLtest.py
@@ -1,9 +1,9 @@
-#!/usr/bin/python
-# Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+#! /usr/bin/env python
+# Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 # Version: 1.0
-# Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-# Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+# Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+# Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 # URL: http://www.orocos.org/kdl
 
 # This library is free software; you can redistribute it and/or
@@ -19,14 +19,28 @@
 # You should have received a copy of the GNU Lesser General Public
 # License along with this library; if not, write to the Free Software
 # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+
 import unittest
+import dynamicstest
 import kinfamtest
 import framestest
 import frameveltest
+import sys
 
 suite = unittest.TestSuite()
+suite.addTest(dynamicstest.suite())
 suite.addTest(framestest.suite())
 suite.addTest(frameveltest.suite())
 suite.addTest(kinfamtest.suite())
 
-unittest.TextTestRunner(verbosity=3).run(suite)
+if sys.version_info < (3, 0):
+    import jointtypetest
+    suite.addTest(jointtypetest.suite())
+
+result = unittest.TextTestRunner(verbosity=3).run(suite)
+
+if result.wasSuccessful():
+    sys.exit(0)
+else:
+    sys.exit(1)
diff --git a/python_orocos_kdl/tests/dynamicstest.py b/python_orocos_kdl/tests/dynamicstest.py
new file mode 100644
index 000000000..d9b889bb7
--- /dev/null
+++ b/python_orocos_kdl/tests/dynamicstest.py
@@ -0,0 +1,75 @@
+# Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
+
+# Version: 1.0
+# Author: Matthijs van der Burgh <MatthijsBurgh at outlook dot com>
+# Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
+# URL: http://www.orocos.org/kdl
+
+# This library is free software; you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation; either
+# version 2.1 of the License, or (at your option) any later version.
+
+# This library is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# Lesser General Public License for more details.
+
+# You should have received a copy of the GNU Lesser General Public
+# License along with this library; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+
+from PyKDL import *
+import unittest
+
+
+class DynamicsTestFunctions(unittest.TestCase):
+    def testJntSpaceInertiaMatrix(self):
+        ll = 3
+        jm = JntSpaceInertiaMatrix(3)
+        # __getitem__
+        for i in range(ll):
+            for j in range(ll):
+                self.assertEqual(jm[i, j], 0)
+        with self.assertRaises(IndexError):
+            _ = jm[-1, 0]
+        with self.assertRaises(IndexError):
+            _ = jm[3, 0]
+        with self.assertRaises(IndexError):
+            _ = jm[2, -1]
+        with self.assertRaises(IndexError):
+            _ = jm[2, 3]
+
+        # __setitem__
+        for i in range(ll):
+            for j in range(ll):
+                jm[i, j] = 3 * i + j
+        for i in range(ll):
+            for j in range(ll):
+                self.assertEqual(jm[i, j], 3 * i + j)
+        with self.assertRaises(IndexError):
+            jm[-1, 0] = 1
+        with self.assertRaises(IndexError):
+            jm[3, 0] = 1
+        with self.assertRaises(IndexError):
+            jm[2, -1] = 1
+        with self.assertRaises(IndexError):
+            jm[2, 3] = 1
+
+
+def suite():
+    suite = unittest.TestSuite()
+    suite.addTest(DynamicsTestFunctions('testJntSpaceInertiaMatrix'))
+    return suite
+
+
+if __name__ == '__main__':
+    import sys
+    suite = suite()
+    result = unittest.TextTestRunner(verbosity=3).run(suite)
+
+    if result.wasSuccessful():
+        sys.exit(0)
+    else:
+        sys.exit(1)
\ No newline at end of file
diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py
index 41429dcf4..e8953cfcc 100644
--- a/python_orocos_kdl/tests/framestest.py
+++ b/python_orocos_kdl/tests/framestest.py
@@ -1,8 +1,9 @@
-# Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+# Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 # Version: 1.0
-# Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-# Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+# Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+# Author: Matthijs van der Burgh <MatthijsBurgh at outlook dot com>
+# Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 # URL: http://www.orocos.org/kdl
 
 # This library is free software; you can redistribute it and/or
@@ -20,167 +21,402 @@
 # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
 
-import unittest
+from math import radians, sqrt
 from PyKDL import *
-from math import *
-    
+import sys
+import unittest
+
+
 class FramesTestFunctions(unittest.TestCase):
+    def testVector(self):
+        v = Vector(3, 4, 5)
+        self.testVectorImpl(v)
+        v = Vector()
+        self.testVectorImpl(v)
+
+        # Equality
+        v = Vector(3, 4, 5)
+        self.assertFalse(v == -v)  # Doesn't work for zero vector
+        self.assertFalse(Equal(v, -v))  # Doesn't work for zero vector
+        self.assertTrue(v != -v)  # Doesn't work for zero vector
+        self.assertTrue(not Equal(v, -v))  # Doesn't work for zero vector
 
-    def testVector2(self,v):
-        self.assertEqual(2*v-v,v)
-        self.assertEqual(v*2-v,v)
-        self.assertEqual(v+v+v-2*v,v)
-        v2=Vector(v)
-        self.assertEqual(v,v2)
-        v2+=v
-        self.assertEqual(2*v,v2)
-        v2-=v
-        self.assertEqual(v,v2)
+        # Test member get and set functions
+        self.assertEqual(v.x(), 3)
+        v.x(1)
+        self.assertEqual(v, Vector(1, 4, 5))
+        self.assertEqual(v.y(), 4)
+        v.y(1)
+        self.assertEqual(v, Vector(1, 1, 5))
+        self.assertEqual(v.z(), 5)
+        v.z(1)
+        self.assertEqual(v, Vector(1, 1, 1))
+
+        # __getitem__
+        self.assertEqual(v[0], 1)
+        self.assertEqual(v[1], 1)
+        self.assertEqual(v[2], 1)
+        with self.assertRaises(IndexError):
+            _ = v[-1]
+        with self.assertRaises(IndexError):
+            _ = v[3]
+
+        # __setitem__
+        v[0] = 3
+        self.assertEqual(v[0], 3)
+        v[1] = 4
+        self.assertEqual(v[1], 4)
+        v[2] = 5
+        self.assertEqual(v[2], 5)
+        with self.assertRaises(IndexError):
+            v[-1] = 1
+        with self.assertRaises(IndexError):
+            v[3] = 1
+
+        # Zero
+        SetToZero(v)
+        self.assertEqual(v, Vector(0, 0, 0))
+        self.assertEqual(Vector.Zero(), Vector(0, 0, 0))
+
+    def testVectorImpl(self, v):
+        self.assertTrue(v == v)
+        self.assertTrue(Equal(v, v))
+        self.assertFalse(v != v)
+        self.assertFalse(not Equal(v, v))
+        self.assertEqual(2*v-v, v)
+        self.assertEqual(v*2-v, v)
+        self.assertEqual(v+v+v-2*v, v)
+        v2 = Vector(v)
+        self.assertEqual(v, v2)
+        v2 += v
+        self.assertEqual(2*v, v2)
+        v2 -= v
+        self.assertEqual(v, v2)
         v2.ReverseSign()
-        self.assertEqual(v,-v2)
+        self.assertEqual(v, -v2)
+        for v2 in [Vector(v), -Vector(v)]:
+            self.assertAlmostEqual(v2.Norm()**2, sum(map(lambda i: i * i, v2)), delta=1e-10)
+            self.assertEqual(v2.Norm(), v2.Normalize())  # Norm before Normalize, so taking norm of un-normalized vector
 
-    def testVector(self):
-        v=Vector(3,4,5)
-        self.testVector2(v)
-        v=Vector.Zero()
-        self.testVector2(v)
-    
-    def testTwist2(self,t):
-        self.assertEqual(2*t-t,t)
-        self.assertEqual(t*2-t,t)
-        self.assertEqual(t+t+t-2*t,t)
-        t2=Twist(t)
-        self.assertEqual(t,t2)
-        t2+=t
-        self.assertEqual(2*t,t2)
-        t2-=t
-        self.assertEqual(t,t2)
-        t.ReverseSign()
-        self.assertEqual(t,-t2)
+        self.assertEqual(dot(v, v), sum(map(lambda i: i * i, v)))
 
     def testTwist(self):
-        t=Twist(Vector(6,3,5),Vector(4,-2,7))
-        self.testTwist2(t)
-        t=Twist.Zero()
-        self.testTwist2(t)
-        t=Twist(Vector(0,-9,-3),Vector(1,-2,-4))
-
-    def testWrench2(self,w):
-        self.assertEqual(2*w-w,w)
-        self.assertEqual(w*2-w,w)
-        self.assertEqual(w+w+w-2*w,w)
-        w2=Wrench(w)
-        self.assertEqual(w,w2)
-        w2+=w
-        self.assertEqual(2*w,w2)
-        w2-=w
-        self.assertEqual(w,w2)
-        w.ReverseSign()
-        self.assertEqual(w,-w2)
+        t = Twist(Vector(6, 3, 5), Vector(4, -2, 7))
+        self.testTwistImpl(t)
+        t = Twist()
+        self.testTwistImpl(t)
+        t = Twist(Vector(0, -9, -3), Vector(1, -2, -4))
+        self.testTwistImpl(t)
+
+        # Equality
+        t = Twist(Vector(1, 2, 3), Vector(1, 2, 3))
+        self.assertFalse(t == -t)  # Doesn't work for zero twist
+        self.assertFalse(Equal(t, -t))  # Doesn't work for zero twist
+        self.assertTrue(t != -t)  # Doesn't work for zero twist
+        self.assertTrue(not Equal(t, -t))  # Doesn't work for zero twist
+
+        # Members
+        v1 = Vector(1, 2, 3)
+        v2 = Vector(4, 5, 6)
+        t = Twist(v1, v2)
+        self.assertEqual(t.vel, v1)
+        self.assertEqual(t.rot, v2)
+        self.assertEqual(Twist(t).vel, t.vel)
+        self.assertEqual(Twist(t).rot, t.rot)
+
+        # __getitem__
+        for i in range(6):
+            self.assertEqual(t[i], i+1)
+        with self.assertRaises(IndexError):
+            _ = t[-1]
+        with self.assertRaises(IndexError):
+            _ = t[6]
+
+        # __setitem__
+        for i in range(6):
+            t[i] = i
+        for i in range(6):
+            self.assertEqual(t[i], i)
+        with self.assertRaises(IndexError):
+            t[-1] = 1
+        with self.assertRaises(IndexError):
+            t[6] = 1
+
+        # Zero
+        SetToZero(t)
+        self.assertEqual(t, Twist())
+        self.assertEqual(Twist.Zero(), Twist())
+
+    def testTwistImpl(self, t):
+        self.assertTrue(t == t)
+        self.assertTrue(Equal(t, t))
+        self.assertFalse(t != t)
+        self.assertFalse(not Equal(t, t))
+        self.assertEqual(2*t-t, t)
+        self.assertEqual(t*2-t, t)
+        self.assertEqual(t+t+t-2*t, t)
+        t2 = Twist(t)
+        self.assertEqual(t, t2)
+        t2 += t
+        self.assertEqual(2*t, t2)
+        t2 -= t
+        self.assertEqual(t, t2)
+        t2.ReverseSign()
+        self.assertEqual(t, -t2)
+        v = Vector(1, 2, 3)
+        t2 = t.RefPoint(v)
+        self.assertEqual(t2.vel, t.vel + t.rot*v)
+        self.assertEqual(t2.rot, t.rot)
+
+        # No need to test the dot functions again for Wrench
+        w = Wrench(v, v)
+        dot_result = dot(t.vel, w.force) + dot(t.rot, w.torque)
+        self.assertEqual(dot(t, w), dot_result)
+        self.assertEqual(dot(w, t), dot_result)
 
     def testWrench(self):
-        w=Wrench(Vector(7,-1,3),Vector(2,-3,3))
-        self.testWrench2(w)
-        w=Wrench.Zero()
-        self.testWrench2(w)
-        w=Wrench(Vector(2,1,4),Vector(5,3,1))
-        self.testWrench2(w)
-
-    def testRotation2(self,v,a,b,c):
-        w=Wrench(Vector(7,-1,3),Vector(2,-3,3))
-        t=Twist(Vector(6,3,5),Vector(4,-2,7))
-        R=Rotation.RPY(a,b,c)
-        
-        self.assertAlmostEqual(dot(R.UnitX(),R.UnitX()),1.0,15)
-        self.assertEqual(dot(R.UnitY(),R.UnitY()),1.0)
-        self.assertEqual(dot(R.UnitZ(),R.UnitZ()),1.0)
-        self.assertAlmostEqual(dot(R.UnitX(),R.UnitY()),0.0,15)
-        self.assertAlmostEqual(dot(R.UnitX(),R.UnitZ()),0.0,15)
-        self.assertEqual(dot(R.UnitY(),R.UnitZ()),0.0)
-        R2=Rotation(R)
-        self.assertEqual(R,R2)
-        self.assertAlmostEqual((R*v).Norm(),v.Norm(),14)
-        self.assertEqual(R.Inverse(R*v),v)
-        self.assertEqual(R.Inverse(R*t),t)
-        self.assertEqual(R.Inverse(R*w),w)
-        self.assertEqual(R*R.Inverse(v),v)
-        self.assertEqual(R*Rotation.Identity(),R)
-        self.assertEqual(Rotation.Identity()*R,R)
-        self.assertEqual(R*(R*(R*v)),(R*R*R)*v)
-        self.assertEqual(R*(R*(R*t)),(R*R*R)*t)
-        self.assertEqual(R*(R*(R*w)),(R*R*R)*w)
-        self.assertEqual(R*R.Inverse(),Rotation.Identity())
-        self.assertEqual(R.Inverse()*R,Rotation.Identity())
-        self.assertEqual(R.Inverse()*v,R.Inverse(v))
-        (ra,rb,rc)=R.GetRPY()
-        self.assertEqual(ra,a)
-        self.assertEqual(rb,b)
-        self.assertEqual(rc,c)
-        R = Rotation.EulerZYX(a,b,c)
-        (ra,rb,rc)=R.GetEulerZYX()
-        self.assertEqual(ra,a)
-        self.assertEqual(rb,b)
-        self.assertEqual(rc,c)
-        R = Rotation.EulerZYZ(a,b,c)
-        (ra,rb,rc)=R.GetEulerZYZ()
-        self.assertEqual(ra,a)
-        self.assertEqual(rb,b)
-        self.assertAlmostEqual(rc,c,15)
-        (angle,v2)= R.GetRotAngle()
-        R2=Rotation.Rot(v2,angle)
-        self.assertEqual(R2,R)
-        R2=Rotation.Rot(v2*1E20,angle)
-        self.assertEqual(R,R2)
-        v2=Vector(6,2,4)
-        self.assertAlmostEqual(v2.Norm(),sqrt(dot(v2,v2)),14)
-    
+        w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3))
+        self.testWrenchImpl(w)
+        w = Wrench()
+        self.testWrenchImpl(w)
+        w = Wrench(Vector(2, 1, 4), Vector(5, 3, 1))
+        self.testWrenchImpl(w)
+
+        # Equality
+        w = Wrench(Vector(1, 2, 3), Vector(1, 2, 3))
+        self.assertFalse(w == -w)  # Doesn't work for zero wrench
+        self.assertFalse(Equal(w, -w))  # Doesn't work for zero wrench
+        self.assertTrue(w != -w)  # Doesn't work for zero wrench
+        self.assertTrue(not Equal(w, -w))  # Doesn't work for zero wrench
+
+        # Members
+        v1 = Vector(1, 2, 3)
+        v2 = Vector(4, 5, 6)
+        w = Wrench(v1, v2)
+        self.assertEqual(w.force, v1)
+        self.assertEqual(w.torque, v2)
+        self.assertEqual(Wrench(w).force, w.force)
+        self.assertEqual(Wrench(w).torque, w.torque)
+
+        # __getitem__
+        for i in range(6):
+            self.assertEqual(w[i], i+1)
+        with self.assertRaises(IndexError):
+            _ = w[-1]
+        with self.assertRaises(IndexError):
+            _ = w[6]
+
+        # __setitem__
+        for i in range(6):
+            w[i] = i
+        for i in range(6):
+            self.assertEqual(w[i], i)
+        with self.assertRaises(IndexError):
+            w[-1] = 1
+        with self.assertRaises(IndexError):
+            w[6] = 1
+
+        # Zero
+        SetToZero(w)
+        self.assertEqual(w, Wrench())
+        self.assertEqual(Wrench.Zero(), Wrench())
+
+    def testWrenchImpl(self, w):
+        self.assertEqual(2*w-w, w)
+        self.assertEqual(w*2-w, w)
+        self.assertEqual(w+w+w-2*w, w)
+        w2 = Wrench(w)
+        self.assertEqual(w, w2)
+        w2 += w
+        self.assertEqual(2*w, w2)
+        w2 -= w
+        self.assertEqual(w, w2)
+        w2.ReverseSign()
+        self.assertEqual(w, -w2)
+
+        v = Vector(1, 2, 3)
+        w2 = w.RefPoint(v)
+        self.assertEqual(w2.force, w.force)
+        self.assertEqual(w2.torque, w.torque + w.force*v)
+
     def testRotation(self):
-        self.testRotation2(Vector(3,4,5),radians(10),radians(20),radians(30))
-    
+        self.testRotationImpl(Rotation.RPY(radians(10), radians(20), radians(30)), Vector(3, 4, 5))
+        self.testRotationImpl(Rotation.RPY(radians(10), radians(20), radians(30)), Vector())
+        self.testRotationImpl(Rotation(), Vector(3, 4, 5))
+        self.testRotationImpl(Rotation(), Vector())
+
+        r = Rotation(*range(1, 10))
+
+        # __getitem__
+        for i in range(3):
+            for j in range(3):
+                self.assertEqual(r[i, j], 3*i+j+1)
+        with self.assertRaises(IndexError):
+            _ = r[-1, 0]
+        with self.assertRaises(IndexError):
+            _ = r[0, -1]
+        with self.assertRaises(IndexError):
+            _ = r[3, 2]
+        with self.assertRaises(IndexError):
+            _ = r[2, 3]
+
+        # __setitem__
+        for i in range(3):
+            for j in range(3):
+                r[i, j] = 3*i+j
+        for i in range(3):
+            for j in range(3):
+                self.assertEqual(r[i, j], 3*i+j)
+        with self.assertRaises(IndexError):
+            r[-1, 0] = 1
+        with self.assertRaises(IndexError):
+            r[0, -1] = 1
+        with self.assertRaises(IndexError):
+            r[3, 2] = 1
+        with self.assertRaises(IndexError):
+            r[2, 3] = 1
+
+    def testRotationImpl(self, r, v):
+        w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3))
+        t = Twist(Vector(6, 3, 5), Vector(4, -2, 7))
+        a, b, c = r.GetRPY()
+
+        self.assertAlmostEqual(dot(r.UnitX(), r.UnitX()), 1.0, 15)
+        self.assertEqual(dot(r.UnitY(), r.UnitY()), 1.0)
+        self.assertEqual(dot(r.UnitZ(), r.UnitZ()), 1.0)
+        self.assertAlmostEqual(dot(r.UnitX(), r.UnitY()), 0.0, 15)
+        self.assertAlmostEqual(dot(r.UnitX(), r.UnitZ()), 0.0, 15)
+        self.assertAlmostEqual(dot(r.UnitY(), r.UnitZ()), 0.0, 15)
+        r2 = Rotation(r)
+        self.assertEqual(r, r2)
+        self.assertAlmostEqual((r*v).Norm(), v.Norm(), 14)
+        self.assertEqual(r.Inverse(r*v), v)
+        self.assertEqual(r.Inverse(r*t), t)
+        self.assertEqual(r.Inverse(r*w), w)
+        self.assertEqual(r*r.Inverse(v), v)
+        self.assertEqual(r*Rotation.Identity(), r)
+        self.assertEqual(Rotation.Identity()*r, r)
+        self.assertEqual(r*(r*(r*v)), (r*r*r)*v)
+        self.assertEqual(r*(r*(r*t)), (r*r*r)*t)
+        self.assertEqual(r*(r*(r*w)), (r*r*r)*w)
+        self.assertEqual(r*r.Inverse(), Rotation.Identity())
+        self.assertEqual(r.Inverse()*r, Rotation.Identity())
+        self.assertEqual(r.Inverse()*v, r.Inverse(v))
+        (ra, rb, rc) = r.GetRPY()
+        self.assertEqual(ra, a)
+        self.assertEqual(rb, b)
+        self.assertEqual(rc, c)
+        r = Rotation.EulerZYX(a, b, c)
+        (ra, rb, rc) = r.GetEulerZYX()
+        self.assertEqual(ra, a)
+        self.assertEqual(rb, b)
+        self.assertEqual(rc, c)
+        r = Rotation.EulerZYZ(a, b, c)
+        (ra, rb, rc) = r.GetEulerZYZ()
+        self.assertEqual(ra, a)
+        self.assertEqual(rb, b)
+        self.assertAlmostEqual(rc, c, 15)
+        (angle, v2) = r.GetRotAngle()
+        r2 = Rotation.Rot(v2, angle)
+        self.assertEqual(r2, r)
+        r2 = Rotation.Rot(v2*1E20, angle)
+        self.assertEqual(r, r2)
+        v2 = Vector(6, 2, 4)
+        self.assertAlmostEqual(v2.Norm(), sqrt(dot(v2, v2)), 14)
+
     def testFrame(self):
-        v=Vector(3,4,5) 
-        w=Wrench(Vector(7,-1,3),Vector(2,-3,3))
-        t=Twist(Vector(6,3,5),Vector(4,-2,7))
-        F = Frame(Rotation.EulerZYX(radians(10),radians(20),radians(-10)),Vector(4,-2,1))
-        F2=Frame(F)
-        self.assertEqual(F,F2)
-        self.assertEqual(F.Inverse(F*v),v)
-        self.assertEqual(F.Inverse(F*t),t)
-        self.assertEqual(F.Inverse(F*w),w)
-        self.assertEqual(F*F.Inverse(v),v)
-        self.assertEqual(F*F.Inverse(t),t)
-        self.assertEqual(F*F.Inverse(w),w)
-        self.assertEqual(F*Frame.Identity(),F)
-        self.assertEqual(Frame.Identity()*F,F)
-        self.assertEqual(F*(F*(F*v)),(F*F*F)*v)
-        self.assertEqual(F*(F*(F*t)),(F*F*F)*t)
-        self.assertEqual(F*(F*(F*w)),(F*F*F)*w)
-        self.assertEqual(F*F.Inverse(),Frame.Identity())
-        self.assertEqual(F.Inverse()*F,Frame.Identity())
-        self.assertEqual(F.Inverse()*v,F.Inverse(v))
+        v = Vector(3, 4, 5)
+        w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3))
+        t = Twist(Vector(6, 3, 5), Vector(4, -2, 7))
+        f = Frame()
+        self.testFrameImpl(v, w, t, f)
+        r = Rotation.EulerZYX(radians(10), radians(20), radians(-10))
+        v2 = Vector(4, -2, 1)
+        f = Frame(r, v2)
+        self.testFrameImpl(v, w, t, f)
+
+        # Equality
+        f2 = Frame(f)
+        self.assertEqual(f, f2)
+
+        # Members
+        self.assertEqual(f.M, r)
+        self.assertEqual(f.p, v2)
+        self.assertEqual(Frame(f).M, f.M)
+        self.assertEqual(Frame(f).p, f.p)
+
+        f = Frame(Rotation(1, 2, 3,
+                           5, 6, 7,
+                           9, 10, 11),
+                  Vector(4, 8, 12))
+        # __getitem__
+        for i in range(3):
+            for j in range(4):
+                self.assertEqual(f[i, j], 4*i+j+1)
+        with self.assertRaises(IndexError):
+            _ = f[-1, 0]
+        with self.assertRaises(IndexError):
+            _ = f[0, -1]
+        with self.assertRaises(IndexError):
+            _ = f[3, 3]
+        with self.assertRaises(IndexError):
+            _ = f[2, 4]
+
+        # __setitem__
+        for i in range(3):
+            for j in range(4):
+                f[i, j] = 4*i+j
+        for i in range(3):
+            for j in range(4):
+                self.assertEqual(f[i, j], 4*i+j)
+        with self.assertRaises(IndexError):
+            f[-1, 0] = 1
+        with self.assertRaises(IndexError):
+            f[0, -1] = 1
+        with self.assertRaises(IndexError):
+            f[3, 3] = 1
+        with self.assertRaises(IndexError):
+            f[2, 4] = 1
+
+    def testFrameImpl(self, v, w, t, f):
+        self.assertEqual(f.Inverse(f*v), v)
+        self.assertEqual(f.Inverse(f*t), t)
+        self.assertEqual(f.Inverse(f*w), w)
+        self.assertEqual(f*f.Inverse(v), v)
+        self.assertEqual(f*f.Inverse(t), t)
+        self.assertEqual(f*f.Inverse(w), w)
+        self.assertEqual(f*Frame.Identity(), f)
+        self.assertEqual(Frame.Identity()*f, f)
+        self.assertEqual(f*(f*(f*v)), (f*f*f)*v)
+        self.assertEqual(f*(f*(f*t)), (f*f*f)*t)
+        self.assertEqual(f*(f*(f*w)), (f*f*f)*w)
+        self.assertEqual(f*f.Inverse(), Frame.Identity())
+        self.assertEqual(f.Inverse()*f, Frame.Identity())
+        self.assertEqual(f.Inverse()*v, f.Inverse(v))
 
     def testPickle(self):
-        import pickle
-        data = {}
-        data['v'] = Vector(1,2,3)
-        data['rot'] = Rotation.RotX(1.3)
+        if sys.version_info < (3, 0):
+            import cPickle as pickle
+        else:
+            import pickle
+        data = dict()
+        data['v'] = Vector(1, 2, 3)
+        data['rot'] = Rotation().EulerZYZ(1, 2, 3)
         data['fr'] = Frame(data['rot'], data['v'])
-        data['tw'] = Twist(data['v'], Vector(4,5,6))
-        data['wr'] = Wrench(Vector(0.1,0.2,0.3), data['v'])
-        
-        f = open('/tmp/pickle_test', 'w')
-        pickle.dump(data, f)
-        f.close()
-        
-        f = open('/tmp/pickle_test', 'r')
-        data1 = pickle.load(f)
-        f.close()
-       
+        data['tw'] = Twist(data['v'], Vector(4, 5, 6))
+        data['wr'] = Wrench(Vector(0.1, 0.2, 0.3), data['v'])
+
+        with open('/tmp/pickle_test', 'wb') as f:
+            pickle.dump(data, f, 2)
+
+        with open('/tmp/pickle_test', 'rb') as f:
+            data1 = pickle.load(f)
+
         self.assertEqual(data, data1)
 
 
 def suite():
-    suite=unittest.TestSuite()
+    suite = unittest.TestSuite()
     suite.addTest(FramesTestFunctions('testVector'))
     suite.addTest(FramesTestFunctions('testTwist'))
     suite.addTest(FramesTestFunctions('testWrench'))
@@ -188,6 +424,13 @@ def suite():
     suite.addTest(FramesTestFunctions('testFrame'))
     suite.addTest(FramesTestFunctions('testPickle'))
     return suite
-    
-#suite = suite()
-#unittest.TextTestRunner(verbosity=3).run(suite)
+
+
+if __name__ == '__main__':
+    suite = suite()
+    result = unittest.TextTestRunner(verbosity=3).run(suite)
+
+    if result.wasSuccessful():
+        sys.exit(0)
+    else:
+        sys.exit(1)
diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py
index ae0e89051..dc627b0ad 100644
--- a/python_orocos_kdl/tests/frameveltest.py
+++ b/python_orocos_kdl/tests/frameveltest.py
@@ -1,100 +1,270 @@
-import unittest
+# Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
+
+# Version: 1.0
+# Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+# Author: Matthijs van der Burgh <MatthijsBurgh at outlook dot com>
+# Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
+# URL: http://www.orocos.org/kdl
+
+# This library is free software; you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation; either
+# version 2.1 of the License, or (at your option) any later version.
+
+# This library is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# Lesser General Public License for more details.
+
+# You should have received a copy of the GNU Lesser General Public
+# License along with this library; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+
+from math import radians
 from PyKDL import *
-from math import *
+import sys
+import unittest
+
 
 class FrameVelTestFunctions(unittest.TestCase):
+    def testdoubleVel(self):
+        d = doubleVel()
+        self.assertEqual(doubleVel(d), d)
+        self.assertEqual(d.t, 0)
+        self.assertEqual(d.grad, 0)
+        self.assertEqual(d.value(), 0)
+        self.assertEqual(d.deriv(), 0)
+        d2 = -d
+        self.assertEqual(d2.t, -d.t)
+        self.assertEqual(d2.grad, -d.grad)
 
     def testVectorVel(self):
-        v=VectorVel(Vector(3,-4,5),Vector(6,3,-5))
-        vt = Vector(-4,-6,-8);
-        self.assertTrue(Equal( 2*v-v,v))
-        self.assertTrue(Equal( v*2-v,v))
-        self.assertTrue(Equal( v+v+v-2*v,v))
-        v2=VectorVel(v)
-        self.assertTrue(Equal( v,v2))
-        v2+=v
-        self.assertTrue(Equal( 2*v,v2))
-        v2-=v
-        self.assertTrue(Equal( v,v2))
+        v = VectorVel()
+        vt = Vector()
+        self.testVectorVelImpl(v, vt)
+        vt = Vector(-4, -6, -8)
+        self.testVectorVelImpl(v, vt)
+        v1 = Vector(3, -4, 5)
+        v2 = Vector(6, 3, -5)
+        v = VectorVel(v1, v2)
+        self.testVectorVelImpl(v, vt)
+
+        # Members
+        self.assertEqual(v.p, v1)
+        self.assertEqual(v.v, v2)
+        self.assertEqual(v.value(), v1)
+        self.assertEqual(v.deriv(), v2)
+
+        # Equality
+        self.assertEqual(VectorVel(v).p, v.p)
+        self.assertEqual(VectorVel(v).v, v.v)
+        self.assertFalse(v == -v)  # Doesn't work for zero VectorVel
+        self.assertFalse(Equal(v, -v))  # Doesn't work for zero VectorVel
+        self.assertTrue(v != -v)  # Doesn't work for zero VectorVel
+        self.assertTrue(not Equal(v, -v))  # Doesn't work for zero VectorVel
+
+        v = VectorVel(v1)
+        self.assertEqual(v, v1)
+        self.assertEqual(v1, v)
+
+        # Zero
+        v = VectorVel(v1, v2)
+        SetToZero(v)
+        self.assertEqual(v, VectorVel())
+        self.assertEqual(VectorVel.Zero(), VectorVel())
+
+        # dot functions
+        v = VectorVel(v1, v2)
+        self.assertEqual(dot(v, v), doubleVel(dot(v.p, v.p), dot(v.p, v.v)+dot(v.v, v.p)))
+        dot_result = doubleVel(dot(v.p, v1), dot(v.v, v1))
+        self.assertEqual(dot(v, v1), dot_result)
+        self.assertEqual(dot(v1, v), dot_result)
+
+    def testVectorVelImpl(self, v, vt):
+        self.assertEqual(2*v-v, v)
+        self.assertEqual(v*2-v, v)
+        self.assertEqual(v+v+v-2*v, v)
+        v2 = VectorVel(v)
+        self.assertEqual(v, v2)
+        v2 += v
+        self.assertEqual(2*v, v2)
+        v2 -= v
+        self.assertEqual(v, v2)
         v2.ReverseSign()
-        self.assertTrue(Equal( v,-v2))
-        self.assertTrue(Equal( v*vt,-vt*v))
-        v2 = VectorVel(Vector(-5,-6,-3),Vector(3,4,5))
-        self.assertTrue(Equal( v*v2,-v2*v))
+        self.assertEqual(v, -v2)
+        self.assertEqual(v*vt, -vt*v)
+        v2 = VectorVel(Vector(-5, -6, -3), Vector(3, 4, 5))
+        self.assertEqual(v*v2, -v2*v)
+
+    def testTwistVel(self):
+        t = TwistVel()
+        self.testTwistVelImpl(t)
+        v1 = Vector(1, 2, 3)
+        v2 = Vector(4, 5, 6)
+        vv1 = VectorVel(v1, v2)
+        vv2 = VectorVel(v2, v1)
+        t = TwistVel(vv1, vv2)
+        self.testTwistVelImpl(t)
+
+        # Alternative constructor
+        tw1 = Twist(v1, v2)
+        tw2 = Twist(v2, v1)
+        t2 = TwistVel(tw1, tw2)
+        self.assertEqual(t, t2)
+
+        # Members
+        self.assertEqual(t.vel, vv1)
+        self.assertEqual(t.rot, vv2)
+        self.assertEqual(t2.value(), tw1)
+        self.assertEqual(t2.deriv(), tw2)
+        self.assertEqual(t2.GetTwist(), tw1)
+        self.assertEqual(t2.GetTwistDot(), tw2)
+
+        # Equality
+        self.assertEqual(TwistVel(t).vel, t.vel)
+        self.assertEqual(TwistVel(t).rot, t.rot)
+        self.assertFalse(t == -t)  # Doesn't work for zero TwistVel
+        self.assertFalse(Equal(t, -t))  # Doesn't work for zero TwistVel
+        self.assertTrue(t != -t)  # Doesn't work for zero TwistVel
+        self.assertTrue(not Equal(t, -t))  # Doesn't work for zero TwistVel
+
+        t = TwistVel(VectorVel(v1), VectorVel(v2))
+        t2 = Twist(v1, v2)
+        self.assertEqual(t, t2)
+        self.assertEqual(t2, t)
+
+        # Zero
+        SetToZero(t)
+        self.assertEqual(t, TwistVel())
+        self.assertEqual(TwistVel.Zero(), TwistVel())
+
+    def testTwistVelImpl(self, t):
+        self.assertEqual(2*t-t, t)
+        self.assertEqual(t*2-t, t)
+        self.assertEqual(t+t+t-2*t, t)
+        t2 = TwistVel(t)
+        self.assertEqual(t, t2)
+        t2 += t
+        self.assertEqual(2*t, t2)
+        t2 -= t
+        self.assertEqual(t, t2)
+        t2.ReverseSign()
+        self.assertEqual(t, -t2)
+        self.assertEqual(t*doubleVel(), doubleVel()*t)
+        self.assertEqual(t*doubleVel(5), doubleVel(5)*t)
+        self.assertEqual(t * doubleVel(3, 5), doubleVel(3, 5) * t)
 
        
     def testRotationVel(self):
-        v=VectorVel(Vector(9,4,-2),Vector(-5,6,-2))
-        vt=Vector(2,3,4)
-        a= radians(-15)
-        b= radians(20)
-        c= radians(-80)
-        R = RotationVel(Rotation.RPY(a,b,c),Vector(2,4,1))
-        R2=RotationVel(R)
-        self.assertTrue(Equal(R,R2))
-        self.assertTrue(Equal((R*v).Norm(),(v.Norm())))
-        self.assertTrue(Equal(R.Inverse(R*v),v))
-        self.assertTrue(Equal(R*R.Inverse(v),v))
-        self.assertTrue(Equal(R*Rotation.Identity(),R))
-        self.assertTrue(Equal(Rotation.Identity()*R,R))
-        self.assertTrue(Equal(R*(R*(R*v)),(R*R*R)*v))
-        self.assertTrue(Equal(R*(R*(R*vt)),(R*R*R)*vt))
-        self.assertTrue(Equal(R*R.Inverse(),RotationVel.Identity()))
-        self.assertTrue(Equal(R.Inverse()*R,RotationVel.Identity()))
-        self.assertTrue(Equal(R.Inverse()*v,R.Inverse(v)))
-        #v2=v*v-2*v
-        #print dot(v2,v2)
-        #self.assertTrue(Equal((v2).Norm(),sqrt(dot(v2,v2))))
+        v = VectorVel()
+        vt = Vector()
+        r = RotationVel()
+        self.testRotationVelImpl(r, v, vt)
+        v = VectorVel(Vector(9, 4, -2), Vector(-5, 6, -2))
+        vt = Vector(2, 3, 4)
+        rot = Rotation.RPY(radians(-15), radians(20), radians(-80))
+        vec = Vector(2, 4, 1)
+        r = RotationVel(rot, vec)
+        self.testRotationVelImpl(r, v, vt)
+
+        # Members
+        self.assertEqual(r.R, rot)
+        self.assertEqual(r.w, vec)
+        self.assertEqual(r.value(), rot)
+        self.assertEqual(r.deriv(), vec)
+
+        # Equality
+        self.assertEqual(RotationVel(r).R, r.R)
+        self.assertEqual(RotationVel(r).w, r.w)
+        self.assertEqual(RotationVel(rot), rot)
+        self.assertEqual(rot, RotationVel(rot))
+
+    def testRotationVelImpl(self, r, v, vt):
+        r2 = RotationVel(r)
+        self.assertEqual(r, r2)
+        self.assertEqual((r*v).Norm(), v.Norm())
+        self.assertEqual(r.Inverse(r*v), v)
+        self.assertEqual(r*r.Inverse(v), v)
+        self.assertEqual(r*Rotation.Identity(), r)
+        self.assertEqual(Rotation.Identity()*r, r)
+        self.assertEqual(r*(r*(r*v)), (r*r*r)*v)
+        self.assertEqual(r*(r*(r*vt)), (r*r*r)*vt)
+        self.assertEqual(r*r.Inverse(), RotationVel.Identity())
+        self.assertEqual(r.Inverse()*r, RotationVel.Identity())
+        self.assertEqual(r.Inverse()*v, r.Inverse(v))
 
     def testFrameVel(self):
-        v=VectorVel(Vector(3,4,5),Vector(-2,-4,-1))
-        vt=Vector(-1,0,-10)
-        F = FrameVel(Frame(Rotation.EulerZYX(radians(10),radians(20),radians(-10)),Vector(4,-2,1)),
-                     Twist(Vector(2,-2,-2),Vector(-5,-3,-2)))
-        F2=FrameVel(F)
-        self.assertTrue(Equal( F,F2))
-        self.assertTrue(Equal( F.Inverse(F*v),v))
-        self.assertTrue(Equal( F.Inverse(F*vt), vt))
-        self.assertTrue(Equal( F*F.Inverse(v),v))
-        self.assertTrue(Equal( F*F.Inverse(vt),vt))
-        self.assertTrue(Equal( F*Frame.Identity(),F))
-        self.assertTrue(Equal( Frame.Identity()*F,F))
-        self.assertTrue(Equal( F*(F*(F*v)),(F*F*F)*v))
-        self.assertTrue(Equal( F*(F*(F*vt)),(F*F*F)*vt))
-        self.assertTrue(Equal( F*F.Inverse(),FrameVel.Identity()))
-        self.assertTrue(Equal( F.Inverse()*F,Frame.Identity()))
-        self.assertTrue(Equal( F.Inverse()*vt,F.Inverse(vt)))
+        v = VectorVel()
+        vt = Vector()
+        f = FrameVel()
+        self.testFrameVelImpl(f, v, vt)
+        fr_m = Rotation.EulerZYX(radians(10), radians(20), radians(-10))
+        fr_p = Vector(4, -2, 1)
+        tw_vel = Vector(2, -2, -2)
+        tw_rot = Vector(-5, -3, -2)
+        fr = Frame(fr_m, fr_p)
+        tw = Twist(tw_vel, tw_rot)
+        f = FrameVel(fr, tw)
+        self.testFrameVelImpl(f, v, vt)
+        v = VectorVel(Vector(3, 4, 5), Vector(-2, -4, -1))
+        vt = Vector(-1, 0, -10)
+        self.testFrameVelImpl(f, v, vt)
+
+        # Alternative constructor
+        rv = RotationVel(fr_m, tw_rot)
+        vv = VectorVel(fr_p, tw_vel)
+        f2 = FrameVel(rv, vv)
+        self.assertEqual(f, f2)
+
+        # Members
+        self.assertEqual(f.M, rv)
+        self.assertEqual(f.p, vv)
+        self.assertEqual(f2.value(), fr)
+        self.assertEqual(f2.deriv(), tw)
+
+        # Equality
+        self.assertEqual(FrameVel(f).M, f.M)
+        self.assertEqual(FrameVel(f).p, f.p)
+
+        f = FrameVel(fr)
+        self.assertEqual(f, fr)
+        self.assertEqual(fr, f)
+
+    def testFrameVelImpl(self, f, v, vt):
+        f2 = FrameVel(f)
+        self.assertEqual(f, f2)
+        self.assertEqual(f.Inverse(f*v), v)
+        self.assertEqual(f.Inverse(f*vt), vt)
+        self.assertEqual(f*f.Inverse(v), v)
+        self.assertEqual(f*f.Inverse(vt), vt)
+        self.assertEqual(f*Frame.Identity(), f)
+        self.assertEqual(Frame.Identity()*f, f)
+        self.assertEqual(f*(f*(f*v)), (f*f*f)*v)
+        self.assertEqual(f*(f*(f*vt)), (f*f*f)*vt)
+        self.assertEqual(f*f.Inverse(), FrameVel.Identity())
+        self.assertEqual(f.Inverse()*f, Frame.Identity())
+        self.assertEqual(f.Inverse()*vt, f.Inverse(vt))
 
 
     def testPickle(self):
-        rot = Rotation.RotX(1.3)
-        import pickle
+        if sys.version_info < (3, 0):
+            import cPickle as pickle
+        else:
+            import pickle
         data = {}
-        data['vv'] = VectorVel(Vector(1,2,3), Vector(4,5,6))
-        data['rv'] = RotationVel(rot, Vector(4.1,5.1,6.1))
+        data['vv'] = VectorVel(Vector(1, 2, 3), Vector(4, 5, 6))
+        data['rv'] = RotationVel(Rotation.RotX(1.3), Vector(4.1, 5.1, 6.1))
         data['fv'] = FrameVel(data['rv'], data['vv'])
         data['tv'] = TwistVel(data['vv'], data['vv'])
-        
-        f = open('/tmp/pickle_test_kdl_framevel', 'w')
-        pickle.dump(data, f)
-        f.close()
-        
-        f = open('/tmp/pickle_test_kdl_framevel', 'r')
-        data1 = pickle.load(f)
-        f.close()
-       
-        self.assertEqual(data['vv'].p, data1['vv'].p)
-        self.assertEqual(data['vv'].v, data1['vv'].v)
-        self.assertEqual(data['rv'].R, data1['rv'].R)
-        self.assertEqual(data['rv'].w, data1['rv'].w)
-        self.assertEqual(data['fv'].M.R, data1['fv'].M.R)
-        self.assertEqual(data['fv'].M.w, data1['fv'].M.w)
-        self.assertEqual(data['fv'].p.p, data1['fv'].p.p)
-        self.assertEqual(data['fv'].p.v, data1['fv'].p.v)
-        self.assertEqual(data['tv'].vel.p, data1['tv'].vel.p)
-        self.assertEqual(data['tv'].vel.v, data1['tv'].vel.v)
-        self.assertEqual(data['tv'].rot.p, data1['tv'].rot.p)
-        self.assertEqual(data['tv'].rot.v, data1['tv'].rot.v)
+
+        with open('/tmp/pickle_test_kdl_framevel', 'wb') as f:
+            pickle.dump(data, f, 2)
+
+        with open('/tmp/pickle_test_kdl_framevel', 'rb') as f:
+            data1 = pickle.load(f)
+
+        self.assertEqual(data, data1)
 
 #void TestTwistVel() {
 #    KDL_CTX;
@@ -189,8 +359,10 @@ def testPickle(self):
 #
 
 def suite():
-    suite=unittest.TestSuite()
+    suite = unittest.TestSuite()
+    suite.addTest(FrameVelTestFunctions('testdoubleVel'))
     suite.addTest(FrameVelTestFunctions('testVectorVel'))
+    suite.addTest(FrameVelTestFunctions('testTwistVel'))
     suite.addTest(FrameVelTestFunctions('testRotationVel'))
     suite.addTest(FrameVelTestFunctions('testFrameVel'))
     suite.addTest(FrameVelTestFunctions('testPickle'))
@@ -201,3 +373,11 @@ def suite():
 
 
 
+if __name__ == '__main__':
+    suite = suite()
+    result = unittest.TextTestRunner(verbosity=3).run(suite)
+
+    if result.wasSuccessful():
+        sys.exit(0)
+    else:
+        sys.exit(1)
diff --git a/python_orocos_kdl/tests/jointtypetest.py b/python_orocos_kdl/tests/jointtypetest.py
new file mode 100644
index 000000000..7081b962e
--- /dev/null
+++ b/python_orocos_kdl/tests/jointtypetest.py
@@ -0,0 +1,48 @@
+# Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
+
+# Version: 1.0
+# Author: Matthijs van der Burgh <MatthijsBurgh at outlook dot com>
+# Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
+# URL: http://www.orocos.org/kdl
+
+# This library is free software; you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation; either
+# version 2.1 of the License, or (at your option) any later version.
+
+# This library is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# Lesser General Public License for more details.
+
+# You should have received a copy of the GNU Lesser General Public
+# License along with this library; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+
+from PyKDL import Joint
+import unittest
+
+
+class JointTypeNoneTest(unittest.TestCase):
+    def testJointType(self):
+        self.assertEqual(Joint.Fixed, Joint.None)
+        self.assertEqual(str(Joint.Fixed), str(Joint.None))
+        self.assertEqual(int(Joint.Fixed), int(Joint.None))
+
+
+def suite():
+    suite = unittest.TestSuite()
+    suite.addTest(JointTypeNoneTest('testJointType'))
+    return suite
+
+
+if __name__ == '__main__':
+    import sys
+    suite = suite()
+    result = unittest.TextTestRunner(verbosity=3).run(suite)
+
+    if result.wasSuccessful():
+        sys.exit(0)
+    else:
+        sys.exit(1)
diff --git a/python_orocos_kdl/tests/kinfamtest.py b/python_orocos_kdl/tests/kinfamtest.py
index 19f39c0f5..5db21d2b9 100644
--- a/python_orocos_kdl/tests/kinfamtest.py
+++ b/python_orocos_kdl/tests/kinfamtest.py
@@ -1,8 +1,9 @@
-# Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+# Copyright  (C)  2020  Ruben Smits <ruben dot smits at intermodalics dot eu>
 
 # Version: 1.0
-# Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
-# Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+# Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
+# Author: Matthijs van der Burgh <MatthijsBurgh at outlook dot com>
+# Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
 # URL: http://www.orocos.org/kdl
 
 # This library is free software; you can redistribute it and/or
@@ -20,138 +21,250 @@
 # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
 
-import unittest
+from builtins import range
+
+import gc
+import psutil
 from PyKDL import *
-from math import *
 import random
+import sys
+import unittest
+
 
 class KinfamTestFunctions(unittest.TestCase):
-    
     def setUp(self):
         self.chain = Chain()
         self.chain.addSegment(Segment(Joint(Joint.RotZ),
-                                 Frame(Vector(0.0,0.0,0.0))))
+                                      Frame(Vector(0.0, 0.0, 0.0))))
         self.chain.addSegment(Segment(Joint(Joint.RotX),
-                                 Frame(Vector(0.0,0.0,0.9))))
-        self.chain.addSegment(Segment(Joint(Joint.None),
-                                 Frame(Vector(-0.4,0.0,0.0))))
+                                      Frame(Vector(0.0, 0.0, 0.9))))
+        self.chain.addSegment(Segment(Joint(Joint.Fixed),
+                                      Frame(Vector(-0.4, 0.0, 0.0))))
         self.chain.addSegment(Segment(Joint(Joint.RotY),
-                             Frame(Vector(0.0,0.0,1.2))))
-        self.chain.addSegment(Segment(Joint(Joint.None),
-                                 Frame(Vector(0.4,0.0,0.0))))
+                                      Frame(Vector(0.0, 0.0, 1.2))))
+        self.chain.addSegment(Segment(Joint(Joint.Fixed),
+                                      Frame(Vector(0.4, 0.0, 0.0))))
         self.chain.addSegment(Segment(Joint(Joint.TransZ),
-                                 Frame(Vector(0.0,0.0,1.4))))
+                                      Frame(Vector(0.0, 0.0, 1.4))))
         self.chain.addSegment(Segment(Joint(Joint.TransX),
-                                 Frame(Vector(0.0,0.0,0.0))))
+                                      Frame(Vector(0.0, 0.0, 0.0))))
         self.chain.addSegment(Segment(Joint(Joint.TransY),
-                                 Frame(Vector(0.0,0.0,0.4))))
-        self.chain.addSegment(Segment(Joint(Joint.None),
-                                 Frame(Vector(0.0,0.0,0.0))))
+                                      Frame(Vector(0.0, 0.0, 0.4))))
+        self.chain.addSegment(Segment(Joint(Joint.Fixed),
+                                      Frame(Vector(0.0, 0.0, 0.0))))
 
-        self.jacsolver   = ChainJntToJacSolver(self.chain)
+        self.jacsolver = ChainJntToJacSolver(self.chain)
         self.fksolverpos = ChainFkSolverPos_recursive(self.chain)
         self.fksolvervel = ChainFkSolverVel_recursive(self.chain)
         self.iksolvervel = ChainIkSolverVel_pinv(self.chain)
-        self.iksolverpos = ChainIkSolverPos_NR(self.chain,self.fksolverpos,self.iksolvervel)
+        self.iksolverpos = ChainIkSolverPos_NR(self.chain, self.fksolverpos, self.iksolvervel)
+
+    def testRotationalInertia(self):
+        ri = RotationalInertia(1, 2, 3, 4, 7, 5)
+        # __getitem__
+        for i in range(3):
+            for j in range(3):
+                self.assertEqual(ri[3*i+j], 2*abs(i-j) + max(i, j) + 1)
+        with self.assertRaises(IndexError):
+            _ = ri[-1]
+        with self.assertRaises(IndexError):
+            _ = ri[9]
+
+        # __setitem__
+        for i in range(3):
+            for j in range(3):
+                ri[i] = i
+        for i in range(3):
+            for j in range(3):
+                self.assertEqual(ri[i], i)
+        with self.assertRaises(IndexError):
+            ri[-1] = 1
+        with self.assertRaises(IndexError):
+            ri[9] = 1
+
+    def testJacobian(self):
+        jac = Jacobian(3)
+        for i in range(jac.columns()):
+            jac.setColumn(i, Twist(Vector(6*i+1, 6*i+2, 6*i+3), Vector(6*i+4, 6*i+5, 6*i+6)))
+        # __getitem__
+        for i in range(6):
+            for j in range(3):
+                self.assertEqual(jac[i, j], 6*j+i+1)
+        with self.assertRaises(IndexError):
+            _ = jac[-1, 0]
+        with self.assertRaises(IndexError):
+            _ = jac[6, 0]
+        with self.assertRaises(IndexError):
+            _ = jac[5, -1]
+        with self.assertRaises(IndexError):
+            _ = jac[5, 3]
+
+        # __setitem__
+        for i in range(6):
+            for j in range(3):
+                jac[i, j] = 3*i+j
+        for i in range(6):
+            for j in range(3):
+                self.assertEqual(jac[i, j], 3*i+j)
+        with self.assertRaises(IndexError):
+            jac[-1, 0] = 1
+        with self.assertRaises(IndexError):
+            jac[6, 0] = 1
+        with self.assertRaises(IndexError):
+            jac[5, -1] = 1
+        with self.assertRaises(IndexError):
+            jac[5, 3] = 1
+
+    def testJntArray(self):
+        ja = JntArray(3)
+        # __getitem__
+        for i in range(3):
+            self.assertEqual(ja[i], 0)
+        with self.assertRaises(IndexError):
+            _ = ja[-1]
+        with self.assertRaises(IndexError):
+            _ = ja[3]
+
+        # __setitem__
+        for i in range(3):
+            ja[i] = 2*i
+        for i in range(3):
+            self.assertEqual(ja[i], 2*i)
+        with self.assertRaises(IndexError):
+            ja[-1] = 1
+        with self.assertRaises(IndexError):
+            ja[3] = 1
 
     def testFkPosAndJac(self):
         deltaq = 1E-4
         epsJ = 1E-4
 
-        F1=Frame()
-        F2=Frame()
+        F1 = Frame()
+        F2 = Frame()
+
+        q = JntArray(self.chain.getNrOfJoints())
+        jac = Jacobian(self.chain.getNrOfJoints())
 
-        q=JntArray(self.chain.getNrOfJoints())
-        jac=Jacobian(self.chain.getNrOfJoints())
-        
         for i in range(q.rows()):
-            q[i]=random.uniform(-3.14,3.14)
+            q[i] = random.uniform(-3.14, 3.14)
+
+        self.jacsolver.JntToJac(q, jac)
 
-        self.jacsolver.JntToJac(q,jac)
-        
         for i in range(q.rows()):
-            oldqi=q[i];
-            q[i]=oldqi+deltaq
-            self.assertTrue(0==self.fksolverpos.JntToCart(q,F2))
-            q[i]=oldqi-deltaq
-            self.assertTrue(0==self.fksolverpos.JntToCart(q,F1))
-            q[i]=oldqi
-            Jcol1 = diff(F1,F2,2*deltaq)
-            Jcol2 = Twist(Vector(jac[0,i],jac[1,i],jac[2,i]),
-                          Vector(jac[3,i],jac[4,i],jac[5,i]))
-            self.assertEqual(Jcol1,Jcol2);
+            oldqi = q[i]
+            q[i] = oldqi+deltaq
+            self.assertTrue(0 == self.fksolverpos.JntToCart(q, F2))
+            q[i] = oldqi-deltaq
+            self.assertTrue(0 == self.fksolverpos.JntToCart(q, F1))
+            q[i] = oldqi
+            Jcol1 = diff(F1, F2, 2*deltaq)
+            Jcol2 = Twist(Vector(jac[0, i], jac[1, i], jac[2, i]),
+                          Vector(jac[3, i], jac[4, i], jac[5, i]))
+            self.assertEqual(Jcol1, Jcol2)
 
     def testFkVelAndJac(self):
         deltaq = 1E-4
-        epsJ   = 1E-4
-    
-        q=JntArray(self.chain.getNrOfJoints())
-        qdot=JntArray(self.chain.getNrOfJoints())
+        epsJ = 1E-4
+
+        q = JntArray(self.chain.getNrOfJoints())
+        qdot = JntArray(self.chain.getNrOfJoints())
         for i in range(q.rows()):
-            q[i]=random.uniform(-3.14,3.14)
-            qdot[i]=random.uniform(-3.14,3.14)
+            q[i] = random.uniform(-3.14, 3.14)
+            qdot[i] = random.uniform(-3.14, 3.14)
 
-        qvel=JntArrayVel(q,qdot);
-        jac=Jacobian(self.chain.getNrOfJoints())
+        qvel = JntArrayVel(q, qdot)
+        jac = Jacobian(self.chain.getNrOfJoints())
 
-        cart = FrameVel.Identity();
-        t = Twist.Zero();
+        cart = FrameVel.Identity()
+        t = Twist.Zero()
 
-        self.jacsolver.JntToJac(qvel.q,jac)
-        self.assertTrue(self.fksolvervel.JntToCart(qvel,cart)==0)
-        MultiplyJacobian(jac,qvel.qdot,t)
-        self.assertEqual(cart.deriv(),t)
+        self.jacsolver.JntToJac(qvel.q, jac)
+        self.assertTrue(self.fksolvervel.JntToCart(qvel, cart) == 0)
+        MultiplyJacobian(jac, qvel.qdot, t)
+        self.assertEqual(cart.deriv(), t)
 
     def testFkVelAndIkVel(self):
         epsJ = 1E-7
 
-        q=JntArray(self.chain.getNrOfJoints())
-        qdot=JntArray(self.chain.getNrOfJoints())
+        q = JntArray(self.chain.getNrOfJoints())
+        qdot = JntArray(self.chain.getNrOfJoints())
         for i in range(q.rows()):
-            q[i]=random.uniform(-3.14,3.14)
-            qdot[i]=random.uniform(-3.14,3.14)
+            q[i] = random.uniform(-3.14, 3.14)
+            qdot[i] = random.uniform(-3.14, 3.14)
+
+        qvel = JntArrayVel(q, qdot)
+        qdot_solved = JntArray(self.chain.getNrOfJoints())
 
-        qvel=JntArrayVel(q,qdot)
-        qdot_solved=JntArray(self.chain.getNrOfJoints())
-        
         cart = FrameVel()
-        
-        self.assertTrue(0==self.fksolvervel.JntToCart(qvel,cart))
-        self.assertTrue(0==self.iksolvervel.CartToJnt(qvel.q,cart.deriv(),qdot_solved))
-        
-        self.assertEqual(qvel.qdot,qdot_solved);
-        
+
+        self.assertTrue(0 == self.fksolvervel.JntToCart(qvel, cart))
+        self.assertTrue(0 == self.iksolvervel.CartToJnt(qvel.q, cart.deriv(), qdot_solved))
+
+        self.assertTrue(Equal(qvel.qdot, qdot_solved, 1e-4))
 
     def testFkPosAndIkPos(self):
-        q=JntArray(self.chain.getNrOfJoints())
+        q = JntArray(self.chain.getNrOfJoints())
         for i in range(q.rows()):
-            q[i]=random.uniform(-3.14,3.14)
-        
-        q_init=JntArray(self.chain.getNrOfJoints())
+            q[i] = random.uniform(-3.14, 3.14)
+
+        q_init = JntArray(self.chain.getNrOfJoints())
         for i in range(q_init.rows()):
-            q_init[i]=q[i]+0.1*random.random()
-            
-        q_solved=JntArray(q.rows())
-
-        F1=Frame.Identity()
-        F2=Frame.Identity()
-    
-        self.assertTrue(0==self.fksolverpos.JntToCart(q,F1))
-        self.assertTrue(0==self.iksolverpos.CartToJnt(q_init,F1,q_solved))
-        self.assertTrue(0==self.fksolverpos.JntToCart(q_solved,F2))
-        
-        self.assertEqual(F1,F2)
-        self.assertEqual(q,q_solved)
-        
-        
+            q_init[i] = q[i]+0.1*random.random()
+
+        q_solved = JntArray(q.rows())
+
+        F1 = Frame.Identity()
+        F2 = Frame.Identity()
+
+        self.assertTrue(0 == self.fksolverpos.JntToCart(q, F1))
+        self.assertTrue(0 == self.iksolverpos.CartToJnt(q_init, F1, q_solved))
+        self.assertTrue(0 == self.fksolverpos.JntToCart(q_solved, F2))
+
+        self.assertEqual(F1, F2)
+        self.assertTrue(Equal(q, q_solved, 1e-4))
+
+
+class KinfamTestTree(unittest.TestCase):
+
+    def setUp(self):
+        self.tree = Tree()
+        self.tree.addSegment(Segment(Joint(Joint.RotZ),
+                                     Frame(Vector(0.0, 0.0, 0.0))), "foo")
+        self.tree.addSegment(Segment(Joint(Joint.Fixed),
+                                     Frame(Vector(0.0, 0.0, 0.0))), "bar")
+
+    def testTreeGetChainMemLeak(self):
+        # test for the memory leak in Tree.getChain described in issue #211
+        process = psutil.Process()
+        self.tree.getChain("foo", "bar")
+        gc.collect()
+        mem_before = process.memory_info().vms
+        # needs at least 2000 iterations on my system to cause a detectable
+        # difference in memory usage
+        for _ in range(10000):
+            self.tree.getChain("foo", "bar")
+        gc.collect()
+        mem_after = process.memory_info().vms
+        self.assertEqual(mem_before, mem_after)
+
+
 def suite():
     suite = unittest.TestSuite()
+    suite.addTest(KinfamTestFunctions('testRotationalInertia'))
+    suite.addTest(KinfamTestFunctions('testJacobian'))
+    suite.addTest(KinfamTestFunctions('testJntArray'))
     suite.addTest(KinfamTestFunctions('testFkPosAndJac'))
     suite.addTest(KinfamTestFunctions('testFkVelAndJac'))
     suite.addTest(KinfamTestFunctions('testFkVelAndIkVel'))
     suite.addTest(KinfamTestFunctions('testFkPosAndIkPos'))
     return suite
 
-#suite = suite()
-#unittest.TextTestRunner(verbosity=3).run(suite)
-            
+
+if __name__ == '__main__':
+    suite = suite()
+    result = unittest.TextTestRunner(verbosity=3).run(suite)
+
+    if result.wasSuccessful():
+        sys.exit(0)
+    else:
+        sys.exit(1)