Merge branch 'orocos:master' into pinv-velocity-solver-sigma-output

Author: craigirobot

.github/workflows/main.yml

@@ -1,6 +1,6 @@
 name: CI
 
-on: [push, pull_request]
+on: [push, pull_request, workflow_dispatch]
 
 env:
   CXXFLAGS: "-Wall -Wextra -Wno-unused-parameter"
@@ -12,28 +12,41 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        os: [ubuntu-20.04]
+        os: [ubuntu-20.04, ubuntu-22.04]
         orocos_build_type: [Debug, Release]
         compiler: [gcc, clang]
-        python_version: ['3.8']
+        python_version: ['3.8', '3.10']
+        exclude:
+          - os: ubuntu-20.04
+            python_version: '3.10'
+          - os: ubuntu-22.04
+            python_version: '3.8'
         include:
-          - os: ubuntu-18.04
+          - os: ubuntu-20.04
             orocos_build_type: Release
             compiler: gcc
-            python_version: '2'
+            python_version: '3.9'
           - os: ubuntu-20.04
             orocos_build_type: Release
             compiler: gcc
-            python_version: '3.9'
+            python_version: '3.10'
+          - os: ubuntu-20.04
+            orocos_build_type: Release
+            compiler: gcc
+            python_version: '3.11'
+          - os: ubuntu-22.04
+            orocos_build_type: Release
+            compiler: gcc
+            python_version: '3.11'
     env:
       CC: ${{ matrix.compiler }}
       OROCOS_BUILD_TYPE: ${{ matrix.orocos_build_type }}
       ROS_PYTHON_VERSION: ${{ matrix.python_version }}
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v4
         with:
           submodules: recursive
-      - uses: actions/setup-python@v2
+      - uses: actions/setup-python@v5
         with:
           python-version: ${{ matrix.python_version }}
       - name: Install
@@ -76,18 +89,6 @@ jobs:
       fail-fast: false
       matrix:
         include:
-          - env:
-              ROS_DISTRO: kinetic
-              ROS_REPO: ros
-              ABICHECK_URL: github:orocos/orocos_kinematics_dynamics#release-1.3
-              ABICHECK_MERGE: false
-            branch: release-1.3
-          - env:
-              ROS_DISTRO: melodic
-              ROS_REPO: ros
-              ABICHECK_URL: github:orocos/orocos_kinematics_dynamics#release-1.4
-              ABICHECK_MERGE: false
-            branch: release-1.4
           - env:
               ROS_DISTRO: noetic
               ROS_REPO: ros
@@ -96,7 +97,7 @@ jobs:
             branch: release-1.5
     env: ${{ matrix.env }}
     steps:
-      - uses: actions/checkout@v1
+      - uses: actions/checkout@v4
         with:
           submodules: recursive
         if: ${{ (github.event_name == 'push' && endsWith(github.ref, matrix.branch)) || (github.event_name == 'pull_request' && endsWith(github.base_ref, matrix.branch)) }}

README.md

@@ -13,3 +13,8 @@ The C++ library is located in the `orocos_kdl` folder. The installation instruct
 The python bindings, are located in the `python_orocos_kdl` folder. The installation instructions can be found in
 [INSTALL.md](python_orocos_kdl/INSTALL.md).
 
+Always use the same version of the C++ library and the python bindings. As a mismatch between these two can cause many issues.
+
+Also when using ROS/catkin, it is preferred to use the catkin installation method over the `cmake/make` method.
+
+There will be no ROS Noetic release.

orocos_kdl/CMakeLists.txt

@@ -1,11 +1,7 @@
 #
 # 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 )
+CMAKE_MINIMUM_REQUIRED(VERSION 3.0.2)
 
 
 ###################################################
@@ -58,7 +54,7 @@ if(NOT EIGEN3_FOUND)
   include(${PROJ_SOURCE_DIR}/cmake/FindEigen3.cmake)
 endif()
 include_directories(${EIGEN3_INCLUDE_DIR})
-SET(KDL_CFLAGS "${KDL_CFLAGS} -I${EIGEN3_INCLUDE_DIR}")
+SET(KDL_CFLAGS "${KDL_CFLAGS} -I\"${EIGEN3_INCLUDE_DIR}\"")
 
 # Check the platform STL containers capabilities
 include(cmake/CheckSTLContainers.cmake)

orocos_kdl/examples/CMakeLists.txt

@@ -8,7 +8,12 @@ IF(ENABLE_EXAMPLES)
   TARGET_LINK_LIBRARIES(trajectory_example orocos-kdl)
 
   add_executable(chainiksolverpos_lma_demo chainiksolverpos_lma_demo.cpp )
