PyKDL: PyBind11 based

.gitmodules

@@ -0,0 +1,4 @@
+[submodule "python_orocos_kdl/pybind11"]
+    path = python_orocos_kdl/pybind11
+    url = https://github.com/pybind/pybind11.git
+    branch = v2.4.3

.travis.yml

@@ -1,16 +1,21 @@
 language: cpp
+os: linux
+dist: xenial
 
 compiler:
   - gcc
   - clang
+
 env:
-  - OROCOS_KDL_BUILD_TYPE=Debug
-  - OROCOS_KDL_BUILD_TYPE=Release
+  - OROCOS_KDL_BUILD_TYPE=Debug BUILD_PYKDL_PYBIND11=OFF
+  - OROCOS_KDL_BUILD_TYPE=Debug BUILD_PYKDL_PYBIND11=ON
+  - OROCOS_KDL_BUILD_TYPE=Release BUILD_PYKDL_PYBIND11=OFF
+  - OROCOS_KDL_BUILD_TYPE=Release BUILD_PYKDL_PYBIND11=ON
 
 before_script:
   - sudo apt-get -qq update
-  - sudo apt-get install -y libeigen3-dev libcppunit-dev python-sip-dev python-psutil
-  #build orocos_kdl
+  - sudo apt-get install -y libeigen3-dev libcppunit-dev python-sip-dev python3-sip-dev python-psutil python3-psutil python-future python3-future
+  # build orocos_kdl
   - cd orocos_kdl
   - mkdir build
   - cd build
@@ -19,23 +24,41 @@ before_script:
   - make
   - sudo make install
   - cd ../..
-  #build python bindings
+
+  # build python bindings python 2
   - cd python_orocos_kdl
-  - mkdir build
-  - cd build
-  - cmake -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} ./..
-  #compile and install python bindings
+  - mkdir build2
+  - cd build2
+  - export ROS_PYTHON_VERSION=2
+  - cmake -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} -DBUILD_PYKDL_PYBIND11=${BUILD_PYKDL_PYBIND11} ./..
+  # compile and install python bindings python 2
+  - make
+  - sudo make install
+  - export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib
+  - sudo ldconfig
+  - cd ../..
+
+  # build python bindings python 3
+  - cd python_orocos_kdl
+  - mkdir build3
+  - cd build3
+  - export ROS_PYTHON_VERSION=3
+  - cmake -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} -DBUILD_PYKDL_PYBIND11=${BUILD_PYKDL_PYBIND11} ./..
+  # compile and install python bindings python 3
   - make
   - sudo make install
   - export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib
   - sudo ldconfig
   - cd ../..
 
 script:
-  #execute orocos_kdl tests
+  # execute orocos_kdl tests
   - cd orocos_kdl/build
   - make check
   - cd ../..
-  #execute python bindings tests
-  - cd python_orocos_kdl/build
-  - python ../tests/PyKDLtest.py
+  # execute python bindings tests
+  - cd python_orocos_kdl
+  # python 2
+  - python2 tests/PyKDLtest.py
+  # python 3
+  - python3 tests/PyKDLtest.py

orocos_kdl/CMakeLists.txt

@@ -47,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
@@ -67,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
@@ -75,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.

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>

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

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

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,22 +82,22 @@ 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)+=chain.getSegment(i).getJoint().getInertia();  // add joint inertia
@@ -107,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)
 	  }
 
@@ -128,18 +129,18 @@ 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
     int ChainDynParam::JntToGravity(const JntArray &q,JntArray &gravity)
     {
 
 	//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);