-  TARGET_LINK_LIBRARIES(chainiksolverpos_lma_demo orocos-kdl orocos-kdl-models)
+  find_package(Boost REQUIRED)
+  IF(${Boost_VERSION_MACRO} LESS 108300)
+    TARGET_LINK_LIBRARIES(chainiksolverpos_lma_demo orocos-kdl orocos-kdl-models)
+  ELSE()
+    TARGET_LINK_LIBRARIES(chainiksolverpos_lma_demo boost_timer orocos-kdl orocos-kdl-models)
+  ENDIF()
 
 ENDIF(ENABLE_EXAMPLES)  
 

orocos_kdl/examples/chainiksolverpos_lma_demo.cpp

@@ -56,15 +56,24 @@ estimate of shortest time per invposkin (ms) 0.155544
 #include <chainfksolverpos_recursive.hpp>
 #include <utilities/utility.h>
 
+#include <boost/version.hpp>
+#if BOOST_VERSION < 108300
 #include <boost/timer.hpp>
+#else
+#include <boost/timer/timer.hpp>
+#endif
 
 /**
  * tests the inverse kinematics on the given kinematic chain for a
  * large number of times and provides statistics  on the result.
  * \TODO provide other examples.
  */
 void test_inverseposkin(KDL::Chain& chain) {
+#if BOOST_VERSION < 108300
     boost::timer timer;
+#else
+    boost::timer::cpu_timer timer;
+#endif
     int num_of_trials = 1000000;
     int total_number_of_iter = 0;
     int n = chain.getNrOfJoints();
@@ -90,9 +99,9 @@ void test_inverseposkin(KDL::Chain& chain) {
     KDL::JntArray q_sol(n);
     for (int trial=0;trial<num_of_trials;++trial) {
         q.data.setRandom();
-        q.data *= PI;
+        q.data *= KDL::PI;
         q_init.data.setRandom();
-        q_init.data *= PI;
+        q_init.data *= KDL::PI;
         KDL::Frame pos_goal,pos_reached;
         fwdkin.JntToCart(q,pos_goal);
         //solver.compute_fwdpos(q.data);
@@ -159,7 +168,12 @@ void test_inverseposkin(KDL::Chain& chain) {
     std::cout << "max. trans. difference after solving " << max_trans_diff << std::endl;
     std::cout << "min. rot. difference after solving " << min_rot_diff << std::endl;
     std::cout << "max. rot. difference after solving " << max_rot_diff << std::endl;
+#if BOOST_VERSION < 108300
     double el = timer.elapsed();
+#else
+    boost::timer::cpu_times const ct(timer.elapsed());
+    double el = ct.user / 1e9;
+#endif
     std::cout << "elapsed time " << el << std::endl;
     std::cout << "estimate of average time per invposkin (ms)" << el/num_of_trials*1000 << std::endl;
     std::cout << "estimate of longest time per invposkin (ms) " << el/total_number_of_iter*max_num_of_iter *1000 << std::endl;
@@ -177,7 +191,7 @@ int main(int argc,char* argv[]) {
               <<  " This routine can take more then 6 minutes to execute. It then gives statistics on execution times\n"
               <<  " and failures.\n"
               <<  " Typically when executed 1 000 000 times, you will still see some small amount of failures\n"
-              <<  " Typically these failures are in the neighbourhoud of singularities.  Most failures of type -2 still\n"
+              <<  " Typically these failures are in the neighbourhood of singularities.  Most failures of type -2 still\n"
               <<  " reach an accuracy better than 1E-4.\n"
               <<  " This is much better than ChainIkSolverPos_NR, which fails a few times per 100 trials.\n";
 

orocos_kdl/models/kukaLWRtestDHnew.cpp

@@ -109,7 +109,7 @@ int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr)
    *READING Qdot = joint velocities
    */
   counter=0;//reset counter
-  ifstream inQdotfile("interpreteerbaar/qdot", std::ios::in);
+  std::ifstream inQdotfile("interpreteerbaar/qdot", std::ios::in);
 
   if (!inQdotfile)
   {

orocos_kdl/orocos_kdl-config.cmake.in

@@ -28,6 +28,7 @@ set(orocos_kdl_INCLUDE_DIRS
   ${EIGEN3_INCLUDE_DIR}
 )
 set(orocos_kdl_LIBRARIES orocos-kdl)
+set(orocos_kdl_TARGETS orocos-kdl)
 
 # where the .pc pkgconfig files are installed
 set(orocos_kdl_PKGCONFIG_DIR "${orocos_kdl_PREFIX}/lib/pkgconfig")

orocos_kdl/package.xml

@@ -26,6 +26,7 @@
 
   <export>
     <build_type>cmake</build_type>
+    <rosdoc config="rosdoc.yaml"/>
   </export>
 
 </package>

orocos_kdl/rosdoc.yaml

@@ -0,0 +1,5 @@
+- builder: doxygen
+  name: C++ API
+  file_patterns: '*.cpp *.cxx *.h *.hpp *.inl'
+  tab_size: 4
+  exclude_patterns: '*.svn* CMake*'

orocos_kdl/src/CMakeLists.txt

@@ -16,46 +16,44 @@ ENDIF(MSVC)
 CONFIGURE_FILE(config.h.in config.h @ONLY)
 
 #### Settings for rpath
-IF(${CMAKE_MINIMUM_REQUIRED_VERSION} VERSION_GREATER "2.8.12")
-    MESSAGE(AUTHOR_WARNING "CMAKE_MINIMUM_REQUIRED_VERSION is now ${CMAKE_MINIMUM_REQUIRED_VERSION}. This check can be removed.")
-ENDIF()
-IF(NOT (CMAKE_VERSION VERSION_LESS 2.8.12))
-    IF(NOT MSVC)
-        #add the option to disable RPATH
-        OPTION(OROCOSKDL_ENABLE_RPATH "Enable RPATH during installation" TRUE)
-        MARK_AS_ADVANCED(OROCOSKDL_ENABLE_RPATH)
-    ENDIF(NOT MSVC)
+IF(NOT MSVC)
+    #add the option to disable RPATH
+    OPTION(OROCOSKDL_ENABLE_RPATH "Enable RPATH during installation" TRUE)
+    MARK_AS_ADVANCED(OROCOSKDL_ENABLE_RPATH)
+ENDIF(NOT MSVC)
 
-    IF(OROCOSKDL_ENABLE_RPATH)
-        #Configure RPATH
-        SET(CMAKE_MACOSX_RPATH TRUE) #enable RPATH on OSX. This also suppress warnings on CMake >= 3.0
-        # when building, don't use the install RPATH already
-        # (but later on when installing)
-        SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
-        #build directory by default is built with RPATH
-        SET(CMAKE_SKIP_BUILD_RPATH  FALSE)
+IF(OROCOSKDL_ENABLE_RPATH)
+    #Configure RPATH
+    SET(CMAKE_MACOSX_RPATH TRUE) #enable RPATH on OSX. This also suppress warnings on CMake >= 3.0
+    # when building, don't use the install RPATH already
+    # (but later on when installing)
+    SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+    #build directory by default is built with RPATH
+    SET(CMAKE_SKIP_BUILD_RPATH  FALSE)
 
-        #This is relative RPATH for libraries built in the same project
-        #I assume that the directory is
-        # - install_dir/something for binaries
-        # - install_dir/lib for libraries
-        LIST(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir)
-        IF("${isSystemDir}" STREQUAL "-1")
-            FILE(RELATIVE_PATH _rel_path "${CMAKE_INSTALL_PREFIX}/bin" "${CMAKE_INSTALL_PREFIX}/lib")
-            IF (${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
-                SET(CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}")
-            ELSE()
-                SET(CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}")
-            ENDIF()
-        ENDIF("${isSystemDir}" STREQUAL "-1")
-        # add the automatically determined parts of the RPATH
-        # which point to directories outside the build tree to the install RPATH
-        SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) #very important!
-    ENDIF()
+    #This is relative RPATH for libraries built in the same project
+    #I assume that the directory is
+    # - install_dir/something for binaries
+    # - install_dir/lib for libraries
+    LIST(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir)
+    IF("${isSystemDir}" STREQUAL "-1")
+        FILE(RELATIVE_PATH _rel_path "${CMAKE_INSTALL_PREFIX}/bin" "${CMAKE_INSTALL_PREFIX}/lib")
+        IF (${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+            SET(CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}")
+        ELSE()
+            SET(CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}")
+        ENDIF()
+    ENDIF("${isSystemDir}" STREQUAL "-1")
+    # add the automatically determined parts of the RPATH
+    # which point to directories outside the build tree to the install RPATH
+    SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) #very important!
 ENDIF()
 #####end RPATH
 
-ADD_LIBRARY(orocos-kdl ${LIB_TYPE} ${KDL_SRCS} config.h)
+ADD_LIBRARY(orocos-kdl ${LIB_TYPE} ${KDL_SRCS})
+
+TARGET_INCLUDE_DIRECTORIES(orocos-kdl PUBLIC
+  "$<INSTALL_INTERFACE:include>")
 
 SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES
   SOVERSION "${KDL_VERSION_MAJOR}.${KDL_VERSION_MINOR}"
@@ -65,22 +63,15 @@ SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES
   )
 
 #### Settings for rpath disabled (back-compatibility)
-IF(${CMAKE_MINIMUM_REQUIRED_VERSION} VERSION_GREATER "2.8.12")
-    MESSAGE(AUTHOR_WARNING "CMAKE_MINIMUM_REQUIRED_VERSION is now ${CMAKE_MINIMUM_REQUIRED_VERSION}. This check can be removed.")
-ENDIF()
-IF(CMAKE_VERSION VERSION_LESS 2.8.12)
+IF(NOT OROCOSKDL_ENABLE_RPATH)
     SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES
       INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}")
-ELSE()
-    IF(NOT OROCOSKDL_ENABLE_RPATH)
-        SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES
-          INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}")
-    ENDIF()
 ENDIF()
 #####end RPATH
 
 # Needed so that the generated config.h can be used
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_BINARY_DIR})
+TARGET_INCLUDE_DIRECTORIES(orocos-kdl PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>")
 TARGET_LINK_LIBRARIES(orocos-kdl ${Boost_LIBRARIES})
 
 INSTALL(TARGETS orocos-kdl

orocos_kdl/src/chainexternalwrenchestimator.hpp

@@ -69,7 +69,7 @@ namespace KDL {
 
         /**
          * Calculates robot's initial momentum in the joint space. 
-         * Bassically, sets the offset for future estimation (momentum calculation).
+         * Basically, sets the offset for future estimation (momentum calculation).
          * If this method is not called by the user, zero values will be taken for the initial momentum.
          */
         int setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity);

orocos_kdl/src/chainhdsolver_vereshchagin.hpp

@@ -311,7 +311,7 @@ namespace KDL
  * simulate constraint forces in certain situations, however, there, final derivation of this principle in the software is different. However, in
  * the case of this more advanced forward dynamics computations, the user needs to be aware of prioritizations between input interfaces
  * (mentioned in "Prioritizations" section above) and internal policies on
- * handling singularities (mentioned in "Singularities and matrix inversions" section bellow).
+ * handling singularities (mentioned in "Singularities and matrix inversions" section below).
  *
  * ### Singularities and matrix inversions
  *

orocos_kdl/src/chainiksolverpos_lma.cpp

@@ -49,7 +49,7 @@ inline void Twist_to_Eigen(const KDL::Twist& t,Eigen::MatrixBase<Derived>& e) {
 
 ChainIkSolverPos_LMA::ChainIkSolverPos_LMA(
 		const KDL::Chain& _chain,
-		const Eigen::Matrix<double,6,1>& _L,
+		const Eigen::Matrix<double,6,1>& _l,
 		double _eps,
 		int _maxiter,
 		double _eps_joints
@@ -68,7 +68,7 @@ ChainIkSolverPos_LMA::ChainIkSolverPos_LMA(
 	maxiter(_maxiter),
 	eps(_eps),
 	eps_joints(_eps_joints),
-	L(_L.cast<ScalarType>()),
+	L(_l.cast<ScalarType>()),
 	T_base_jointroot(nj),
 	T_base_jointtip(nj),
 	q(nj),

orocos_kdl/src/chainiksolverpos_lma.hpp

@@ -83,7 +83,7 @@ class ChainIkSolverPos_LMA : public KDL::ChainIkSolverPos
 	 * \f$ E = \Delta \mathbf{x}^T \mathbf{L} \mathbf{L}^T \Delta \mathbf{x} \f$, with \f$\mathbf{L}\f$ a diagonal matrix.
 	 *
 	 * \param _chain specifies the kinematic chain.
-	 * \param _L specifies the "square root" of the weight (diagonal) matrix in task space. This diagonal matrix is specified as a vector.
+	 * \param _l specifies the "square root" of the weight (diagonal) matrix in task space. This diagonal matrix is specified as a vector.
 	 * \param _eps specifies the desired accuracy in task space; <B>after</B> weighing with
 	 *        the weight matrix, it is applied on \f$E\f$.
 	 * \param _maxiter specifies the maximum number of iterations.
@@ -94,7 +94,7 @@ class ChainIkSolverPos_LMA : public KDL::ChainIkSolverPos
      */
     ChainIkSolverPos_LMA(
     		const KDL::Chain& _chain,
-    		const Eigen::Matrix<double,6,1>& _L,
+		const Eigen::Matrix<double,6,1>& _l,
     		double _eps=1E-5,
     		int _maxiter=500,
     		double _eps_joints=1E-15

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 (error = E_NOT_IMPLEMENTED);};
 
         /**
          * Set eps