From 2ba6ab7e8832cc1532c7d1988140799314cb4253 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 5 Oct 2021 08:39:29 +0200 Subject: [PATCH 01/78] Bump python_orocos_kdl/pybind11 from `787d2c8` to `e315e1f` (#361) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `787d2c8` to `e315e1f`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/787d2c88cafa4d07fb38c9519c485a86323cfcf4...e315e1fe2bad553ff541ce3bc00804e0e70583a2) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index 787d2c88..e315e1fe 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit 787d2c88cafa4d07fb38c9519c485a86323cfcf4 +Subproject commit e315e1fe2bad553ff541ce3bc00804e0e70583a2 From 88d5c8fb189c6cd0f0b2c4c1945ed2b0e315e203 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 20 Oct 2021 11:45:07 +0200 Subject: [PATCH 02/78] (README) use same version and prefer catkin --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 3de6b187..2a4aabd1 100644 --- a/README.md +++ b/README.md @@ -13,3 +13,6 @@ 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. From ef39a4fd5cfb1400b2e6e034b1a99b8ad91192cf Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Fri, 1 May 2020 10:20:08 -0700 Subject: [PATCH 03/78] export modern CMake interface target * export modern CMake interface target * remove redundant include_directories Signed-off-by: Dirk Thomas Original pull request on ROS 2 fork: https://github.com/ros2/orocos_kinematics_dynamics/pull/14 Signed-off-by: Jacob Perron --- orocos_kdl/orocos_kdl-config.cmake.in | 1 + orocos_kdl/src/CMakeLists.txt | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/orocos_kdl/orocos_kdl-config.cmake.in b/orocos_kdl/orocos_kdl-config.cmake.in index 4cc8bc59..d17d4334 100644 --- a/orocos_kdl/orocos_kdl-config.cmake.in +++ b/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") diff --git a/orocos_kdl/src/CMakeLists.txt b/orocos_kdl/src/CMakeLists.txt index ca63079c..2b1e40a3 100644 --- a/orocos_kdl/src/CMakeLists.txt +++ b/orocos_kdl/src/CMakeLists.txt @@ -57,6 +57,9 @@ ENDIF() ADD_LIBRARY(orocos-kdl ${LIB_TYPE} ${KDL_SRCS} config.h) +TARGET_INCLUDE_DIRECTORIES(orocos-kdl PUBLIC + "$") + SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES SOVERSION "${KDL_VERSION_MAJOR}.${KDL_VERSION_MINOR}" VERSION "${KDL_VERSION}" @@ -80,7 +83,8 @@ 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 + "$") TARGET_LINK_LIBRARIES(orocos-kdl ${Boost_LIBRARIES}) INSTALL(TARGETS orocos-kdl From 1c16c9cc8a56c61659e99c5ac101ea51b9861cf7 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 28 Oct 2021 00:51:31 +0200 Subject: [PATCH 04/78] Bump python_orocos_kdl/pybind11 from `e315e1f` to `acae930` (#368) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `e315e1f` to `acae930`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/e315e1fe2bad553ff541ce3bc00804e0e70583a2...acae930123bcd331aff73a30e4fb7e2103fd7fca) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index e315e1fe..acae9301 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit e315e1fe2bad553ff541ce3bc00804e0e70583a2 +Subproject commit acae930123bcd331aff73a30e4fb7e2103fd7fca From b3415d55f4eb48455dff5e033f1169f4f5ef5264 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Fri, 8 Oct 2021 21:22:58 +0200 Subject: [PATCH 05/78] python_orocos_kdl: fix Python bindings for static member functions Frame::DH() and Frame::DH_Craig1989() --- python_orocos_kdl/PyKDL/frames.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/PyKDL/frames.cpp b/python_orocos_kdl/PyKDL/frames.cpp index a6852caa..5aa9d1f5 100644 --- a/python_orocos_kdl/PyKDL/frames.cpp +++ b/python_orocos_kdl/PyKDL/frames.cpp @@ -436,8 +436,8 @@ void init_frames(py::module &m) { return Frame(self); }, py::arg("memo")); - frame.def("DH_Craig1989", &Frame::DH_Craig1989); - frame.def("DH", &Frame::DH); + frame.def_static("DH_Craig1989", &Frame::DH_Craig1989); + frame.def_static("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); From c61b2ae214a866b950d0be7591c877adc9b31d4d Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Wed, 13 Oct 2021 14:47:34 +0200 Subject: [PATCH 06/78] Add unit tests for Frame initialization from Denavit-Hartenberg parameters --- orocos_kdl/tests/CMakeLists.txt | 20 ++++++++++---------- orocos_kdl/tests/framestest.cpp | 14 ++++++++++++++ python_orocos_kdl/tests/framestest.py | 17 ++++++++++++++++- 3 files changed, 40 insertions(+), 11 deletions(-) diff --git a/orocos_kdl/tests/CMakeLists.txt b/orocos_kdl/tests/CMakeLists.txt index 80cd20ba..ed045b2f 100644 --- a/orocos_kdl/tests/CMakeLists.txt +++ b/orocos_kdl/tests/CMakeLists.txt @@ -8,64 +8,64 @@ IF(ENABLE_TESTS) SET(TESTNAME "framestest") SET_TARGET_PROPERTIES( framestest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") - ADD_TEST(framestest framestest) + ADD_TEST(NAME framestest COMMAND framestest) ADD_EXECUTABLE(kinfamtest kinfamtest.cpp test-runner.cpp) TARGET_LINK_LIBRARIES(kinfamtest orocos-kdl ${CPPUNIT}) SET(TESTNAME "kinfamtest") SET_TARGET_PROPERTIES( kinfamtest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS}") - ADD_TEST(kinfamtest kinfamtest) + ADD_TEST(NAME kinfamtest COMMAND kinfamtest) ADD_EXECUTABLE(solvertest solvertest.cpp test-runner.cpp) TARGET_LINK_LIBRARIES(solvertest orocos-kdl ${CPPUNIT}) SET(TESTNAME "solvertest") SET_TARGET_PROPERTIES( solvertest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") - ADD_TEST(solvertest solvertest) + ADD_TEST(NAME solvertest COMMAND solvertest) ADD_EXECUTABLE(inertiatest inertiatest.cpp test-runner.cpp) TARGET_LINK_LIBRARIES(inertiatest orocos-kdl ${CPPUNIT}) SET(TESTNAME "inertiatest") SET_TARGET_PROPERTIES( inertiatest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") - ADD_TEST(inertiatest inertiatest) + ADD_TEST(NAME inertiatest COMMAND inertiatest) ADD_EXECUTABLE(jacobiantest jacobiantest.cpp test-runner.cpp) SET(TESTNAME "jacobiantest") TARGET_LINK_LIBRARIES(jacobiantest orocos-kdl ${CPPUNIT}) SET_TARGET_PROPERTIES( jacobiantest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") - ADD_TEST(jacobiantest jacobiantest) + ADD_TEST(NAME jacobiantest COMMAND jacobiantest) ADD_EXECUTABLE(jacobiandottest jacobiandottest.cpp test-runner.cpp) SET(TESTNAME "jacobiandottest") TARGET_LINK_LIBRARIES(jacobiandottest orocos-kdl ${CPPUNIT}) SET_TARGET_PROPERTIES( jacobiandottest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") - ADD_TEST(jacobiandottest jacobiandottest) + ADD_TEST(NAME jacobiandottest COMMAND jacobiandottest) ADD_EXECUTABLE(velocityprofiletest velocityprofiletest.cpp test-runner.cpp) SET(TESTNAME "velocityprofiletest") TARGET_LINK_LIBRARIES(velocityprofiletest orocos-kdl ${CPPUNIT}) SET_TARGET_PROPERTIES( velocityprofiletest PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") - ADD_TEST(velocityprofiletest velocityprofiletest) + ADD_TEST(NAME velocityprofiletest COMMAND 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_TEST(NAME treeinvdyntest COMMAND treeinvdyntest) # ADD_EXECUTABLE(rframestest rframestest.cpp) # TARGET_LINK_LIBRARIES(rframestest orocos-kdl) -# ADD_TEST(rframestest rframestest) +# ADD_TEST(NAME rframestest COMMAND rframestest) # # ADD_EXECUTABLE(rallnumbertest rallnumbertest.cpp) # TARGET_LINK_LIBRARIES(rallnumbertest orocos-kdl) -# ADD_TEST(rallnumbertest rallnumbertest) +# ADD_TEST(NAME rallnumbertest COMMAND rallnumbertest) ADD_CUSTOM_TARGET(check ctest -V WORKING_DIRECTORY ${PROJ_BINARY_DIR}/tests) ENDIF(ENABLE_TESTS) diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index 6b1e6235..be347b52 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -633,6 +633,20 @@ void FramesTest::TestFrame() { CPPUNIT_ASSERT_EQUAL(F*F.Inverse(),Frame::Identity()); CPPUNIT_ASSERT_EQUAL(F.Inverse()*F,Frame::Identity()); CPPUNIT_ASSERT_EQUAL(F.Inverse()*v,F.Inverse(v)); + + // Denavit-Hartenberg + CPPUNIT_ASSERT(Equal( + Frame::DH(0.0, M_PI_2, 0.36, 0.0), + Frame(Rotation(1, 0, 0, + 0, 0, -1, + 0, 1, 0), + Vector(0, 0, 0.36)))); + CPPUNIT_ASSERT(Equal( + Frame::DH_Craig1989(0.0, M_PI_2, 0.36, 0.0), + Frame(Rotation(1, 0, 0, + 0, 0, -1, + 0, 1, 0), + Vector(0, -0.36, 0)))); } JntArray CreateRandomJntArray(int size) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index eeed0531..9f00a429 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -22,7 +22,7 @@ # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -from math import radians, sqrt +from math import pi, radians, sqrt from PyKDL import * import sys import unittest @@ -346,6 +346,21 @@ def testFrame(self): self.assertEqual(Frame(f).M, f.M) self.assertEqual(Frame(f).p, f.p) + # Denavit-Hartenberg + f = Frame.DH(0.0, pi/2, 0.36, 0.0) + self.assertTrue(Equal( + Frame.DH(0.0, pi/2, 0.36, 0.0), + Frame(Rotation(1, 0, 0, + 0, 0, -1, + 0, 1, 0), + Vector(0, 0, 0.36)))) + self.assertTrue(Equal( + Frame.DH_Craig1989(0.0, pi/2, 0.36, 0.0), + Frame(Rotation(1, 0, 0, + 0, 0, -1, + 0, 1, 0), + Vector(0, -0.36, 0)))) + f = Frame(Rotation(1, 2, 3, 5, 6, 7, 9, 10, 11), From 0d3d96e14d31c7c77b4e7c93a803a0a8ce77838b Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 29 Oct 2021 11:22:14 +0200 Subject: [PATCH 07/78] (KDL) test DH both on class and instance --- orocos_kdl/tests/framestest.cpp | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index be347b52..c6bae97d 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -635,18 +635,19 @@ void FramesTest::TestFrame() { CPPUNIT_ASSERT_EQUAL(F.Inverse()*v,F.Inverse(v)); // Denavit-Hartenberg - CPPUNIT_ASSERT(Equal( - Frame::DH(0.0, M_PI_2, 0.36, 0.0), - Frame(Rotation(1, 0, 0, - 0, 0, -1, - 0, 1, 0), - Vector(0, 0, 0.36)))); - CPPUNIT_ASSERT(Equal( - Frame::DH_Craig1989(0.0, M_PI_2, 0.36, 0.0), - Frame(Rotation(1, 0, 0, - 0, 0, -1, - 0, 1, 0), - Vector(0, -0.36, 0)))); + Frame F_DH = Frame(Rotation(1, 0, 0, + 0, 0, -1, + 0, 1, 0), + Vector(0, 0, 0.36)); + CPPUNIT_ASSERT(Equal(Frame::DH(0.0, M_PI_2, 0.36, 0.0), F_DH)); + CPPUNIT_ASSERT(Equal(Frame().DH(0.0, M_PI_2, 0.36, 0.0), F_DH)); + + Frame F_DH_Craig1989 = Frame(Rotation(1, 0, 0, + 0, 0, -1, + 0, 1, 0), + Vector(0, -0.36, 0)); + CPPUNIT_ASSERT(Equal(Frame::DH_Craig1989(0.0, M_PI_2, 0.36, 0.0), F_DH_Craig1989)); + CPPUNIT_ASSERT(Equal(Frame().DH_Craig1989(0.0, M_PI_2, 0.36, 0.0), F_DH_Craig1989)); } JntArray CreateRandomJntArray(int size) From 392e23bdbcb004c4c175fead77fe487f89469de4 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 29 Oct 2021 11:29:03 +0200 Subject: [PATCH 08/78] (PyKDL) test DH both on class and instance --- python_orocos_kdl/tests/framestest.py | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index 9f00a429..ec581e24 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -347,19 +347,19 @@ def testFrame(self): self.assertEqual(Frame(f).p, f.p) # Denavit-Hartenberg - f = Frame.DH(0.0, pi/2, 0.36, 0.0) - self.assertTrue(Equal( - Frame.DH(0.0, pi/2, 0.36, 0.0), - Frame(Rotation(1, 0, 0, - 0, 0, -1, - 0, 1, 0), - Vector(0, 0, 0.36)))) - self.assertTrue(Equal( - Frame.DH_Craig1989(0.0, pi/2, 0.36, 0.0), - Frame(Rotation(1, 0, 0, - 0, 0, -1, - 0, 1, 0), - Vector(0, -0.36, 0)))) + f_dh = Frame(Rotation(1, 0, 0, + 0, 0, -1, + 0, 1, 0), + Vector(0, 0, 0.36)) + self.assertTrue(Equal(Frame.DH(0.0, pi/2, 0.36, 0.0), f_dh)) + self.assertTrue(Equal(Frame().DH(0.0, pi/2, 0.36, 0.0), f_dh)) + + f_dh_craig1989 = Frame(Rotation(1, 0, 0, + 0, 0, -1, + 0, 1, 0), + Vector(0, -0.36, 0)) + self.assertTrue(Equal(Frame.DH_Craig1989(0.0, pi/2, 0.36, 0.0), f_dh_craig1989)) + self.assertTrue(Equal(Frame().DH_Craig1989(0.0, pi / 2, 0.36, 0.0), f_dh_craig1989)) f = Frame(Rotation(1, 2, 3, 5, 6, 7, From 4c9768bc056f57c8641aefb425d08ba3c36deb41 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 29 Oct 2021 16:41:47 +0200 Subject: [PATCH 09/78] Add commments about static function calls on instances --- orocos_kdl/tests/framestest.cpp | 4 ++-- python_orocos_kdl/tests/framestest.py | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index c6bae97d..d217ba5a 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -640,14 +640,14 @@ void FramesTest::TestFrame() { 0, 1, 0), Vector(0, 0, 0.36)); CPPUNIT_ASSERT(Equal(Frame::DH(0.0, M_PI_2, 0.36, 0.0), F_DH)); - CPPUNIT_ASSERT(Equal(Frame().DH(0.0, M_PI_2, 0.36, 0.0), F_DH)); + CPPUNIT_ASSERT(Equal(Frame().DH(0.0, M_PI_2, 0.36, 0.0), F_DH)); // Only for testing purposes, shouldn't use static function of instances Frame F_DH_Craig1989 = Frame(Rotation(1, 0, 0, 0, 0, -1, 0, 1, 0), Vector(0, -0.36, 0)); CPPUNIT_ASSERT(Equal(Frame::DH_Craig1989(0.0, M_PI_2, 0.36, 0.0), F_DH_Craig1989)); - CPPUNIT_ASSERT(Equal(Frame().DH_Craig1989(0.0, M_PI_2, 0.36, 0.0), F_DH_Craig1989)); + CPPUNIT_ASSERT(Equal(Frame().DH_Craig1989(0.0, M_PI_2, 0.36, 0.0), F_DH_Craig1989)); // Only for testing purposes, shouldn't use static function of instances } JntArray CreateRandomJntArray(int size) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index ec581e24..0e277e18 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -352,6 +352,7 @@ def testFrame(self): 0, 1, 0), Vector(0, 0, 0.36)) self.assertTrue(Equal(Frame.DH(0.0, pi/2, 0.36, 0.0), f_dh)) + # Only for testing purposes, shouldn't use static function of instances self.assertTrue(Equal(Frame().DH(0.0, pi/2, 0.36, 0.0), f_dh)) f_dh_craig1989 = Frame(Rotation(1, 0, 0, @@ -359,6 +360,7 @@ def testFrame(self): 0, 1, 0), Vector(0, -0.36, 0)) self.assertTrue(Equal(Frame.DH_Craig1989(0.0, pi/2, 0.36, 0.0), f_dh_craig1989)) + # Only for testing purposes, shouldn't use static function of instances self.assertTrue(Equal(Frame().DH_Craig1989(0.0, pi / 2, 0.36, 0.0), f_dh_craig1989)) f = Frame(Rotation(1, 2, 3, From ef1aef51039dfac2b5bec9337b10e550384e77ab Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 29 Oct 2021 17:42:53 +0200 Subject: [PATCH 10/78] (kdl) Use minimal cmake 3.0.2 --- orocos_kdl/CMakeLists.txt | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt index a15bd15d..a1b335cd 100644 --- a/orocos_kdl/CMakeLists.txt +++ b/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) ################################################### From cd1d7e83ef60a5a89727ca3edbb71e111d729529 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 30 Oct 2021 11:15:22 +0200 Subject: [PATCH 11/78] (PyKDL) disable function with vector by reference arg This doesn't work in PyBind11 --- python_orocos_kdl/PyKDL/kinfam.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/PyKDL/kinfam.cpp b/python_orocos_kdl/PyKDL/kinfam.cpp index d489aabd..d899a803 100644 --- a/python_orocos_kdl/PyKDL/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/kinfam.cpp @@ -338,8 +338,9 @@ void init_kinfam(pybind11::module &m) py::class_ 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&, int)) &ChainFkSolverPos::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_pos.def("JntToCart", (int (ChainFkSolverPos::*)(const JntArray&, std::vector&, int)) &ChainFkSolverPos::JntToCart, +// py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); // -------------------- From 7d23d7341fc8aaf9aa0ccda8c1f44a6c06a97ae7 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 30 Oct 2021 12:38:38 +0200 Subject: [PATCH 12/78] (KDL) return correct not_implemented_error --- orocos_kdl/src/chainiksolvervel_pinv.hpp | 2 +- orocos_kdl/src/chainiksolvervel_pinv_nso.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/orocos_kdl/src/chainiksolvervel_pinv.hpp b/orocos_kdl/src/chainiksolvervel_pinv.hpp index b31f2dd4..63a6f9c4 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 (error = E_NOT_IMPLEMENTED);}; /** * Retrieve the number of singular values of the jacobian that are < eps; diff --git a/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp b/orocos_kdl/src/chainiksolvervel_pinv_nso.hpp index 0cbe1ff6..97cd570b 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 (error = E_NOT_IMPLEMENTED);}; /** * Request the joint weights for optimization criterion From 65bda929d762784d1ef33a07730417ec15cc7dfe Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 30 Oct 2021 12:39:04 +0200 Subject: [PATCH 13/78] (PyKDL) correct comment, not implemented in cpp --- python_orocos_kdl/PyKDL/kinfam.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/PyKDL/kinfam.cpp b/python_orocos_kdl/PyKDL/kinfam.cpp index d899a803..d73193e9 100644 --- a/python_orocos_kdl/PyKDL/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/kinfam.cpp @@ -382,7 +382,7 @@ void init_kinfam(pybind11::module &m) py::class_ 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 +// Not yet implemented in orocos_kdl // 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")); From 23473d27ac3f2402974b08194bf49593952db49b Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 30 Oct 2021 12:44:03 +0200 Subject: [PATCH 14/78] (Actions) test with python 3.10 --- .github/workflows/main.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 97a79b8e..57064626 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -25,6 +25,10 @@ jobs: orocos_build_type: Release compiler: gcc python_version: '3.9' + - os: ubuntu-20.04 + orocos_build_type: Release + compiler: gcc + python_version: '3.10' env: CC: ${{ matrix.compiler }} OROCOS_BUILD_TYPE: ${{ matrix.orocos_build_type }} From 23846ae69bf6ccc8646f0062b52b432634e58e31 Mon Sep 17 00:00:00 2001 From: xtkoba <69125751+xtkoba@users.noreply.github.com> Date: Fri, 5 Nov 2021 01:48:54 +0900 Subject: [PATCH 15/78] Follow-up of "Get rid of using std namespace" --- orocos_kdl/models/kukaLWRtestDHnew.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/models/kukaLWRtestDHnew.cpp b/orocos_kdl/models/kukaLWRtestDHnew.cpp index b9365777..f0dcedbc 100644 --- a/orocos_kdl/models/kukaLWRtestDHnew.cpp +++ b/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) { From 77b42d00dcdc8446bf39811b924a5dc7e1fe463d Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 28 Dec 2021 22:03:51 +0000 Subject: [PATCH 16/78] Bump python_orocos_kdl/pybind11 from `acae930` to `9b4f71d` Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `acae930` to `9b4f71d`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/acae930123bcd331aff73a30e4fb7e2103fd7fca...9b4f71d12de4f910a77aeb00d37ffb2220e645e7) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index acae9301..9b4f71d1 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit acae930123bcd331aff73a30e4fb7e2103fd7fca +Subproject commit 9b4f71d12de4f910a77aeb00d37ffb2220e645e7 From e25a13fc5820dbca6b23d10506407bca9bcdd25f Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 7 Jan 2022 10:48:17 -0800 Subject: [PATCH 17/78] Skip building pybind11 if already available on system (#375) * Skip building pybind11 if already available on system Signed-off-by: Jacob Perron * Update message Co-authored-by: Matthijs van der Burgh Co-authored-by: Matthijs van der Burgh --- python_orocos_kdl/CMakeLists.txt | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index 09897cb3..d29319fc 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -46,7 +46,13 @@ else() set(PYTHON_MODULE_EXTENSION ".so") endif() -add_subdirectory(pybind11) +find_package(pybind11 QUIET) +if(NOT ${pybind11_FOUND}) + message(STATUS "pybind11 not found, building from source") + add_subdirectory(pybind11) +else() + message(STATUS "pybind11 found, building against installed version") +endif() pybind11_add_module(${LIBRARY_NAME} PyKDL/PyKDL.h PyKDL/PyKDL.cpp From e482f0469fdf5a5a74552fa555755181dedcc47d Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 25 Jan 2022 12:21:09 +0100 Subject: [PATCH 18/78] Python3 is the default --- python_orocos_kdl/INSTALL.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/INSTALL.md b/python_orocos_kdl/INSTALL.md index c3f2d5de..f95f10ac 100644 --- a/python_orocos_kdl/INSTALL.md +++ b/python_orocos_kdl/INSTALL.md @@ -33,7 +33,7 @@ These install instructions are focused on Debian/Ubuntu systems. 6. Execute cmake: `cmake ..` - (Optional) Adapt `CMAKE_INSTALL_PREFIX` to the desired installation directory - (Optional) To change the build type, add: `-DCMAKE_BUILD_TYPE=` - - (Optional) To change the python version (default=2), set `ROS_PYTHON_VERSION` environment variable to either `2` or `3`. + - (Optional) To change the python version (default=3), set `ROS_PYTHON_VERSION` environment variable to either `2` or `3`. 7. Compile: `make` 8. Install the python bindings: `sudo make install` 9. Make sure `LD_LIBRARY_PATH` is set correctly: `export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib`. Add this also From 4f705fe9e1533802578b5275ddad02b065b2be4e Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Fri, 4 Feb 2022 01:38:22 +0100 Subject: [PATCH 19/78] Bump python_orocos_kdl/pybind11 from `9b4f71d` to `ffa3468` (#380) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `9b4f71d` to `ffa3468`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/9b4f71d12de4f910a77aeb00d37ffb2220e645e7...ffa346860b306c9bbfb341aed9c14c067751feb8) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index 9b4f71d1..ffa34686 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit 9b4f71d12de4f910a77aeb00d37ffb2220e645e7 +Subproject commit ffa346860b306c9bbfb341aed9c14c067751feb8 From d7d5c8d5098cf3b3ac848ea214d8bbdb088dfd17 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 24 Feb 2022 11:58:54 -0800 Subject: [PATCH 20/78] Remove CMake warning related to minimum version Since the minimum required version of cmake is now 3.0.2, we now see these warnings to remove conditions. This commit removes the warnings and conditions that are no longer needed. Signed-off-by: Jacob Perron --- orocos_kdl/src/CMakeLists.txt | 73 ++++++++++++++--------------------- 1 file changed, 30 insertions(+), 43 deletions(-) diff --git a/orocos_kdl/src/CMakeLists.txt b/orocos_kdl/src/CMakeLists.txt index 2b1e40a3..d1e0b615 100644 --- a/orocos_kdl/src/CMakeLists.txt +++ b/orocos_kdl/src/CMakeLists.txt @@ -16,42 +16,37 @@ 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 @@ -68,17 +63,9 @@ 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 From 7c887f11877dc7004b7f7abf772a9f1b8114ec29 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 1 Mar 2022 08:01:45 +0100 Subject: [PATCH 21/78] Bump actions/setup-python from 2 to 3 (#386) Bumps [actions/setup-python](https://github.com/actions/setup-python) from 2 to 3. - [Release notes](https://github.com/actions/setup-python/releases) - [Commits](https://github.com/actions/setup-python/compare/v2...v3) --- updated-dependencies: - dependency-name: actions/setup-python dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 57064626..8a58fd54 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -37,7 +37,7 @@ jobs: - uses: actions/checkout@v2 with: submodules: recursive - - uses: actions/setup-python@v2 + - uses: actions/setup-python@v3 with: python-version: ${{ matrix.python_version }} - name: Install From 11d8bd62128092ff64ab20e419b295ca72aada32 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 2 Mar 2022 09:07:14 +0100 Subject: [PATCH 22/78] Bump actions/checkout from 2 to 3 (#387) Bumps [actions/checkout](https://github.com/actions/checkout) from 2 to 3. - [Release notes](https://github.com/actions/checkout/releases) - [Changelog](https://github.com/actions/checkout/blob/main/CHANGELOG.md) - [Commits](https://github.com/actions/checkout/compare/v2...v3) --- updated-dependencies: - dependency-name: actions/checkout dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/main.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 8a58fd54..ac85a647 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -34,7 +34,7 @@ jobs: OROCOS_BUILD_TYPE: ${{ matrix.orocos_build_type }} ROS_PYTHON_VERSION: ${{ matrix.python_version }} steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: recursive - uses: actions/setup-python@v3 @@ -100,7 +100,7 @@ jobs: branch: release-1.5 env: ${{ matrix.env }} steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v3 with: submodules: recursive if: ${{ (github.event_name == 'push' && endsWith(github.ref, matrix.branch)) || (github.event_name == 'pull_request' && endsWith(github.base_ref, matrix.branch)) }} From 3b4d0cfd1feef7417e089e35b6ceb6133bd4d036 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 31 Mar 2022 18:31:59 -0700 Subject: [PATCH 23/78] Fix implicit conversion warnings These were caught as MSBuild warnings on Windows. In most cases, acknowledge that the conversions may lose precision by doing a static cast. In one case we change the type of an argument to be more accurate. Signed-off-by: Jacob Perron --- orocos_kdl/src/chainiksolvervel_pinv_givens.cpp | 8 ++++---- orocos_kdl/src/jacobian.cpp | 4 ++-- orocos_kdl/src/jntarray.cpp | 4 ++-- orocos_kdl/src/jntspaceinertiamatrix.cpp | 4 ++-- orocos_kdl/src/tree.cpp | 2 +- orocos_kdl/src/treeiksolverpos_online.cpp | 2 +- orocos_kdl/src/treeiksolverpos_online.hpp | 2 +- orocos_kdl/src/utilities/ldl_solver_eigen.cpp | 2 +- orocos_kdl/src/utilities/svd_eigen_HH.cpp | 4 ++-- 9 files changed, 16 insertions(+), 16 deletions(-) diff --git a/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp b/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp index b38ce04b..fbf102b4 100644 --- a/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp +++ b/orocos_kdl/src/chainiksolvervel_pinv_givens.cpp @@ -30,8 +30,8 @@ namespace KDL jnt2jac(chain), jac(nj), transpose(nj>6),toggle(true), - m(max(6,nj)), - n(min(6,nj)), + m(static_cast(max(6,nj))), + n(static_cast(min(6,nj))), jac_eigen(m,n), U(Eigen::MatrixXd::Identity(m,m)), V(Eigen::MatrixXd::Identity(n,n)), @@ -50,8 +50,8 @@ namespace KDL jnt2jac.updateInternalDataStructures(); jac.resize(nj); transpose = (nj > 6); - m = max(6,nj); - n = min(6,nj); + m = static_cast(max(6,nj)); + n = static_cast(min(6,nj)); jac_eigen.conservativeResize(m,n); U.conservativeResizeLike(Eigen::MatrixXd::Identity(m,m)); V.conservativeResizeLike(Eigen::MatrixXd::Identity(n,n)); diff --git a/orocos_kdl/src/jacobian.cpp b/orocos_kdl/src/jacobian.cpp index a4942076..21d3d649 100644 --- a/orocos_kdl/src/jacobian.cpp +++ b/orocos_kdl/src/jacobian.cpp @@ -68,12 +68,12 @@ namespace KDL unsigned int Jacobian::rows()const { - return data.rows(); + return static_cast(data.rows()); } unsigned int Jacobian::columns()const { - return data.cols(); + return static_cast(data.cols()); } void SetToZero(Jacobian& jac) diff --git a/orocos_kdl/src/jntarray.cpp b/orocos_kdl/src/jntarray.cpp index 6dc26218..32c875d0 100644 --- a/orocos_kdl/src/jntarray.cpp +++ b/orocos_kdl/src/jntarray.cpp @@ -69,12 +69,12 @@ namespace KDL unsigned int JntArray::rows()const { - return data.rows(); + return static_cast(data.rows()); } unsigned int JntArray::columns()const { - return data.cols(); + return static_cast(data.cols()); } void Add(const JntArray& src1,const JntArray& src2,JntArray& dest) diff --git a/orocos_kdl/src/jntspaceinertiamatrix.cpp b/orocos_kdl/src/jntspaceinertiamatrix.cpp index 31ea4200..fb8a4fed 100644 --- a/orocos_kdl/src/jntspaceinertiamatrix.cpp +++ b/orocos_kdl/src/jntspaceinertiamatrix.cpp @@ -67,12 +67,12 @@ namespace KDL unsigned int JntSpaceInertiaMatrix::rows()const { - return data.rows(); + return static_cast(data.rows()); } unsigned int JntSpaceInertiaMatrix::columns()const { - return data.cols(); + return static_cast(data.cols()); } diff --git a/orocos_kdl/src/tree.cpp b/orocos_kdl/src/tree.cpp index 6d8943de..f933e7ea 100644 --- a/orocos_kdl/src/tree.cpp +++ b/orocos_kdl/src/tree.cpp @@ -156,7 +156,7 @@ bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, } // add the segments from the common frame to the tip frame - for (int s=parents_chain_tip.size()-1; s>-1; s--){ + for (int s=static_cast(parents_chain_tip.size())-1; s>-1; s--){ chain.addSegment(GetTreeElementSegment(getSegment(parents_chain_tip[s])->second)); } return true; diff --git a/orocos_kdl/src/treeiksolverpos_online.cpp b/orocos_kdl/src/treeiksolverpos_online.cpp index 3e3d1318..84d3146c 100644 --- a/orocos_kdl/src/treeiksolverpos_online.cpp +++ b/orocos_kdl/src/treeiksolverpos_online.cpp @@ -26,7 +26,7 @@ namespace KDL { -TreeIkSolverPos_Online::TreeIkSolverPos_Online(const double& nr_of_jnts, +TreeIkSolverPos_Online::TreeIkSolverPos_Online(const unsigned int& nr_of_jnts, const std::vector& endpoints, const JntArray& q_min, const JntArray& q_max, diff --git a/orocos_kdl/src/treeiksolverpos_online.hpp b/orocos_kdl/src/treeiksolverpos_online.hpp index a9172a36..0ceda06f 100644 --- a/orocos_kdl/src/treeiksolverpos_online.hpp +++ b/orocos_kdl/src/treeiksolverpos_online.hpp @@ -61,7 +61,7 @@ class TreeIkSolverPos_Online: public TreeIkSolverPos { * @return */ - TreeIkSolverPos_Online(const double& nr_of_jnts, + TreeIkSolverPos_Online(const unsigned int& nr_of_jnts, const std::vector& endpoints, const JntArray& q_min, const JntArray& q_max, diff --git a/orocos_kdl/src/utilities/ldl_solver_eigen.cpp b/orocos_kdl/src/utilities/ldl_solver_eigen.cpp index 1729ead9..3f47afdc 100644 --- a/orocos_kdl/src/utilities/ldl_solver_eigen.cpp +++ b/orocos_kdl/src/utilities/ldl_solver_eigen.cpp @@ -25,7 +25,7 @@ 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(); + const int n = static_cast(A.rows()); int error=SolverI::E_NOERROR; //Check sizes diff --git a/orocos_kdl/src/utilities/svd_eigen_HH.cpp b/orocos_kdl/src/utilities/svd_eigen_HH.cpp index 2fdb9d2b..7c7bd379 100644 --- a/orocos_kdl/src/utilities/svd_eigen_HH.cpp +++ b/orocos_kdl/src/utilities/svd_eigen_HH.cpp @@ -26,8 +26,8 @@ namespace KDL{ int svd_eigen_HH(const Eigen::MatrixXd &A, Eigen::MatrixXd &U, Eigen::VectorXd &S, Eigen::MatrixXd &V, Eigen::VectorXd &tmp, int maxiter, double epsilon) { //get the rows/columns of the matrix - const int rows = A.rows(); - const int cols = A.cols(); + const int rows = static_cast(A.rows()); + const int cols = static_cast(A.cols()); U.setZero(); U.topLeftCorner(rows,cols)=A; From ef9b0ee228f1e69649d1b6762940c78293bc2873 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 31 Mar 2022 18:36:59 -0700 Subject: [PATCH 24/78] Use strncpy_s instead of strncpy on Windows Fixes a MSBuild warning on Windows. Signed-off-by: Jacob Perron --- orocos_kdl/src/utilities/error_stack.cxx | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/orocos_kdl/src/utilities/error_stack.cxx b/orocos_kdl/src/utilities/error_stack.cxx index 78c5027a..c4837e6e 100644 --- a/orocos_kdl/src/utilities/error_stack.cxx +++ b/orocos_kdl/src/utilities/error_stack.cxx @@ -57,7 +57,11 @@ void IOTracePopStr(char* buffer,int size) { *buffer = 0; return; } +#if defined(_WIN32) + strncpy_s(buffer,size,errorstack.top().c_str(),size); +#else strncpy(buffer,errorstack.top().c_str(),size); +#endif buffer[size - 1] = '\0'; errorstack.pop(); } From d70e705f2f766adc4b22f931a3aaa7ffd470feac Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 1 Apr 2022 08:56:02 -0700 Subject: [PATCH 25/78] Do not break API Use static cast instead to avoid compiler warnings. Signed-off-by: Jacob Perron --- orocos_kdl/src/treeiksolverpos_online.cpp | 10 +++++----- orocos_kdl/src/treeiksolverpos_online.hpp | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/orocos_kdl/src/treeiksolverpos_online.cpp b/orocos_kdl/src/treeiksolverpos_online.cpp index 84d3146c..35b2d5fb 100644 --- a/orocos_kdl/src/treeiksolverpos_online.cpp +++ b/orocos_kdl/src/treeiksolverpos_online.cpp @@ -26,7 +26,7 @@ namespace KDL { -TreeIkSolverPos_Online::TreeIkSolverPos_Online(const unsigned int& nr_of_jnts, +TreeIkSolverPos_Online::TreeIkSolverPos_Online(const double& nr_of_jnts, const std::vector& endpoints, const JntArray& q_min, const JntArray& q_max, @@ -35,12 +35,12 @@ TreeIkSolverPos_Online::TreeIkSolverPos_Online(const unsigned int& nr_of_jnts, const double x_dot_rot_max, TreeFkSolverPos& fksolver, TreeIkSolverVel& iksolver) : - q_min_(nr_of_jnts), - q_max_(nr_of_jnts), - q_dot_max_(nr_of_jnts), + q_min_(static_cast(nr_of_jnts)), + q_max_(static_cast(nr_of_jnts)), + q_dot_max_(static_cast(nr_of_jnts)), fksolver_(fksolver), iksolver_(iksolver), - q_dot_(nr_of_jnts) + q_dot_(static_cast(nr_of_jnts)) { q_min_ = q_min; q_max_ = q_max; diff --git a/orocos_kdl/src/treeiksolverpos_online.hpp b/orocos_kdl/src/treeiksolverpos_online.hpp index 0ceda06f..a9172a36 100644 --- a/orocos_kdl/src/treeiksolverpos_online.hpp +++ b/orocos_kdl/src/treeiksolverpos_online.hpp @@ -61,7 +61,7 @@ class TreeIkSolverPos_Online: public TreeIkSolverPos { * @return */ - TreeIkSolverPos_Online(const unsigned int& nr_of_jnts, + TreeIkSolverPos_Online(const double& nr_of_jnts, const std::vector& endpoints, const JntArray& q_min, const JntArray& q_max, From fc0dfd80cbe1810f7e27c10a860c7c1896174f99 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 1 Apr 2022 09:17:14 -0700 Subject: [PATCH 26/78] Remove members from initializer list They are set inside the constructor anyways. Signed-off-by: Jacob Perron --- orocos_kdl/src/treeiksolverpos_online.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/orocos_kdl/src/treeiksolverpos_online.cpp b/orocos_kdl/src/treeiksolverpos_online.cpp index 35b2d5fb..594c3a05 100644 --- a/orocos_kdl/src/treeiksolverpos_online.cpp +++ b/orocos_kdl/src/treeiksolverpos_online.cpp @@ -35,9 +35,6 @@ TreeIkSolverPos_Online::TreeIkSolverPos_Online(const double& nr_of_jnts, const double x_dot_rot_max, TreeFkSolverPos& fksolver, TreeIkSolverVel& iksolver) : - q_min_(static_cast(nr_of_jnts)), - q_max_(static_cast(nr_of_jnts)), - q_dot_max_(static_cast(nr_of_jnts)), fksolver_(fksolver), iksolver_(iksolver), q_dot_(static_cast(nr_of_jnts)) From 03269847366dfbd7938f1378b63e2a01d7637867 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 1 Apr 2022 12:16:43 -0700 Subject: [PATCH 27/78] Do not pass generated header to add_library Newer versions of cmake (>3.22.1) emit the following warning: CMake Warning (dev) at src/CMakeLists.txt:69 (ADD_LIBRARY): Policy CMP0115 is not set: Source file extensions must be explicit. Run "cmake --help-policy CMP0115" for policy details. Use the cmake_policy command to set the policy and suppress this warning. I don't know exactly why passing this generated header triggers the warning, but passing it to add_library doesn't seem necessary. Signed-off-by: Jacob Perron --- orocos_kdl/src/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/src/CMakeLists.txt b/orocos_kdl/src/CMakeLists.txt index d1e0b615..954b8fc0 100644 --- a/orocos_kdl/src/CMakeLists.txt +++ b/orocos_kdl/src/CMakeLists.txt @@ -50,7 +50,7 @@ IF(OROCOSKDL_ENABLE_RPATH) 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 "$") From 3324420efff9b58b53d37680beb01a81e440d6f6 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 1 Apr 2022 17:45:28 -0700 Subject: [PATCH 28/78] Use reverse iterator instead of decrementing index Signed-off-by: Jacob Perron --- orocos_kdl/src/tree.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/orocos_kdl/src/tree.cpp b/orocos_kdl/src/tree.cpp index f933e7ea..70151e66 100644 --- a/orocos_kdl/src/tree.cpp +++ b/orocos_kdl/src/tree.cpp @@ -156,8 +156,8 @@ bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, } // add the segments from the common frame to the tip frame - for (int s=static_cast(parents_chain_tip.size())-1; s>-1; s--){ - chain.addSegment(GetTreeElementSegment(getSegment(parents_chain_tip[s])->second)); + for (auto rit=parents_chain_tip.rbegin(); rit != parents_chain_tip.rend(); ++rit){ + chain.addSegment(GetTreeElementSegment(getSegment(*rit)->second)); } return true; } From 7a9cb6f6a31ef4f07e5e8677f4b85ce9750c3cdc Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 4 Apr 2022 11:50:56 -0700 Subject: [PATCH 29/78] Fix CMake warning on Windows Because the minimum cmake version is set to 3.0.2, we see the following policy warning on Windows: CMake Warning (dev) at C:/ci/ws/install/share/cmake/pybind11/pybind11Common.cmake:127 (if): Policy CMP0054 is not set: Only interpret if() arguments as variables or keywords when unquoted. Run "cmake --help-policy CMP0054" for policy details. Use the cmake_policy command to set the policy and suppress this warning. Quoted variables like "MSVC" will no longer be dereferenced when the policy is set to NEW. Since the policy is not set the OLD behavior will be used. If we want to continue to support older versions of CMake, we should opt into the new behavior to not break the logic in pybind11Common.cmake. Alternatively, we could increase the miminum cmake version of this project. Signed-off-by: Jacob Perron --- python_orocos_kdl/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index d29319fc..916be8fb 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -2,6 +2,10 @@ cmake_minimum_required(VERSION 3.0.2) if(POLICY CMP0048) cmake_policy(SET CMP0048 NEW) endif() +# Avoid warning from pybind11 on Windows +if(POLICY CMP0054) + cmake_policy(SET CMP0054 NEW) +endif() if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) # support IN_LISTS which is required for PyBind11 2.6 and newer endif() From 884289867827645d56faa4bab85127f2f8188397 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 12 May 2022 07:51:50 +0200 Subject: [PATCH 30/78] Bump python_orocos_kdl/pybind11 from `ffa3468` to `914c06f` (#395) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `ffa3468` to `914c06f`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/ffa346860b306c9bbfb341aed9c14c067751feb8...914c06fb252b6cc3727d0eedab6736e88a3fcb01) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index ffa34686..914c06fb 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit ffa346860b306c9bbfb341aed9c14c067751feb8 +Subproject commit 914c06fb252b6cc3727d0eedab6736e88a3fcb01 From 08b283cba8e5f820d3af5df4e15bcda3ecd3b560 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Thu, 12 May 2022 15:25:44 +0200 Subject: [PATCH 31/78] (PyKDL) add more argument names --- python_orocos_kdl/PyKDL/dynamics.cpp | 20 ++--- python_orocos_kdl/PyKDL/frames.cpp | 66 ++++++++-------- python_orocos_kdl/PyKDL/framevel.cpp | 20 ++--- python_orocos_kdl/PyKDL/kinfam.cpp | 114 +++++++++++++-------------- 4 files changed, 110 insertions(+), 110 deletions(-) diff --git a/python_orocos_kdl/PyKDL/dynamics.cpp b/python_orocos_kdl/PyKDL/dynamics.cpp index 2098fbd5..46d0f503 100644 --- a/python_orocos_kdl/PyKDL/dynamics.cpp +++ b/python_orocos_kdl/PyKDL/dynamics.cpp @@ -43,7 +43,7 @@ void init_dynamics(pybind11::module &m) jnt_space_inertia_matrix.def(py::init<>()); jnt_space_inertia_matrix.def(py::init()); jnt_space_inertia_matrix.def(py::init()); - jnt_space_inertia_matrix.def("resize", &JntSpaceInertiaMatrix::resize); + jnt_space_inertia_matrix.def("resize", &JntSpaceInertiaMatrix::resize, py::arg("new_size")); 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 idx) @@ -72,12 +72,12 @@ void init_dynamics(pybind11::module &m) }); 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("Add", (void (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&, JntSpaceInertiaMatrix&)) &KDL::Add, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Subtract", (void (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&,JntSpaceInertiaMatrix&)) &KDL::Subtract, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Multiply", (void (*)(const JntSpaceInertiaMatrix&, const double&, JntSpaceInertiaMatrix&)) &KDL::Multiply, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("Divide", (void (*)(const JntSpaceInertiaMatrix&, const double&, JntSpaceInertiaMatrix&)) &KDL::Divide, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("Multiply", (void (*)(const JntSpaceInertiaMatrix&, const JntArray&, JntArray&)) &KDL::Multiply, py::arg("src"), py::arg("vec"), py::arg("dest")); + m.def("SetToZero", (void (*)(JntSpaceInertiaMatrix&)) &KDL::SetToZero, py::arg("matrix")); m.def("Equal", (bool (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&, double)) &KDL::Equal, py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon); @@ -87,7 +87,7 @@ void init_dynamics(pybind11::module &m) // -------------------- py::class_ chain_dyn_param(m, "ChainDynParam"); chain_dyn_param.def(py::init()); - chain_dyn_param.def("JntToCoriolis", &ChainDynParam::JntToCoriolis); - chain_dyn_param.def("JntToMass", &ChainDynParam::JntToMass); - chain_dyn_param.def("JntToGravity", &ChainDynParam::JntToGravity); + chain_dyn_param.def("JntToCoriolis", &ChainDynParam::JntToCoriolis, py::arg("q"), py::arg("q_dot"), py::arg("coriolis")); + chain_dyn_param.def("JntToMass", &ChainDynParam::JntToMass, py::arg("q"), py::arg("H")); + chain_dyn_param.def("JntToGravity", &ChainDynParam::JntToGravity, py::arg("q"), py::arg("gravity")); } diff --git a/python_orocos_kdl/PyKDL/frames.cpp b/python_orocos_kdl/PyKDL/frames.cpp index 5aa9d1f5..b24dd76d 100644 --- a/python_orocos_kdl/PyKDL/frames.cpp +++ b/python_orocos_kdl/PyKDL/frames.cpp @@ -40,9 +40,9 @@ void init_frames(py::module &m) vector.def(py::init<>()); vector.def(py::init(), py::arg("x"), py::arg("y"), py::arg("z")); vector.def(py::init()); - 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", (void (Vector::*)(double)) &Vector::x, py::arg("value")); + vector.def("y", (void (Vector::*)(double)) &Vector::y, py::arg("value")); + vector.def("z", (void (Vector::*)(double)) &Vector::z, py::arg("value")); 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); @@ -52,14 +52,14 @@ void init_frames(py::module &m) throw py::index_error("Vector index out of range"); return v(i); - }); + }, py::arg("index")); 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; - }); + }, py::arg("index"), py::arg("value")); vector.def("__repr__", [](const Vector &v) { std::ostringstream oss; @@ -108,7 +108,7 @@ void init_frames(py::module &m) return v; })); - m.def("SetToZero", (void (*)(Vector&)) &KDL::SetToZero); + m.def("SetToZero", (void (*)(Vector&)) &KDL::SetToZero, py::arg("vector")); 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); @@ -129,14 +129,14 @@ void init_frames(py::module &m) throw py::index_error("Wrench index out of range"); return t(i); - }); + }, py::arg("index")); 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; - }); + }, py::arg("index"), py::arg("value")); wrench.def("__repr__", [](const Wrench &t) { std::ostringstream oss; @@ -183,7 +183,7 @@ void init_frames(py::module &m) return wr; })); - m.def("SetToZero", (void (*)(Wrench&)) &KDL::SetToZero); + m.def("SetToZero", (void (*)(Wrench&)) &KDL::SetToZero, py::arg("wrench")); m.def("Equal", (bool (*)(const Wrench&, const Wrench&, double eps)) &KDL::Equal, py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); @@ -203,14 +203,14 @@ void init_frames(py::module &m) throw py::index_error("Twist index out of range"); return t(i); - }); + }, py::arg("index")); 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; - }); + }, py::arg("index"), py::arg("value")); twist.def("__repr__", [](const Twist &t) { std::ostringstream oss; @@ -259,7 +259,7 @@ void init_frames(py::module &m) 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("SetToZero", (void (*)(Twist&)) &KDL::SetToZero, py::arg("twist")); m.def("Equal", (bool (*)(const Twist&, const Twist&, double eps)) &KDL::Equal, py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); @@ -284,7 +284,7 @@ void init_frames(py::module &m) throw py::index_error("Rotation index out of range"); return r(i, j); - }); + }, py::arg("index")); rotation.def("__setitem__", [](Rotation &r, std::tuple idx, double value) { int i = std::get<0>(idx); @@ -293,7 +293,7 @@ void init_frames(py::module &m) throw py::index_error("Rotation index out of range"); r(i, j) = value; - }); + }, py::arg("index"), py::arg("value")); rotation.def("__repr__", [](const Rotation &r) { std::ostringstream oss; @@ -314,18 +314,18 @@ void init_frames(py::module &m) 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("RotX", &Rotation::RotX, py::arg("angle")); + rotation.def_static("RotY", &Rotation::RotY, py::arg("angle")); + rotation.def_static("RotZ", &Rotation::RotZ, py::arg("angle")); + rotation.def_static("Rot", &Rotation::Rot, py::arg("rotvec"), py::arg("angle")); + rotation.def_static("Rot2", &Rotation::Rot2, py::arg("rotvec"), py::arg("angle")); + rotation.def_static("EulerZYZ", &Rotation::EulerZYZ, py::arg("alpha"), py::arg("beta"), py::arg("gamma")); + rotation.def_static("RPY", &Rotation::RPY, py::arg("roll"), py::arg("pitch"), py::arg("yaw")); + rotation.def_static("EulerZYX", &Rotation::EulerZYX, py::arg("alpha"), py::arg("beta"), py::arg("gamma")); rotation.def_static("Quaternion", &Rotation::Quaternion, py::arg("x"), py::arg("y"), py::arg("z"), py::arg("w")); - rotation.def("DoRotX", &Rotation::DoRotX); - rotation.def("DoRotY", &Rotation::DoRotY); - rotation.def("DoRotZ", &Rotation::DoRotZ); + rotation.def("DoRotX", &Rotation::DoRotX, py::arg("angle")); + rotation.def("DoRotY", &Rotation::DoRotY, py::arg("angle")); + rotation.def("DoRotZ", &Rotation::DoRotZ, py::arg("angle")); rotation.def("GetRot", &Rotation::GetRot); rotation.def("GetRotAngle", [](const Rotation &r, double eps) { @@ -360,9 +360,9 @@ void init_frames(py::module &m) 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("UnitX", (void (Rotation::*)(const Vector&)) &Rotation::UnitX, py::arg("vec")); + rotation.def("UnitY", (void (Rotation::*)(const Vector&)) &Rotation::UnitY, py::arg("vec")); + rotation.def("UnitZ", (void (Rotation::*)(const Vector&)) &Rotation::UnitZ, py::arg("vec")); rotation.def(py::self * Vector()); rotation.def(py::self * Twist()); rotation.def(py::self * Wrench()); @@ -409,7 +409,7 @@ void init_frames(py::module &m) throw py::index_error("Frame index out of range"); return frm(i, j); - }); + }, py::arg("index")); frame.def("__setitem__", [](Frame &frm, std::tuple idx, double value) { int i = std::get<0>(idx); @@ -421,7 +421,7 @@ void init_frames(py::module &m) frm.p(i) = value; else frm.M(i, j) = value; - }); + }, py::arg("index"), py::arg("value")); frame.def("__repr__", [](const Frame &frm) { std::ostringstream oss; @@ -436,14 +436,14 @@ void init_frames(py::module &m) { return Frame(self); }, py::arg("memo")); - frame.def_static("DH_Craig1989", &Frame::DH_Craig1989); - frame.def_static("DH", &Frame::DH); + frame.def_static("DH_Craig1989", &Frame::DH_Craig1989, py::arg("a"), py::arg("alpha"), py::arg("d"), py::arg("theta")); + frame.def_static("DH", &Frame::DH, py::arg("a"), py::arg("alpha"), py::arg("d"), py::arg("theta")); 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("Integrate", &Frame::Integrate, py::arg("twist"), py::arg("sample_frequency")); frame.def(py::self * Vector()); frame.def(py::self * Wrench()); frame.def(py::self * Twist()); diff --git a/python_orocos_kdl/PyKDL/framevel.cpp b/python_orocos_kdl/PyKDL/framevel.cpp index bf8609bd..1a660278 100644 --- a/python_orocos_kdl/PyKDL/framevel.cpp +++ b/python_orocos_kdl/PyKDL/framevel.cpp @@ -152,7 +152,7 @@ void init_framevel(pybind11::module &m) return vv; })); - m.def("SetToZero", (void (*)(VectorVel&)) &KDL::SetToZero); + m.def("SetToZero", (void (*)(VectorVel&)) &KDL::SetToZero, py::arg("vector_vel")); 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, @@ -237,7 +237,7 @@ void init_framevel(pybind11::module &m) return tv; })); - m.def("SetToZero", (void (*)(TwistVel&)) &KDL::SetToZero); + m.def("SetToZero", (void (*)(TwistVel&)) &KDL::SetToZero, py::arg("twist_vel")); 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, @@ -279,14 +279,14 @@ void init_framevel(pybind11::module &m) 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("DoRotX", &RotationVel::DoRotX, py::arg("angle")); + rotation_vel.def("DoRotY", &RotationVel::DoRotY, py::arg("angle")); + rotation_vel.def("DoRotZ", &RotationVel::DoRotZ, py::arg("angle")); + rotation_vel.def_static("RotX", &RotationVel::RotX, py::arg("angle")); + rotation_vel.def_static("RotY", &RotationVel::RotY, py::arg("angle")); + rotation_vel.def_static("RotZ", &RotationVel::RotZ, py::arg("angle")); + rotation_vel.def_static("Rot", &RotationVel::Rot, py::arg("rotvec"), py::arg("angle")); + rotation_vel.def_static("Rot2", &RotationVel::Rot2, py::arg("rotvec"), py::arg("angle")); rotation_vel.def("Inverse", (TwistVel (RotationVel::*)(const TwistVel&) const) &RotationVel::Inverse); rotation_vel.def("Inverse", (TwistVel (RotationVel::*)(const Twist&) const) &RotationVel::Inverse); diff --git a/python_orocos_kdl/PyKDL/kinfam.cpp b/python_orocos_kdl/PyKDL/kinfam.cpp index d73193e9..7b302c45 100644 --- a/python_orocos_kdl/PyKDL/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/kinfam.cpp @@ -85,8 +85,8 @@ void init_kinfam(pybind11::module &m) 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()); - joint.def("pose", &Joint::pose); - joint.def("twist", &Joint::twist); + joint.def("pose", &Joint::pose, py::arg("q")); + joint.def("twist", &Joint::twist, py::arg("q_dot")); joint.def("JointAxis", &Joint::JointAxis); joint.def("JointOrigin", &Joint::JointOrigin); joint.def("getName", &Joint::getName); @@ -114,14 +114,14 @@ void init_kinfam(pybind11::module &m) throw py::index_error("RotationalInertia index out of range"); return inertia.data[i]; - }); + }, py::arg("index")); 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; - }); + }, py::arg("index"), py::arg("value")); rotational_inertia.def(double() * py::self); rotational_inertia.def(py::self + py::self); rotational_inertia.def(py::self * Vector()); @@ -135,7 +135,7 @@ void init_kinfam(pybind11::module &m) 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("RefPoint", &RigidBodyInertia::RefPoint, py::arg("p")); rigid_body_inertia.def("getMass", &RigidBodyInertia::getMass); rigid_body_inertia.def("getCOG", &RigidBodyInertia::getCOG); rigid_body_inertia.def("getRotationalInertia", &RigidBodyInertia::getRotationalInertia); @@ -158,12 +158,12 @@ void init_kinfam(pybind11::module &m) py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero")); segment.def(py::init()); segment.def("getFrameToTip", &Segment::getFrameToTip); - segment.def("pose", &Segment::pose); - segment.def("twist", &Segment::twist); + segment.def("pose", &Segment::pose, py::arg("q")); + segment.def("twist", &Segment::twist, py::arg("q"), py::arg("q_dot")); segment.def("getName", &Segment::getName); segment.def("getJoint", &Segment::getJoint); segment.def("getInertia", &Segment::getInertia); - segment.def("setInertia", &Segment::setInertia); + segment.def("setInertia", &Segment::setInertia, py::arg("I_in")); // -------------------- @@ -172,12 +172,12 @@ void init_kinfam(pybind11::module &m) py::class_ chain(m, "Chain"); chain.def(py::init<>()); chain.def(py::init()); - chain.def("addSegment", &Chain::addSegment); - chain.def("addChain", &Chain::addChain); + chain.def("addSegment", &Chain::addSegment, py::arg("segment")); + chain.def("addChain", &Chain::addChain, py::arg("chain")); 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("getSegment", (Segment& (Chain::*)(unsigned int)) &Chain::getSegment, py::arg("index")); + chain.def("getSegment", (const Segment& (Chain::*)(unsigned int) const) &Chain::getSegment, py::arg("index")); chain.def("__repr__", [](const Chain &c) { std::ostringstream oss; @@ -215,13 +215,13 @@ void init_kinfam(pybind11::module &m) // -------------------- py::class_ jacobian(m, "Jacobian"); jacobian.def(py::init<>()); - jacobian.def(py::init()); + jacobian.def(py::init(), py::arg("nr_columns")); jacobian.def(py::init()); 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("resize", &Jacobian::resize, py::arg("nr_columns")); + jacobian.def("getColumn", &Jacobian::getColumn, py::arg("index")); + jacobian.def("setColumn", &Jacobian::setColumn, py::arg("index"), py::arg("t")); jacobian.def("changeRefPoint", &Jacobian::changeRefPoint, py::arg("base")); jacobian.def("changeBase", &Jacobian::changeBase, py::arg("rot")); jacobian.def("changeRefFrame", &Jacobian::changeRefFrame, py::arg("frame")); @@ -232,7 +232,7 @@ void init_kinfam(pybind11::module &m) if (i < 0 || i > 5 || j < 0 || (unsigned int)j >= jac.columns()) throw py::index_error("Jacobian index out of range"); return jac((unsigned int)i, (unsigned int)j); - }); + }, py::arg("index")); jacobian.def("__setitem__", [](Jacobian &jac, std::tuple idx, double value) { int i = std::get<0>(idx); @@ -241,7 +241,7 @@ void init_kinfam(pybind11::module &m) throw py::index_error("Jacobian index out of range"); jac((unsigned int)i, (unsigned int)j) = value; - }); + }, py::arg("index"), py::arg("value")); jacobian.def("__repr__", [](const Jacobian &jac) { std::ostringstream oss; @@ -249,10 +249,10 @@ void init_kinfam(pybind11::module &m) 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); + m.def("SetToZero", (void (*)(Jacobian&)) &KDL::SetToZero, py::arg("jac")); + m.def("changeRefPoint", (void (*)(const Jacobian&, const Vector&, Jacobian&)) &KDL::changeRefPoint, py::arg("src"), py::arg("base"), py::arg("dest")); + m.def("changeBase", (void (*)(const Jacobian&, const Rotation&, Jacobian&)) &KDL::changeBase, py::arg("src"), py::arg("rot"), py::arg("dest")); + m.def("SetToZero", (void (*)(const Jacobian&, const Frame&, Jacobian&)) &KDL::changeRefFrame, py::arg("src"), py::arg("frame"), py::arg("dest")); // -------------------- @@ -260,25 +260,25 @@ void init_kinfam(pybind11::module &m) // -------------------- py::class_ jnt_array(m, "JntArray"); jnt_array.def(py::init<>()); - jnt_array.def(py::init()); + jnt_array.def(py::init(), py::arg("size")); jnt_array.def(py::init()); jnt_array.def("rows", &JntArray::rows); jnt_array.def("columns", &JntArray::columns); - jnt_array.def("resize", &JntArray::resize); + jnt_array.def("resize", &JntArray::resize, py::arg("size")); jnt_array.def("__getitem__", [](const JntArray &ja, int i) { if (i < 0 || (unsigned int)i >= ja.rows()) throw py::index_error("JntArray index out of range"); return ja(i); - }); + }, py::arg("index")); jnt_array.def("__setitem__", [](JntArray &ja, int i, double value) { if (i < 0 || (unsigned int)i >= ja.rows()) throw py::index_error("JntArray index out of range"); ja(i) = value; - }); + }, py::arg("index"), py::arg("value")); jnt_array.def("__repr__", [](const JntArray &ja) { std::ostringstream oss; @@ -287,12 +287,12 @@ void init_kinfam(pybind11::module &m) }); 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("Add", (void (*)(const JntArray&, const JntArray&, JntArray&)) &KDL::Add, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Subtract", (void (*)(const JntArray&, const JntArray&, JntArray&)) &KDL::Subtract, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Multiply", (void (*)(const JntArray&, const double&, JntArray&)) &KDL::Multiply, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("Divide", (void (*)(const JntArray&, const double&, JntArray&)) &KDL::Divide, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("MultiplyJacobian", (void (*)(const Jacobian&, const JntArray&, Twist&)) &KDL::MultiplyJacobian, py::arg("jac"), py::arg("src"), py::arg("dest")); + m.def("SetToZero", (void (*)(JntArray&)) &KDL::SetToZero, py::arg("jnt_array")); m.def("Equal", (bool (*)(const JntArray&, const JntArray&, double)) &KDL::Equal, py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon); @@ -303,22 +303,22 @@ void init_kinfam(pybind11::module &m) py::class_ 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()); - jnt_array_vel.def(py::init(), py::arg("q"), py::arg("qdot")); + jnt_array_vel.def(py::init(), py::arg("size")); + jnt_array_vel.def(py::init(), py::arg("q"), py::arg("q_dot")); jnt_array_vel.def(py::init()); - jnt_array_vel.def("resize", &JntArrayVel::resize); + jnt_array_vel.def("resize", &JntArrayVel::resize, py::arg("size")); 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("Add", (void (*)(const JntArrayVel&, const JntArrayVel&, JntArrayVel&)) &KDL::Add, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Add", (void (*)(const JntArrayVel&, const JntArray&, JntArrayVel&)) &KDL::Add, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Subtract", (void (*)(const JntArrayVel&, const JntArrayVel&, JntArrayVel&)) &KDL::Subtract, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Subtract", (void (*)(const JntArrayVel&, const JntArray&, JntArrayVel&)) &KDL::Subtract, py::arg("src1"), py::arg("src2"), py::arg("dest")); + m.def("Multiply", (void (*)(const JntArrayVel&, const double&, JntArrayVel&)) &KDL::Multiply, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("Multiply", (void (*)(const JntArrayVel&, const doubleVel&, JntArrayVel&)) &KDL::Multiply, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("Divide", (void (*)(const JntArrayVel&, const double&, JntArrayVel&)) &KDL::Divide, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("Divide", (void (*)(const JntArrayVel&, const doubleVel&, JntArrayVel&)) &KDL::Divide, py::arg("src"), py::arg("factor"), py::arg("dest")); + m.def("SetToZero", (void (*)(JntArrayVel&)) &KDL::SetToZero, py::arg("jnt_array_vel")); m.def("Equal", (bool (*)(const JntArrayVel&, const JntArrayVel&, double)) &KDL::Equal, py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon); @@ -328,7 +328,7 @@ void init_kinfam(pybind11::module &m) // -------------------- py::class_ solver_i(m, "SolverI"); solver_i.def("getError", &SolverI::getError); - solver_i.def("strError", &SolverI::strError); + solver_i.def("strError", &SolverI::strError, py::arg("error")); solver_i.def("updateInternalDataStructures", &SolverI::updateInternalDataStructures); @@ -381,7 +381,7 @@ void init_kinfam(pybind11::module &m) // -------------------- py::class_ 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")); + py::arg("q_in"), py::arg("v_in"), py::arg("q_dot_out")); // Not yet implemented in orocos_kdl // 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")); @@ -420,14 +420,14 @@ void init_kinfam(pybind11::module &m) py::class_ chain_ik_solver_vel_wdls(m, "ChainIkSolverVel_wdls"); chain_ik_solver_vel_wdls.def(py::init(), 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("setWeightTS", &ChainIkSolverVel_wdls::setWeightTS, py::arg("matrix")); + chain_ik_solver_vel_wdls.def("setWeightJS", &ChainIkSolverVel_wdls::setWeightJS, py::arg("matrix")); + chain_ik_solver_vel_wdls.def("setLambda", &ChainIkSolverVel_wdls::setLambda, py::arg("lambda")); + chain_ik_solver_vel_wdls.def("setEps", &ChainIkSolverVel_wdls::setEps, py::arg("eps")); + chain_ik_solver_vel_wdls.def("setMaxIter", &ChainIkSolverVel_wdls::setMaxIter, py::arg("max_iter")); 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("getSigma", &ChainIkSolverVel_wdls::getSigma, py::arg("sigma_out")); 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); @@ -452,9 +452,9 @@ void init_kinfam(pybind11::module &m) py::class_ chain_ik_solver_vel_pinv_nso(m, "ChainIkSolverVel_pinv_nso"); chain_ik_solver_vel_pinv_nso.def(py::init(), 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("setWeights", &ChainIkSolverVel_pinv_nso::setWeights, py::arg("weights")); + chain_ik_solver_vel_pinv_nso.def("setOptPos", &ChainIkSolverVel_pinv_nso::setOptPos, py::arg("opt_pos")); + chain_ik_solver_vel_pinv_nso.def("setAlpha", &ChainIkSolverVel_pinv_nso::setAlpha, py::arg("alpha")); 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); @@ -474,7 +474,7 @@ void init_kinfam(pybind11::module &m) chain_jnt_to_jac_solver.def(py::init(), py::arg("chain")); chain_jnt_to_jac_solver.def("JntToJac", &ChainJntToJacSolver::JntToJac, py::arg("q_in"), py::arg("jac"), py::arg("seg_nr")=-1); - chain_jnt_to_jac_solver.def("setLockedJoints", &ChainJntToJacSolver::setLockedJoints); + chain_jnt_to_jac_solver.def("setLockedJoints", &ChainJntToJacSolver::setLockedJoints, py::arg("locked_joints")); // ------------------------------ @@ -508,7 +508,7 @@ void init_kinfam(pybind11::module &m) // ChainIdSolver // ------------------------------ py::class_ chain_id_solver(m, "ChainIdSolver"); - chain_id_solver.def("CartToJnt", &ChainIdSolver::CartToJnt); + chain_id_solver.def("CartToJnt", &ChainIdSolver::CartToJnt, py::arg("q"), py::arg("q_dot"), py::arg("q_dot_dot"), py::arg("f_ext"), py::arg("torques")); // ------------------------------ From 6480921b236baacf282d32857c9a4a1c6f9bd971 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 11:00:32 +0200 Subject: [PATCH 32/78] Add KDL::hash_combine function Inspired by https://github.com/boostorg/multiprecision/blob/boost-1.79.0/include/boost/multiprecision/detail/hash.hpp#L35-L41 --- orocos_kdl/src/utilities/hash_combine.h | 26 +++++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 orocos_kdl/src/utilities/hash_combine.h diff --git a/orocos_kdl/src/utilities/hash_combine.h b/orocos_kdl/src/utilities/hash_combine.h new file mode 100644 index 00000000..978903a2 --- /dev/null +++ b/orocos_kdl/src/utilities/hash_combine.h @@ -0,0 +1,26 @@ +#ifndef KDL_HASH_COMBINE_H_ +#define KDL_HASH_COMBINE_H_ + +#include + +namespace KDL +{ + +/** + * @brief Combine hash of object \p v to the \p seed + * @param seed Seed to append the hash of \p v + * @param v Object of which the hash should be appended to the seed + * + * Inspired by: + * @link https://github.com/boostorg/multiprecision/blob/boost-1.79.0/include/boost/multiprecision/detail/hash.hpp#L35-L41 + */ +template +inline void hash_combine(std::size_t& seed, const T& v) +{ + std::hash hasher; + seed ^= hasher(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2); +} + +} + +#endif From e947516c32d848b5f6fcd35eb098332b0019c371 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 11:24:26 +0200 Subject: [PATCH 33/78] (KDL) add hash functions for frames --- orocos_kdl/src/frames.hpp | 101 ++++++++++++++++++++++++++++++++++++-- 1 file changed, 97 insertions(+), 4 deletions(-) diff --git a/orocos_kdl/src/frames.hpp b/orocos_kdl/src/frames.hpp index 055beb3b..d816f0da 100644 --- a/orocos_kdl/src/frames.hpp +++ b/orocos_kdl/src/frames.hpp @@ -127,13 +127,15 @@ #include "utilities/kdl-config.h" #include "utilities/utility.h" +#include "utilities/hash_combine.h" + +#include ///////////////////////////////////////////////////////////// namespace KDL { - class Vector; class Rotation; class Frame; @@ -711,6 +713,7 @@ class Frame { inline friend bool operator!=(const Frame& a,const Frame& b); }; + /** * \brief represents both translational and rotational velocities. * @@ -788,9 +791,9 @@ class Twist { // = Friends friend class Rotation; friend class Frame; - }; + /** * \brief represents both translational and rotational acceleration. * @@ -950,8 +953,6 @@ class Wrench friend class Rotation; friend class Frame; - - }; @@ -1091,6 +1092,7 @@ class Rotation2 inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps); }; + //! A 2D frame class, for further documentation see the Frames class //! for methods with unchanged semantics. class Frame2 @@ -1257,5 +1259,96 @@ IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1); } +template<> struct std::hash +{ + std::size_t operator()(KDL::Vector const& v) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, v.x()); + KDL::hash_combine(seed, v.y()); + KDL::hash_combine(seed, v.z()); + return seed; + } +}; + +template<> struct std::hash +{ + std::size_t operator()(KDL::Rotation const& r) const noexcept + { + size_t seed = 0; + double x, y, z, w; + r.GetQuaternion(x, y, z, w); + KDL::hash_combine(seed, x); + KDL::hash_combine(seed, y); + KDL::hash_combine(seed, z); + KDL::hash_combine(seed, w); + return seed; + } +}; + +template<> struct std::hash +{ + std::size_t operator()(KDL::Frame const& f) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, f.p); + KDL::hash_combine(seed, f.M); + return seed; + } +}; + +template<> struct std::hash +{ + std::size_t operator()(KDL::Wrench const& w) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, w.force); + KDL::hash_combine(seed, w.torque); + return seed; + } +}; + +template<> struct std::hash +{ + std::size_t operator()(KDL::Twist const& t) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, t.vel); + KDL::hash_combine(seed, t.rot); + return seed; + } +}; + +template<> struct std::hash +{ + std::size_t operator()(KDL::Vector2 const& v) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, v.x()); + KDL::hash_combine(seed, v.y()); + return seed; + } +}; + +template<> struct std::hash +{ + std::size_t operator()(KDL::Rotation2 const& r) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, r.GetRot()); + return seed; + } +}; + +template<> struct std::hash +{ + std::size_t operator()(KDL::Frame2 const& f) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, f.p); + KDL::hash_combine(seed, f.M); + return seed; + } +}; #endif From 8d3ef2c52b19ff987cf2399816c8a4f2f841a14a Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 11:24:40 +0200 Subject: [PATCH 34/78] (KDL) add hash functions for framevel --- orocos_kdl/src/framevel.hpp | 63 +++++++++++++++++++++++++++++++------ 1 file changed, 54 insertions(+), 9 deletions(-) diff --git a/orocos_kdl/src/framevel.hpp b/orocos_kdl/src/framevel.hpp index 650bd3c8..b84e69c1 100644 --- a/orocos_kdl/src/framevel.hpp +++ b/orocos_kdl/src/framevel.hpp @@ -30,7 +30,6 @@ #include "frames.hpp" - namespace KDL { typedef Rall1d doubleVel; @@ -81,6 +80,7 @@ IMETHOD bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon); IMETHOD bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon); IMETHOD bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon); + class VectorVel // = TITLE // An VectorVel is a Vector and its first derivative @@ -143,7 +143,6 @@ class VectorVel }; - class RotationVel // = TITLE // An RotationVel is a Rotation and its first derivative, a rotation vector @@ -207,8 +206,6 @@ class RotationVel }; - - class FrameVel // = TITLE // An FrameVel is a Frame and its first derivative, a Twist vector @@ -268,9 +265,6 @@ class FrameVel }; - - - //very similar to Wrench class. class TwistVel // = TITLE @@ -416,10 +410,61 @@ IMETHOD void posrandom(FrameVel& F) { #include "framevel.inl" #endif -} // namespace +} // namespace KDL -#endif +template<> struct std::hash +{ + std::size_t operator()(KDL::doubleVel const& dv) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, dv.value()); + KDL::hash_combine(seed, dv.deriv()); + return seed; + } +}; + +template<> struct std::hash +{ + std::size_t operator()(KDL::VectorVel const& vv) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, vv.p); + KDL::hash_combine(seed, vv.v); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::RotationVel const& rv) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, rv.R); + KDL::hash_combine(seed, rv.w); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::FrameVel const& fv) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, fv.M); + KDL::hash_combine(seed, fv.p); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::TwistVel const& tv) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, tv.vel); + KDL::hash_combine(seed, tv.rot); + return seed; + } +}; +#endif From 987f4015167f02d43e3363e5cc06edcb6f829ada Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 11:24:55 +0200 Subject: [PATCH 35/78] (KDL) add hash functions for frameacc --- orocos_kdl/src/frameacc.hpp | 73 ++++++++++++++++++++++++++++--------- 1 file changed, 56 insertions(+), 17 deletions(-) diff --git a/orocos_kdl/src/frameacc.hpp b/orocos_kdl/src/frameacc.hpp index 20d4631b..a8cd3529 100644 --- a/orocos_kdl/src/frameacc.hpp +++ b/orocos_kdl/src/frameacc.hpp @@ -34,7 +34,6 @@ #include "frames.hpp" - namespace KDL { class TwistAcc; @@ -58,6 +57,7 @@ IMETHOD bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon); IMETHOD bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon); IMETHOD bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon); + class VectorAcc { public: @@ -106,7 +106,6 @@ class VectorAcc }; - class RotationAcc { public: @@ -160,8 +159,6 @@ class RotationAcc }; - - class FrameAcc { public: @@ -199,12 +196,6 @@ class FrameAcc }; - - - - - - //very similar to Wrench class. class TwistAcc { @@ -257,20 +248,68 @@ class TwistAcc }; - - - - - - #ifdef KDL_INLINE #include "frameacc.inl" #endif -} +} // namespace KDL +template<> struct std::hash +{ + std::size_t operator()(KDL::doubleAcc const& da) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, da.t); + KDL::hash_combine(seed, da.d); + KDL::hash_combine(seed, da.dd); + return seed; + } +}; + +template<> struct std::hash +{ + std::size_t operator()(KDL::VectorAcc const& va) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, va.p); + KDL::hash_combine(seed, va.v); + KDL::hash_combine(seed, va.dv); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::RotationAcc const& ra) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, ra.R); + KDL::hash_combine(seed, ra.w); + KDL::hash_combine(seed, ra.dw); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::FrameAcc const& fa) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, fa.M); + KDL::hash_combine(seed, fa.p); + return seed; + } +}; +template<> struct std::hash +{ + std::size_t operator()(KDL::TwistAcc const& ta) const noexcept + { + size_t seed = 0; + KDL::hash_combine(seed, ta.vel); + KDL::hash_combine(seed, ta.rot); + return seed; + } +}; #endif From 896e36ba7b0ae307a790fd9fc2d28e33d579aafe Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 11:33:48 +0200 Subject: [PATCH 36/78] (PyKDL) add hash to frames --- python_orocos_kdl/PyKDL/frames.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/python_orocos_kdl/PyKDL/frames.cpp b/python_orocos_kdl/PyKDL/frames.cpp index b24dd76d..9cbee1b9 100644 --- a/python_orocos_kdl/PyKDL/frames.cpp +++ b/python_orocos_kdl/PyKDL/frames.cpp @@ -77,6 +77,7 @@ void init_frames(py::module &m) vector.def(py::self * py::self); vector.def(py::self == py::self); vector.def(py::self != py::self); + vector.def(py::hash(py::self)); vector.def("__neg__", [](const Vector &a) { return operator-(a); @@ -163,6 +164,7 @@ void init_frames(py::module &m) wrench.def(py::self - py::self); wrench.def(py::self == py::self); wrench.def(py::self != py::self); + wrench.def(py::hash(py::self)); wrench.def("__neg__", [](const Wrench &w) { return operator-(w); @@ -237,6 +239,7 @@ void init_frames(py::module &m) twist.def(py::self - py::self); twist.def(py::self == py::self); twist.def(py::self != py::self); + twist.def(py::hash(py::self)); twist.def("__neg__", [](const Twist &a) { return operator-(a); @@ -369,6 +372,7 @@ void init_frames(py::module &m) rotation.def(py::self == py::self); rotation.def(py::self != py::self); rotation.def(py::self * py::self); + rotation.def(py::hash(py::self)); rotation.def(py::pickle( [](const Rotation &rot) { // __getstate__ @@ -450,6 +454,7 @@ void init_frames(py::module &m) frame.def(py::self * py::self); frame.def(py::self == py::self); frame.def(py::self != py::self); + frame.def(py::hash(py::self)); frame.def(py::pickle( [](const Frame &frm) { // __getstate__ From 79ef715e217d0fac2e0468b63a056a8b1f6df57f Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 11:34:12 +0200 Subject: [PATCH 37/78] (PyKDL) add hash to framevel --- python_orocos_kdl/PyKDL/framevel.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/python_orocos_kdl/PyKDL/framevel.cpp b/python_orocos_kdl/PyKDL/framevel.cpp index 1a660278..b8518a06 100644 --- a/python_orocos_kdl/PyKDL/framevel.cpp +++ b/python_orocos_kdl/PyKDL/framevel.cpp @@ -62,7 +62,7 @@ void init_framevel(pybind11::module &m) double_vel.def(py::self == py::self); double_vel.def(py::self != py::self); - + double_vel.def(py::hash(py::self)); double_vel.def("__neg__", [](const doubleVel &a) { return operator-(a); @@ -132,6 +132,7 @@ void init_framevel(pybind11::module &m) vector_vel.def(Vector() != py::self); vector_vel.def(py::self == Vector()); vector_vel.def(py::self != Vector()); + vector_vel.def(py::hash(py::self)); vector_vel.def("__neg__", [](const VectorVel &a) { return operator-(a); @@ -217,6 +218,7 @@ void init_framevel(pybind11::module &m) twist_vel.def(Twist() != py::self); twist_vel.def(py::self == Twist()); twist_vel.def(py::self != Twist()); + twist_vel.def(py::hash(py::self)); twist_vel.def("__neg__", [](const TwistVel &a) { return operator-(a); @@ -305,6 +307,7 @@ void init_framevel(pybind11::module &m) rotation_vel.def(Rotation() != py::self); rotation_vel.def(py::self == Rotation()); rotation_vel.def(py::self != Rotation()); + rotation_vel.def(py::hash(py::self)); rotation_vel.def(py::pickle( [](const RotationVel &rv) { // __getstate__ @@ -378,6 +381,7 @@ void init_framevel(pybind11::module &m) frame_vel.def(Frame() != py::self); frame_vel.def(py::self == Frame()); frame_vel.def(py::self != Frame()); + frame_vel.def(py::hash(py::self)); frame_vel.def(py::pickle( [](const FrameVel &fv) { // __getstate__ From b61a278daa295c1c962474c574521d58c5919828 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 12:02:17 +0200 Subject: [PATCH 38/78] (PyKDL) add hash tests for frames --- python_orocos_kdl/tests/framestest.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index 0e277e18..e374bfe1 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -42,6 +42,10 @@ def testVector(self): self.assertTrue(v != -v) # Doesn't work for zero vector self.assertTrue(not Equal(v, -v)) # Doesn't work for zero vector + # Hash + self.assertEqual(hash(v), 3450679443808348711) + self.assertEqual(hash(Vector()), 11093822414574) + # Test member get and set functions self.assertEqual(v.x(), 3) v.x(1) @@ -89,6 +93,7 @@ def testVectorImpl(self, v): self.assertEqual(v+v+v-2*v, v) v2 = Vector(v) self.assertEqual(v, v2) + self.assertEqual(hash(v), hash(v2)) v2 += v self.assertEqual(2*v, v2) v2 -= v @@ -116,6 +121,10 @@ def testTwist(self): self.assertTrue(t != -t) # Doesn't work for zero twist self.assertTrue(not Equal(t, -t)) # Doesn't work for zero twist + # Hash + self.assertEqual(hash(t), 551895977443887016) + self.assertEqual(hash(Twist()), 730713428471863) + # Members v1 = Vector(1, 2, 3) v2 = Vector(4, 5, 6) @@ -158,6 +167,7 @@ def testTwistImpl(self, t): self.assertEqual(t+t+t-2*t, t) t2 = Twist(t) self.assertEqual(t, t2) + self.assertEqual(hash(t), hash(t2)) t2 += t self.assertEqual(2*t, t2) t2 -= t @@ -190,6 +200,10 @@ def testWrench(self): self.assertTrue(w != -w) # Doesn't work for zero wrench self.assertTrue(not Equal(w, -w)) # Doesn't work for zero wrench + # Hash + self.assertEqual(hash(w), 551895977443887016) + self.assertEqual(hash(Wrench()), 730713428471863) + # Members v1 = Vector(1, 2, 3) v2 = Vector(4, 5, 6) @@ -228,6 +242,7 @@ def testWrenchImpl(self, w): self.assertEqual(w+w+w-2*w, w) w2 = Wrench(w) self.assertEqual(w, w2) + self.assertEqual(hash(w), hash(w2)) w2 += w self.assertEqual(2*w, w2) w2 -= w @@ -246,6 +261,10 @@ def testRotation(self): self.testRotationImpl(Rotation(), Vector(3, 4, 5)) self.testRotationImpl(Rotation(), Vector()) + # Hash + self.assertEqual(hash(Rotation.Quaternion(1, 0, 0, 0)), 5526237894416316219) + self.assertEqual(hash(Rotation()), 8386870752212395617) + r = Rotation(*range(1, 10)) # __getitem__ @@ -290,6 +309,7 @@ def testRotationImpl(self, r, v): self.assertAlmostEqual(dot(r.UnitY(), r.UnitZ()), 0.0, 15) r2 = Rotation(r) self.assertEqual(r, r2) + self.assertEqual(hash(r), hash(r2)) self.assertAlmostEqual((r*v).Norm(), v.Norm(), 14) self.assertEqual(r.Inverse(r*v), v) self.assertEqual(r.Inverse(r*t), t) @@ -339,6 +359,11 @@ def testFrame(self): # Equality f2 = Frame(f) self.assertEqual(f, f2) + self.assertEqual(hash(f), hash(f2)) + + # Hash + self.assertEqual(hash(f), 6112004106257185417) + self.assertEqual(hash(Frame()), 8387572672274540708) # Members self.assertEqual(f.M, r) From c403ff6f450b90b30ec33558a27183a8f66f7ca6 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 12:08:18 +0200 Subject: [PATCH 39/78] (PyKDL) add hash tests for framevel --- python_orocos_kdl/tests/frameveltest.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 614501a2..2f20fa93 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -32,6 +32,7 @@ class FrameVelTestFunctions(unittest.TestCase): def testdoubleVel(self): d = doubleVel() self.assertEqual(doubleVel(d), d) + self.assertEqual(hash(d), hash(doubleVel(d))) self.assertEqual(d.t, 0) self.assertEqual(d.grad, 0) self.assertEqual(d.value(), 0) @@ -65,6 +66,10 @@ def testVectorVel(self): self.assertTrue(v != -v) # Doesn't work for zero VectorVel self.assertTrue(not Equal(v, -v)) # Doesn't work for zero VectorVel + # Hash + self.assertEqual(hash(v), 2049006275376269995) + self.assertEqual(hash(VectorVel()), 730713428471863) + v = VectorVel(v1) self.assertEqual(v, v1) self.assertEqual(v1, v) @@ -88,6 +93,7 @@ def testVectorVelImpl(self, v, vt): self.assertEqual(v+v+v-2*v, v) v2 = VectorVel(v) self.assertEqual(v, v2) + self.assertEqual(hash(v), hash(v2)) v2 += v self.assertEqual(2*v, v2) v2 -= v @@ -135,6 +141,10 @@ def testTwistVel(self): self.assertEqual(t, t2) self.assertEqual(t2, t) + # Hash + self.assertEqual(hash(t), 141466195908239803) + self.assertEqual(hash(TwistVel()), 48409940491385244) + # Zero SetToZero(t) self.assertEqual(t, TwistVel()) @@ -146,6 +156,7 @@ def testTwistVelImpl(self, t): self.assertEqual(t+t+t-2*t, t) t2 = TwistVel(t) self.assertEqual(t, t2) + self.assertEqual(hash(t), hash(t2)) t2 += t self.assertEqual(2*t, t2) t2 -= t @@ -180,9 +191,14 @@ def testRotationVel(self): self.assertEqual(RotationVel(rot), rot) self.assertEqual(rot, RotationVel(rot)) + # Hash + self.assertEqual(hash(r), 6921222406909663069) + self.assertEqual(hash(RotationVel()), 4775665235973850935) + def testRotationVelImpl(self, r, v, vt): r2 = RotationVel(r) self.assertEqual(r, r2) + self.assertEqual(hash(r), hash(r2)) self.assertEqual((r*v).Norm(), v.Norm()) self.assertEqual(r.Inverse(r*v), v) self.assertEqual(r*r.Inverse(v), v) @@ -231,9 +247,14 @@ def testFrameVel(self): self.assertEqual(f, fr) self.assertEqual(fr, f) + # Hash + self.assertEqual(hash(fr), 6112004106257185417) + self.assertEqual(hash(FrameVel()), 35564562501293795) + def testFrameVelImpl(self, f, v, vt): f2 = FrameVel(f) self.assertEqual(f, f2) + self.assertEqual(hash(f), hash(f2)) self.assertEqual(f.Inverse(f*v), v) self.assertEqual(f.Inverse(f*vt), vt) self.assertEqual(f*f.Inverse(v), v) From 0fd60ef6306ba0af55bd2a4d66980616a181c8de Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 13:23:56 +0200 Subject: [PATCH 40/78] (PyKDL) sync twist test to cpp --- python_orocos_kdl/tests/framestest.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index e374bfe1..f72c9452 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -115,14 +115,14 @@ def testTwist(self): self.testTwistImpl(t) # Equality - t = Twist(Vector(1, 2, 3), Vector(1, 2, 3)) + t = Twist(Vector(0, -9, -3), Vector(1, -2, -4)) 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 # Hash - self.assertEqual(hash(t), 551895977443887016) + self.assertEqual(hash(t), 3373832976806748309) self.assertEqual(hash(Twist()), 730713428471863) # Members From 36de6310f7c6a90d6b983615c026ea105be526fd Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 15:16:03 +0200 Subject: [PATCH 41/78] (PyKDL) sync wrench test to cpp --- python_orocos_kdl/tests/framestest.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index f72c9452..7f4324e8 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -194,7 +194,6 @@ def testWrench(self): 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 From 58f2e4eece8ab2989351004c998a50b4e9f90642 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 15:18:09 +0200 Subject: [PATCH 42/78] (PyKDL) fix wrench hash test This shows that the python wrapper is doing something crazy with the return value --- python_orocos_kdl/tests/framestest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index 7f4324e8..c6927708 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -200,7 +200,7 @@ def testWrench(self): self.assertTrue(not Equal(w, -w)) # Doesn't work for zero wrench # Hash - self.assertEqual(hash(w), 551895977443887016) + self.assertEqual(hash(w), 13897938943539516747) self.assertEqual(hash(Wrench()), 730713428471863) # Members From 6a329bb17eb2d4e3fdadb338507dfa5623e427a2 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 15:19:59 +0200 Subject: [PATCH 43/78] (KDL) add hash tests for frames --- orocos_kdl/tests/framestest.cpp | 26 +++++++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index d217ba5a..d2c53406 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -21,6 +21,7 @@ void FramesTest::TestVector2(Vector& v) { CPPUNIT_ASSERT_EQUAL(v+v+v-2*v,v); v2=v; CPPUNIT_ASSERT_EQUAL(v,v2); + CPPUNIT_ASSERT_EQUAL(std::hash{}(v), std::hash{}(v2)); v2+=v; CPPUNIT_ASSERT_EQUAL(2*v,v2); v2-=v; @@ -34,6 +35,9 @@ void FramesTest::TestVector() { TestVector2(v); Vector v2(0,0,0); TestVector2(v2); + + CPPUNIT_ASSERT_EQUAL(std::hash{}(v), static_cast(3450679443808348711)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(v2), static_cast(11093822414574)); Vector nu(0,0,0); CPPUNIT_ASSERT_EQUAL(nu.Norm(),0.0); @@ -59,11 +63,12 @@ void FramesTest::TestTwist2(Twist& t) { CPPUNIT_ASSERT_EQUAL(t+t+t-2*t,t); t2=t; CPPUNIT_ASSERT_EQUAL(t,t2); + CPPUNIT_ASSERT_EQUAL(std::hash{}(t), std::hash{}(t2)); t2+=t; CPPUNIT_ASSERT_EQUAL(2*t,t2); t2-=t; CPPUNIT_ASSERT_EQUAL(t,t2); - t.ReverseSign(); + t2.ReverseSign(); CPPUNIT_ASSERT_EQUAL(t,-t2); } @@ -74,6 +79,9 @@ void FramesTest::TestTwist() { TestTwist2(t2); Twist t3(Vector(0,-9,-3),Vector(1,-2,-4)); TestTwist2(t3); + + CPPUNIT_ASSERT_EQUAL(std::hash{}(t3), static_cast(3373832976806748309)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(t2), static_cast(730713428471863)); } void FramesTest::TestWrench2(Wrench& w) { @@ -84,11 +92,12 @@ void FramesTest::TestWrench2(Wrench& w) { CPPUNIT_ASSERT_EQUAL(w+w+w-2*w,w); w2=w; CPPUNIT_ASSERT_EQUAL(w,w2); + CPPUNIT_ASSERT_EQUAL(std::hash{}(w), std::hash{}(w2)); w2+=w; CPPUNIT_ASSERT_EQUAL(2*w,w2); w2-=w; CPPUNIT_ASSERT_EQUAL(w,w2); - w.ReverseSign(); + w2.ReverseSign(); CPPUNIT_ASSERT_EQUAL(w,-w2); } @@ -98,7 +107,10 @@ void FramesTest::TestWrench() { Wrench w2(Vector(0,0,0),Vector(0,0,0)); TestWrench2(w2); Wrench w3(Vector(2,1,4),Vector(5,3,1)); - TestWrench2(w3); + TestWrench2(w3); + + CPPUNIT_ASSERT_EQUAL(std::hash{}(w3), static_cast(13897938943539516747)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(w2), static_cast(730713428471863)); } void FramesTest::TestRotation2(const Vector& v,double a,double b,double c) { @@ -122,6 +134,7 @@ void FramesTest::TestRotation2(const Vector& v,double a,double b,double c) { CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitY(),R.UnitZ()),0.0,epsilon); R2=R; CPPUNIT_ASSERT_EQUAL(R,R2); + CPPUNIT_ASSERT_EQUAL(std::hash{}(R), std::hash{}(R2)); CPPUNIT_ASSERT_DOUBLES_EQUAL((R*v).Norm(),v.Norm(),epsilon); CPPUNIT_ASSERT_EQUAL(R.Inverse(R*v),v); CPPUNIT_ASSERT_EQUAL(R.Inverse(R*t),t); @@ -406,6 +419,9 @@ void FramesTest::TestRotation() { TestOneRotation("rot([-1,-1,-1],-180)", KDL::Rotation::Rot(KDL::Vector(-1,-1,-1),-180*deg2rad), 180*deg2rad, Vector(1,1,1)/sqrt(3.0)); TestOneRotation("rot([0.707107, 0, 0.707107", KDL::Rotation::RPY(-2.9811968953315162, -atan(1)*2, -0.1603957582582825), 180*deg2rad, Vector(0.707107,0,0.707107) ); + + CPPUNIT_ASSERT_EQUAL(std::hash{}(Rotation::Quaternion(1, 0, 0, 0)), static_cast(5526237894416316219)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(Rotation()), static_cast(8386870752212395617)); } void FramesTest::TestGetRotAngle() { @@ -619,6 +635,7 @@ void FramesTest::TestFrame() { F = Frame(Rotation::EulerZYX(10*deg2rad,20*deg2rad,-10*deg2rad),Vector(4,-2,1)); F2=F ; CPPUNIT_ASSERT_EQUAL(F,F2); + CPPUNIT_ASSERT_EQUAL(std::hash{}(F), std::hash{}(F2)); CPPUNIT_ASSERT_EQUAL(F.Inverse(F*v),v); CPPUNIT_ASSERT_EQUAL(F.Inverse(F*t),t); CPPUNIT_ASSERT_EQUAL(F.Inverse(F*w),w); @@ -648,6 +665,9 @@ void FramesTest::TestFrame() { Vector(0, -0.36, 0)); CPPUNIT_ASSERT(Equal(Frame::DH_Craig1989(0.0, M_PI_2, 0.36, 0.0), F_DH_Craig1989)); CPPUNIT_ASSERT(Equal(Frame().DH_Craig1989(0.0, M_PI_2, 0.36, 0.0), F_DH_Craig1989)); // Only for testing purposes, shouldn't use static function of instances + + CPPUNIT_ASSERT_EQUAL(std::hash{}(F), static_cast(6112004106257185417)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(Frame()), static_cast(8387572672274540708)); } JntArray CreateRandomJntArray(int size) From 7dd2e45ce47d6b734a0bf29ea83fcc53747d249c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 16:09:09 +0200 Subject: [PATCH 44/78] Revert "(PyKDL) fix wrench hash test" This reverts commit 69e7829f3d674c4f454762e295d136f50dc8675e. Python truncates hashes based on the system bit width, see https://docs.python.org/3/library/functions.html#hash --- python_orocos_kdl/tests/framestest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index c6927708..7f4324e8 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -200,7 +200,7 @@ def testWrench(self): self.assertTrue(not Equal(w, -w)) # Doesn't work for zero wrench # Hash - self.assertEqual(hash(w), 13897938943539516747) + self.assertEqual(hash(w), 551895977443887016) self.assertEqual(hash(Wrench()), 730713428471863) # Members From 652cd9daa153a9627f4e29c46dc84d850f4ac2c3 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 16:36:41 +0200 Subject: [PATCH 45/78] (PyKDL) Fix wrench hash test, let it match the cpp value --- python_orocos_kdl/tests/framestest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index 7f4324e8..cd6abe1a 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -200,7 +200,7 @@ def testWrench(self): self.assertTrue(not Equal(w, -w)) # Doesn't work for zero wrench # Hash - self.assertEqual(hash(w), 551895977443887016) + self.assertEqual(hash(w), hash(13897938943539516747)) # 551895977443887016 self.assertEqual(hash(Wrench()), 730713428471863) # Members From fa1492a36355b42b511e838650a431b8c272e33e Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 11 May 2022 17:07:22 +0200 Subject: [PATCH 46/78] (KDL) hash tests force unsigned int in values --- orocos_kdl/tests/framestest.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp index d2c53406..2a3ab4a6 100644 --- a/orocos_kdl/tests/framestest.cpp +++ b/orocos_kdl/tests/framestest.cpp @@ -36,8 +36,8 @@ void FramesTest::TestVector() { Vector v2(0,0,0); TestVector2(v2); - CPPUNIT_ASSERT_EQUAL(std::hash{}(v), static_cast(3450679443808348711)); - CPPUNIT_ASSERT_EQUAL(std::hash{}(v2), static_cast(11093822414574)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(v), static_cast(3450679443808348711u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(v2), static_cast(11093822414574u)); Vector nu(0,0,0); CPPUNIT_ASSERT_EQUAL(nu.Norm(),0.0); @@ -80,8 +80,8 @@ void FramesTest::TestTwist() { Twist t3(Vector(0,-9,-3),Vector(1,-2,-4)); TestTwist2(t3); - CPPUNIT_ASSERT_EQUAL(std::hash{}(t3), static_cast(3373832976806748309)); - CPPUNIT_ASSERT_EQUAL(std::hash{}(t2), static_cast(730713428471863)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(t3), static_cast(3373832976806748309u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(t2), static_cast(730713428471863u)); } void FramesTest::TestWrench2(Wrench& w) { @@ -109,8 +109,8 @@ void FramesTest::TestWrench() { Wrench w3(Vector(2,1,4),Vector(5,3,1)); TestWrench2(w3); - CPPUNIT_ASSERT_EQUAL(std::hash{}(w3), static_cast(13897938943539516747)); - CPPUNIT_ASSERT_EQUAL(std::hash{}(w2), static_cast(730713428471863)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(w3), static_cast(13897938943539516747u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(w2), static_cast(730713428471863u)); } void FramesTest::TestRotation2(const Vector& v,double a,double b,double c) { @@ -420,8 +420,8 @@ void FramesTest::TestRotation() { TestOneRotation("rot([0.707107, 0, 0.707107", KDL::Rotation::RPY(-2.9811968953315162, -atan(1)*2, -0.1603957582582825), 180*deg2rad, Vector(0.707107,0,0.707107) ); - CPPUNIT_ASSERT_EQUAL(std::hash{}(Rotation::Quaternion(1, 0, 0, 0)), static_cast(5526237894416316219)); - CPPUNIT_ASSERT_EQUAL(std::hash{}(Rotation()), static_cast(8386870752212395617)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(Rotation::Quaternion(1, 0, 0, 0)), static_cast(5526237894416316219u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(Rotation()), static_cast(8386870752212395617u)); } void FramesTest::TestGetRotAngle() { @@ -666,8 +666,8 @@ void FramesTest::TestFrame() { CPPUNIT_ASSERT(Equal(Frame::DH_Craig1989(0.0, M_PI_2, 0.36, 0.0), F_DH_Craig1989)); CPPUNIT_ASSERT(Equal(Frame().DH_Craig1989(0.0, M_PI_2, 0.36, 0.0), F_DH_Craig1989)); // Only for testing purposes, shouldn't use static function of instances - CPPUNIT_ASSERT_EQUAL(std::hash{}(F), static_cast(6112004106257185417)); - CPPUNIT_ASSERT_EQUAL(std::hash{}(Frame()), static_cast(8387572672274540708)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(F), static_cast(6112004106257185417u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(Frame()), static_cast(8387572672274540708u)); } JntArray CreateRandomJntArray(int size) From eb38dbb99be12649955dd68ef62f02028d6c753c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Thu, 12 May 2022 11:30:05 +0200 Subject: [PATCH 47/78] (PyKDL) hash produces different values in python2 --- python_orocos_kdl/tests/frameveltest.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 2f20fa93..7503852d 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -67,7 +67,11 @@ def testVectorVel(self): self.assertTrue(not Equal(v, -v)) # Doesn't work for zero VectorVel # Hash - self.assertEqual(hash(v), 2049006275376269995) + if sys.version_info < (3, 0): + self.assertEqual(hash(v), -4868522752264811866) + else: + self.assertEqual(hash(v), 2049006275376269995) + self.assertEqual(hash(VectorVel()), 730713428471863) v = VectorVel(v1) @@ -249,7 +253,10 @@ def testFrameVel(self): # Hash self.assertEqual(hash(fr), 6112004106257185417) - self.assertEqual(hash(FrameVel()), 35564562501293795) + if sys.version_info < (3, 0): + self.assertEqual(hash(FrameVel()), -2270278446712400164) + else: + self.assertEqual(hash(FrameVel()), 35564562501293795) def testFrameVelImpl(self, f, v, vt): f2 = FrameVel(f) From c42f23c747e6529f7c84cf4a710fe903adb7412d Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 29 May 2022 13:44:06 +0200 Subject: [PATCH 48/78] (Joint) make thread safe Keep the class variables for ABI compatibility --- orocos_kdl/src/joint.cpp | 31 +++++++++---------------------- orocos_kdl/src/joint.hpp | 6 ++---- 2 files changed, 11 insertions(+), 26 deletions(-) diff --git a/orocos_kdl/src/joint.cpp b/orocos_kdl/src/joint.cpp index b8481e30..e27bf251 100644 --- a/orocos_kdl/src/joint.cpp +++ b/orocos_kdl/src/joint.cpp @@ -28,8 +28,8 @@ namespace KDL { const double& _inertia, const double& _damping, const double& _stiffness): name(_name),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) { - if (type == RotAxis || type == TransAxis) throw joint_type_ex; - q_previous = 0; + if (type == RotAxis || type == TransAxis) + throw joint_type_ex; } // constructor for joint along x,y or z axis, at origin of reference frame @@ -37,8 +37,8 @@ namespace KDL { const double& _inertia, const double& _damping, const double& _stiffness): name("NoName"),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) { - if (type == RotAxis || type == TransAxis) throw joint_type_ex; - q_previous = 0; + if (type == RotAxis || type == TransAxis) + throw joint_type_ex; } // constructor for joint along arbitrary axis, at arbitrary origin @@ -47,12 +47,8 @@ namespace KDL { name(_name), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) , axis(_axis / _axis.Norm()), origin(_origin) { - if (type != RotAxis && type != TransAxis) throw joint_type_ex; - - // initialize - joint_pose.p = origin; - joint_pose.M = Rotation::Rot2(axis, offset); - q_previous = 0; + if (type != RotAxis && type != TransAxis) + throw joint_type_ex; } // constructor for joint along arbitrary axis, at arbitrary origin @@ -61,12 +57,8 @@ namespace KDL { name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness), axis(_axis / _axis.Norm()),origin(_origin) { - if (type != RotAxis && type != TransAxis) throw joint_type_ex; - - // initialize - joint_pose.p = origin; - joint_pose.M = Rotation::Rot2(axis, offset); - q_previous = 0; + if (type != RotAxis && type != TransAxis) + throw joint_type_ex; } Joint::~Joint() @@ -77,12 +69,7 @@ namespace KDL { { switch(type){ case RotAxis: - // calculate the rotation matrix around the vector "axis" - if (q != q_previous){ - q_previous = q; - joint_pose.M = Rotation::Rot2(axis, scale*q+offset); - } - return joint_pose; + return Frame(Rotation::Rot2(axis, scale*q+offset), origin); case RotX: return Frame(Rotation::RotX(scale*q+offset)); case RotY: diff --git a/orocos_kdl/src/joint.hpp b/orocos_kdl/src/joint.hpp index cc2e939b..86822d89 100644 --- a/orocos_kdl/src/joint.hpp +++ b/orocos_kdl/src/joint.hpp @@ -235,10 +235,8 @@ namespace KDL { // variables for RotAxis joint Vector axis, origin; - mutable Frame joint_pose; - mutable double q_previous; - - + mutable Frame joint_pose; // Deprecated, but keeping for ABI compatibility + mutable double q_previous; // Deprecated, but keeping for ABI compatibility class joint_type_exception: public std::exception{ virtual const char* what() const throw(){ From 909aa555bb8cd16a2fdead752ef390eb0ef4019c Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 9 Jun 2022 10:16:28 +0200 Subject: [PATCH 49/78] Bump actions/setup-python from 3 to 4 (#400) Bumps [actions/setup-python](https://github.com/actions/setup-python) from 3 to 4. - [Release notes](https://github.com/actions/setup-python/releases) - [Commits](https://github.com/actions/setup-python/compare/v3...v4) --- updated-dependencies: - dependency-name: actions/setup-python dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index ac85a647..66f746d1 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -37,7 +37,7 @@ jobs: - uses: actions/checkout@v3 with: submodules: recursive - - uses: actions/setup-python@v3 + - uses: actions/setup-python@v4 with: python-version: ${{ matrix.python_version }} - name: Install From f94cc0bef3720012b0e35fd39b42a60c1ef53842 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 30 Aug 2022 20:21:19 +0200 Subject: [PATCH 50/78] (README) no noetic release --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 2a4aabd1..4c18ea7e 100644 --- a/README.md +++ b/README.md @@ -16,3 +16,5 @@ The python bindings, are located in the `python_orocos_kdl` folder. The installa 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. From eecefd1028ac3e1b8c1235c4c45dd0884ac75c03 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 25 Oct 2022 18:42:30 +0200 Subject: [PATCH 51/78] (actions) allow manual trigger --- .github/workflows/main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 66f746d1..617b4d72 100644 --- a/.github/workflows/main.yml +++ b/.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" From 4cc7cf946165e7a9e8cc987063c2ae1388695822 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 23 Nov 2022 22:39:40 +0100 Subject: [PATCH 52/78] Drop python2 compatibility (#418) * Bump python_orocos_kdl/pybind11 from `914c06f` to `5b63222` Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `914c06f` to `5b63222`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/914c06fb252b6cc3727d0eedab6736e88a3fcb01...5b632229a9f7d4bc8a2ac112a40249eb03b4e7f2) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] * [PyKDL](package.xml) drop python2 compatibility * [PyKDL](kinfam) drop python2 compatibility * (actions) drop python2 compatibility * [PyKDL](PyKDLtest) drop python2 compatibility * [PyKDL](frameveltest) drop python2 compatibility * Delete jointtypetest.py * [PyKDL](test) move import * [PyKDL](frameveltest) drop python2 compatibility * [PyKDL](install) drop python2 support Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: Matthijs van der Burgh --- .github/workflows/main.yml | 16 -------- python_orocos_kdl/INSTALL.md | 13 ++----- python_orocos_kdl/PyKDL/kinfam.cpp | 3 -- python_orocos_kdl/package.xml | 12 ++---- python_orocos_kdl/pybind11 | 2 +- python_orocos_kdl/tests/PyKDLtest.py | 7 +--- python_orocos_kdl/tests/frameveltest.py | 17 ++------ python_orocos_kdl/tests/jointtypetest.py | 49 ------------------------ python_orocos_kdl/tests/kinfamtest.py | 2 +- 9 files changed, 14 insertions(+), 107 deletions(-) delete mode 100644 python_orocos_kdl/tests/jointtypetest.py diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 617b4d72..313aa010 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -17,10 +17,6 @@ jobs: compiler: [gcc, clang] python_version: ['3.8'] include: - - os: ubuntu-18.04 - orocos_build_type: Release - compiler: gcc - python_version: '2' - os: ubuntu-20.04 orocos_build_type: Release compiler: gcc @@ -80,18 +76,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 diff --git a/python_orocos_kdl/INSTALL.md b/python_orocos_kdl/INSTALL.md index f95f10ac..becd791b 100644 --- a/python_orocos_kdl/INSTALL.md +++ b/python_orocos_kdl/INSTALL.md @@ -5,12 +5,8 @@ These install instructions are focused on Debian/Ubuntu systems. ## Shared instructions 1. Follow the shared instructions of the C++ library from [orocos_kdl/INSTALL.md](../orocos_kdl/INSTALL.md#shared-instructions) -2. Depending on your python version install the `future` and `psutil` module - - Python 2: `sudo apt-get install python-psutil python-future` - - Python 3: `sudo apt-get install python3-psutil python3-future` -3. (Optional) Install `Sphinx` to generate API-documentation - - Python 2: `sudo apt-get install python-sphinx` - - Python 3: `sudo apt-get install python3-sphinx` +2. Install the `future` and `psutil` module: `sudo apt-get install python3-psutil python3-future` +3. (Optional) Install `Sphinx` to generate API-documentation: `sudo apt-get install python3-sphinx` ## Compilation @@ -33,15 +29,12 @@ These install instructions are focused on Debian/Ubuntu systems. 6. Execute cmake: `cmake ..` - (Optional) Adapt `CMAKE_INSTALL_PREFIX` to the desired installation directory - (Optional) To change the build type, add: `-DCMAKE_BUILD_TYPE=` - - (Optional) To change the python version (default=3), set `ROS_PYTHON_VERSION` environment variable to either `2` or `3`. 7. Compile: `make` 8. Install the python bindings: `sudo make install` 9. Make sure `LD_LIBRARY_PATH` is set correctly: `export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib`. Add this also to your `.bashrc`. 10. Execute `ldconfig`: `sudo ldconfig` -11. (Optional) Execute tests: - - Python 2: `python2 ../tests/PyKDLtest.py` - - Python 3: `python3 ../tests/PyKDLtest.py` +11. (Optional) Execute tests: `python3 ../tests/PyKDLtest.py` 12. (Optional) To create the API-documentation: `sphinx-build ../doc docs`. The API-documentation will be generated at `/docs`. diff --git a/python_orocos_kdl/PyKDL/kinfam.cpp b/python_orocos_kdl/PyKDL/kinfam.cpp index 7b302c45..3e6c5d2d 100644 --- a/python_orocos_kdl/PyKDL/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/kinfam.cpp @@ -66,9 +66,6 @@ void init_kinfam(pybind11::module &m) 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<>()); diff --git a/python_orocos_kdl/package.xml b/python_orocos_kdl/package.xml index d28c64b0..89ae3400 100644 --- a/python_orocos_kdl/package.xml +++ b/python_orocos_kdl/package.xml @@ -18,19 +18,15 @@ orocos_kdl - python - python3 + python3 catkin orocos_kdl - python-future - python3-future - python-psutil - python3-psutil + python3-future + python3-psutil - python-sphinx - python3-sphinx + python3-sphinx diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index 914c06fb..5b632229 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit 914c06fb252b6cc3727d0eedab6736e88a3fcb01 +Subproject commit 5b632229a9f7d4bc8a2ac112a40249eb03b4e7f2 diff --git a/python_orocos_kdl/tests/PyKDLtest.py b/python_orocos_kdl/tests/PyKDLtest.py index 2a1d0e2b..af5e80e3 100644 --- a/python_orocos_kdl/tests/PyKDLtest.py +++ b/python_orocos_kdl/tests/PyKDLtest.py @@ -28,19 +28,14 @@ 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()) -if sys.version_info < (3, 0): - import jointtypetest - suite.addTest(jointtypetest.suite()) - if __name__ == "__main__": + import sys result = unittest.TextTestRunner(verbosity=3).run(suite) if result.wasSuccessful(): diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 7503852d..c6ffede1 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -24,7 +24,6 @@ from math import radians from PyKDL import * -import sys import unittest @@ -67,10 +66,7 @@ def testVectorVel(self): self.assertTrue(not Equal(v, -v)) # Doesn't work for zero VectorVel # Hash - if sys.version_info < (3, 0): - self.assertEqual(hash(v), -4868522752264811866) - else: - self.assertEqual(hash(v), 2049006275376269995) + self.assertEqual(hash(v), 2049006275376269995) self.assertEqual(hash(VectorVel()), 730713428471863) @@ -253,10 +249,7 @@ def testFrameVel(self): # Hash self.assertEqual(hash(fr), 6112004106257185417) - if sys.version_info < (3, 0): - self.assertEqual(hash(FrameVel()), -2270278446712400164) - else: - self.assertEqual(hash(FrameVel()), 35564562501293795) + self.assertEqual(hash(FrameVel()), 35564562501293795) def testFrameVelImpl(self, f, v, vt): f2 = FrameVel(f) @@ -275,10 +268,7 @@ def testFrameVelImpl(self, f, v, vt): self.assertEqual(f.Inverse()*vt, f.Inverse(vt)) def testPickle(self): - if sys.version_info < (3, 0): - import cPickle as pickle - else: - import pickle + import pickle data = {} 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)) @@ -330,6 +320,7 @@ def suite(): if __name__ == '__main__': + import sys suite = suite() result = unittest.TextTestRunner(verbosity=3).run(suite) diff --git a/python_orocos_kdl/tests/jointtypetest.py b/python_orocos_kdl/tests/jointtypetest.py deleted file mode 100644 index dfe1e240..00000000 --- a/python_orocos_kdl/tests/jointtypetest.py +++ /dev/null @@ -1,49 +0,0 @@ -# Copyright (C) 2007 Ruben Smits - -# Version: 1.0 -# Author: Matthijs van der Burgh -# Maintainer: Ruben Smits -# Maintainer: Matthijs van der Burgh -# 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 5fb4fe9b..9300bea4 100644 --- a/python_orocos_kdl/tests/kinfamtest.py +++ b/python_orocos_kdl/tests/kinfamtest.py @@ -28,7 +28,6 @@ import psutil from PyKDL import * import random -import sys import unittest @@ -364,6 +363,7 @@ def suite(): if __name__ == '__main__': + import sys suite = suite() result = unittest.TextTestRunner(verbosity=3).run(suite) From 9a35f0adadef132ffb7ccff35e898e6a0951c038 Mon Sep 17 00:00:00 2001 From: Masa0u0 <66519864+Masa0u0@users.noreply.github.com> Date: Mon, 28 Nov 2022 02:38:48 +0900 Subject: [PATCH 53/78] Add accessors (#419) * edit: joint.hpp add accessors of scale and offset * edit: segment.hpp add accessor of f_tip * edit: rigidbodyinertia.hpp add accessor of h * edit: rigidbodyinertia.hpp change return types of getMass() and getRotationalInertia() * edit: TreeFkSolverPos_recursive,TreeJntToJacSolver privat variale this->tree to a const reference * edit: rigidbodyinertia.hpp const double& getMass() -> double getMass() * (joint) Update return types and docstrings * Revert class member type change Not ABI compatible * (joint) change return types * Revert class member ref change Not ABI compatible * (RigidBodyInertia) revert return type change Not ABI compatible * Add missing docstring * edit : segment.hpp getFrameToTipRaw -> getFrameToTipZero add docstring to get FrameToTipZero * edit: segment.hpp add docstring to get FrameToTipZero Co-authored-by: Matthijs van der Burgh --- orocos_kdl/src/joint.hpp | 40 +++++++++++++++++++++++++++++ orocos_kdl/src/rigidbodyinertia.hpp | 10 +++++++- orocos_kdl/src/segment.hpp | 12 +++++++++ 3 files changed, 61 insertions(+), 1 deletion(-) diff --git a/orocos_kdl/src/joint.hpp b/orocos_kdl/src/joint.hpp index 86822d89..c7fccd09 100644 --- a/orocos_kdl/src/joint.hpp +++ b/orocos_kdl/src/joint.hpp @@ -192,6 +192,26 @@ namespace KDL { } }; + /** + * Request the scale of the joint. + * + * @return const reference to the scale of the joint + */ + const double& getScale() const + { + return scale; + } + + /** + * Request the offset of the joint. + * + * @return const reference to the offset of the joint + */ + const double& getOffset() const + { + return offset; + } + /** * Request the inertia of the joint. * @@ -222,6 +242,26 @@ namespace KDL { return stiffness; }; + /** + * Request the axis of the joint. + * + * @return const reference to the axis of the joint + */ + const Vector& getAxis() const + { + return axis; + } + + /** + * Request the origin of the joint. + * + * @return const reference to the origin of the joint + */ + const Vector& getOrigin() const + { + return origin; + } + virtual ~Joint(); private: diff --git a/orocos_kdl/src/rigidbodyinertia.hpp b/orocos_kdl/src/rigidbodyinertia.hpp index 8992c4f9..42fc5d0f 100644 --- a/orocos_kdl/src/rigidbodyinertia.hpp +++ b/orocos_kdl/src/rigidbodyinertia.hpp @@ -71,7 +71,15 @@ namespace KDL { double getMass() const{ return m; }; - + + /** + * Get the spatial momentum of the rigid body + */ + const Vector& getSpatialMomentum() const + { + return h; + } + /** * Get the center of gravity of the rigid body */ diff --git a/orocos_kdl/src/segment.hpp b/orocos_kdl/src/segment.hpp index be561736..80007636 100644 --- a/orocos_kdl/src/segment.hpp +++ b/orocos_kdl/src/segment.hpp @@ -159,6 +159,18 @@ namespace KDL { * @param f_tip_new pose from the joint end to the tip of the segment */ void setFrameToTip(const Frame& f_tip_new); + + /** + * Request the pose from the end of the joint to the tip of the segment + * at joint position 0. + * + * @return const reference to the pose from the end of the joint to the tip of the segment + * at joint position 0 + */ + const Frame& getFrameToTipZero() const + { + return f_tip; + } }; }//end of namespace KDL From e625691dc5d09edd6f2c145d69459fa2b3e9c207 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 14 Dec 2022 21:03:29 +0100 Subject: [PATCH 54/78] (PyKDL) use FindPython (#425) --- python_orocos_kdl/CMakeLists.txt | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index 916be8fb..1fa965b2 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.12.0) if(POLICY CMP0048) cmake_policy(SET CMP0048 NEW) endif() @@ -25,11 +25,10 @@ endif() set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION} CACHE STRING "Python version used by PyBind11") -find_package(PythonInterp ${PYTHON_VERSION} REQUIRED) -find_package(PythonLibs ${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR} REQUIRED) +find_package(Python ${PYTHON_VERSION} COMPONENTS Interpreter Development REQUIRED) # 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") # This might be overridden below if built with catkin. +set(PYTHON_SITE_PACKAGES_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/python${Python_VERSION_MAJOR}.${Python_VERSION_MINOR}/dist-packages") # This might be overridden below if built with catkin. set(LIBRARY_NAME "PyKDL") # catkin-specific configuration (optional) From f322f31a94190e056f13d4ead5c506717b2bf90f Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 21 Dec 2022 10:18:10 +0100 Subject: [PATCH 55/78] (actions) test python 3.11 (#426) --- .github/workflows/main.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 313aa010..5fb9c9ff 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -25,6 +25,10 @@ jobs: orocos_build_type: Release compiler: gcc python_version: '3.10' + - os: ubuntu-20.04 + orocos_build_type: Release + compiler: gcc + python_version: '3.11' env: CC: ${{ matrix.compiler }} OROCOS_BUILD_TYPE: ${{ matrix.orocos_build_type }} From 89e26e0e6acc0f4d8e8a54710232fd2ed80da7b4 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 21 Dec 2022 23:06:00 +0100 Subject: [PATCH 56/78] Bump python_orocos_kdl/pybind11 from `5b63222` to `d36a41e` (#427) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `5b63222` to `d36a41e`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/5b632229a9f7d4bc8a2ac112a40249eb03b4e7f2...d36a41ea98b070761eec8384656378dfd4f415d9) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index 5b632229..d36a41ea 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit 5b632229a9f7d4bc8a2ac112a40249eb03b4e7f2 +Subproject commit d36a41ea98b070761eec8384656378dfd4f415d9 From 8bb71073267730dd2faafe2fc7185b25d0ef1aec Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 3 Jan 2023 23:47:11 +0100 Subject: [PATCH 57/78] Bump python_orocos_kdl/pybind11 from `d36a41e` to `12852cd` (#429) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `d36a41e` to `12852cd`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/d36a41ea98b070761eec8384656378dfd4f415d9...12852cd33bdaae0fe5fecc0db39b788dbbcc7b3d) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index d36a41ea..12852cd3 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit d36a41ea98b070761eec8384656378dfd4f415d9 +Subproject commit 12852cd33bdaae0fe5fecc0db39b788dbbcc7b3d From 29e95227c4aeec52f5ad8cad4d8b07cefaa4bc2c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 7 Jan 2023 15:36:00 +0100 Subject: [PATCH 58/78] (PyKDL) use minimal pybind11 2.6 --- python_orocos_kdl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index 1fa965b2..64fdd215 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -49,7 +49,7 @@ else() set(PYTHON_MODULE_EXTENSION ".so") endif() -find_package(pybind11 QUIET) +find_package(pybind11 2.6 QUIET) if(NOT ${pybind11_FOUND}) message(STATUS "pybind11 not found, building from source") add_subdirectory(pybind11) From c369c545ccbe30ddc00ed11337b7b265413fa708 Mon Sep 17 00:00:00 2001 From: luzpaz Date: Wed, 25 Jan 2023 03:52:41 +0000 Subject: [PATCH 59/78] fix typos --- orocos_kdl/examples/chainiksolverpos_lma_demo.cpp | 2 +- orocos_kdl/src/chainexternalwrenchestimator.hpp | 2 +- orocos_kdl/src/chainhdsolver_vereshchagin.hpp | 2 +- orocos_kdl/src/path_roundedcomposite.hpp | 2 +- orocos_kdl/src/utilities/ldl_solver_eigen.hpp | 2 +- orocos_kdl/tests/solvertest.cpp | 14 +++++++------- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp index 5697b250..353ea71c 100644 --- a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp +++ b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp @@ -177,7 +177,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"; diff --git a/orocos_kdl/src/chainexternalwrenchestimator.hpp b/orocos_kdl/src/chainexternalwrenchestimator.hpp index c60b882d..3147a5df 100644 --- a/orocos_kdl/src/chainexternalwrenchestimator.hpp +++ b/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); diff --git a/orocos_kdl/src/chainhdsolver_vereshchagin.hpp b/orocos_kdl/src/chainhdsolver_vereshchagin.hpp index 14179f39..86be6471 100644 --- a/orocos_kdl/src/chainhdsolver_vereshchagin.hpp +++ b/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 * diff --git a/orocos_kdl/src/path_roundedcomposite.hpp b/orocos_kdl/src/path_roundedcomposite.hpp index 2ccb860a..a7451359 100644 --- a/orocos_kdl/src/path_roundedcomposite.hpp +++ b/orocos_kdl/src/path_roundedcomposite.hpp @@ -88,7 +88,7 @@ class Path_RoundedComposite : public Path Path_RoundedComposite(double radius,double eqradius,RotationalInterpolation* orient, bool aggregate=true); /** - * Adds a point to this rounded composite, between to adjecent points + * Adds a point to this rounded composite, between two adjacent points * a Path_Line will be created, between two lines there will be * rounding with the given radius with a Path_Circle * diff --git a/orocos_kdl/src/utilities/ldl_solver_eigen.hpp b/orocos_kdl/src/utilities/ldl_solver_eigen.hpp index f54aa947..4153bd32 100644 --- a/orocos_kdl/src/utilities/ldl_solver_eigen.hpp +++ b/orocos_kdl/src/utilities/ldl_solver_eigen.hpp @@ -37,7 +37,7 @@ namespace KDL * * 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 + * to be computed without explicitly 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: diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index 24bde309..c42c8b69 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -922,12 +922,12 @@ void SolverTest::VereshchaginTest() Twist unit_force_x_a( Vector(0.0, 0.0, 0.0), Vector(0.0, 0.0, 0.0)); - alpha_unit_force.setColumn(3, unit_force_x_a); // constraint diabled... In this direction, end-effector's motion is left to emerge naturally + alpha_unit_force.setColumn(3, unit_force_x_a); // constraint disabled... In this direction, end-effector's motion is left to emerge naturally Twist unit_force_y_a( Vector(0.0, 0.0, 0.0), Vector(0.0, 0.0, 0.0)); - alpha_unit_force.setColumn(4, unit_force_y_a); // constraint diabled... In this direction, end-effector's motion is left to emerge naturally + alpha_unit_force.setColumn(4, unit_force_y_a); // constraint disabled... In this direction, end-effector's motion is left to emerge naturally Twist unit_force_z_a( Vector(0.0, 0.0, 0.0), @@ -1515,8 +1515,8 @@ void SolverTest::FdAndVereshchaginSolversConsistencyTest() // ######################################################################################### // Vereshchagin Hybrid Dynamics solver - // When the Cartesian Acceleration Constraints are deactivated, the computations perfomed - // in the Vereshchagin solver are completely the same as the computations perfomed in + // When the Cartesian Acceleration Constraints are deactivated, the computations performed + // in the Vereshchagin solver are completely the same as the computations performed in // the well-known FD Articulated Body Algorithm (ABA) developed by Featherstone // Constraint Unit forces at the end-effector. Set to zero to deactivate all constraints @@ -1533,7 +1533,7 @@ void SolverTest::FdAndVereshchaginSolversConsistencyTest() Vector linearAcc(0.0, 0.0, 9.81); Vector angularAcc(0.0, 0.0, 0.0); Twist root_Acc(linearAcc, angularAcc); - // Torques felt in robot's joints due to constrait forces acting on the end-effector + // Torques felt in robot's joints due to constraint forces acting on the end-effector JntArray constraint_tau(nj); // In this test, all elements of this array should result to zero JntArray q_dd_Ver(nj); // Resultant joint accelerations @@ -1574,7 +1574,7 @@ void SolverTest::ExternalWrenchEstimatorTest() /** * This EPS has a slightly different purpose than the EPSes of the other solver-tests. While other EPSes are taking care of the differences that * originate from e.g. floating-number imprecisions, different compilers (or same compiler but different flags) used between different machines (OS), etc. - * The EPS specified bellow is there to cover those imperfections as well but, it's also there to + * The EPS specified below is there to cover those imperfections as well but, it's also there to * take into account the noise in estimated signals (the differences between estimated and ground-truth wrenches), caused by other computations in this test * (ones coming from the implemented controller and the dynamics simulator) not just those coming from the estimator itself. */ @@ -1637,7 +1637,7 @@ void SolverTest::ExternalWrenchEstimatorTest() std::vector jnt_pos; std::vector wrench_reference; - // Intialize random generator + // Initialize random generator std::random_device rd; //Will be used to obtain a seed for the random number engine std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd() std::uniform_real_distribution<> dis_force(-15.0, 15.0); From 86c7893234aeccec3b9bf24cf20de9380d64bdf3 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Fri, 17 Mar 2023 08:00:45 +0100 Subject: [PATCH 60/78] Bump python_orocos_kdl/pybind11 from `12852cd` to `be97c5a` (#435) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `12852cd` to `be97c5a`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/12852cd33bdaae0fe5fecc0db39b788dbbcc7b3d...be97c5a98b4b252c524566f508b5c79410d118c6) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index 12852cd3..be97c5a9 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit 12852cd33bdaae0fe5fecc0db39b788dbbcc7b3d +Subproject commit be97c5a98b4b252c524566f508b5c79410d118c6 From fded882b8255a29bd27648263265d20bbf098a66 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 12 Apr 2023 16:21:38 +0200 Subject: [PATCH 61/78] [kdl] add rosdoc config (#437) --- orocos_kdl/package.xml | 1 + orocos_kdl/rosdoc.yaml | 5 +++++ 2 files changed, 6 insertions(+) create mode 100644 orocos_kdl/rosdoc.yaml diff --git a/orocos_kdl/package.xml b/orocos_kdl/package.xml index 0046b555..fffdd7d3 100644 --- a/orocos_kdl/package.xml +++ b/orocos_kdl/package.xml @@ -26,6 +26,7 @@ cmake + diff --git a/orocos_kdl/rosdoc.yaml b/orocos_kdl/rosdoc.yaml new file mode 100644 index 00000000..58107573 --- /dev/null +++ b/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*' From 0c3285bed7071cb61a9282e8c95544522d2a0001 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Sat, 15 Jul 2023 14:07:40 +0200 Subject: [PATCH 62/78] Bump python_orocos_kdl/pybind11 from `be97c5a` to `0dcf6f2` (#443) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `be97c5a` to `0dcf6f2`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/be97c5a98b4b252c524566f508b5c79410d118c6...0dcf6f289dd034f2322de382ec72484206ba4555) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index be97c5a9..0dcf6f28 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit be97c5a98b4b252c524566f508b5c79410d118c6 +Subproject commit 0dcf6f289dd034f2322de382ec72484206ba4555 From b35de34ab6ca9cb9a776531f519d95d973f83d7f Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 18 Jul 2023 10:19:53 +0200 Subject: [PATCH 63/78] Bump python_orocos_kdl/pybind11 from `0dcf6f2` to `8b03ffa` (#444) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `0dcf6f2` to `8b03ffa`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/0dcf6f289dd034f2322de382ec72484206ba4555...8b03ffa7c06cd9c8a38297b1c8923695d1ff1b07) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index 0dcf6f28..8b03ffa7 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit 0dcf6f289dd034f2322de382ec72484206ba4555 +Subproject commit 8b03ffa7c06cd9c8a38297b1c8923695d1ff1b07 From 7134c46c86c0fcf86f6814f098042a69ac43629f Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 5 Sep 2023 08:43:51 +0200 Subject: [PATCH 64/78] Bump actions/checkout from 3 to 4 (#446) Bumps [actions/checkout](https://github.com/actions/checkout) from 3 to 4. - [Release notes](https://github.com/actions/checkout/releases) - [Changelog](https://github.com/actions/checkout/blob/main/CHANGELOG.md) - [Commits](https://github.com/actions/checkout/compare/v3...v4) --- updated-dependencies: - dependency-name: actions/checkout dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/main.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 5fb9c9ff..252d9535 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -34,7 +34,7 @@ jobs: OROCOS_BUILD_TYPE: ${{ matrix.orocos_build_type }} ROS_PYTHON_VERSION: ${{ matrix.python_version }} steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: recursive - uses: actions/setup-python@v4 @@ -88,7 +88,7 @@ jobs: branch: release-1.5 env: ${{ matrix.env }} steps: - - uses: actions/checkout@v3 + - 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)) }} From 0bfa6c0a7f3192efadd53515928fbe84055f7973 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 6 Sep 2023 13:44:24 +0200 Subject: [PATCH 65/78] fix(cmake) whitespace splitting (#440) Fixes https://github.com/orocos/orocos_kinematics_dynamics/issues/439 --- orocos_kdl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt index a1b335cd..f9d37e02 100644 --- a/orocos_kdl/CMakeLists.txt +++ b/orocos_kdl/CMakeLists.txt @@ -54,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) From 41ca68680ac28170381e59fba42e7381db48d89f Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Sep 2023 17:24:11 +0200 Subject: [PATCH 66/78] (actions) also test ubuntu 22.04 (#448) --- .github/workflows/main.yml | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 252d9535..0dc629bb 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -12,10 +12,15 @@ 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-20.04 orocos_build_type: Release @@ -29,6 +34,10 @@ jobs: 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 }} From 7478f96d01963db105e214ba79bd43709b0a3e5a Mon Sep 17 00:00:00 2001 From: kmartin36 Date: Thu, 12 Oct 2023 06:15:43 -0400 Subject: [PATCH 67/78] Fix KDL::PI in chainiksolverpos_lma_demo.cpp (#452) --- orocos_kdl/examples/chainiksolverpos_lma_demo.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp index 353ea71c..55ffe7e7 100644 --- a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp +++ b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp @@ -90,9 +90,9 @@ void test_inverseposkin(KDL::Chain& chain) { KDL::JntArray q_sol(n); for (int trial=0;trial Date: Thu, 7 Dec 2023 17:52:12 +0100 Subject: [PATCH 68/78] Bump actions/setup-python from 4 to 5 (#455) Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4 to 5. - [Release notes](https://github.com/actions/setup-python/releases) - [Commits](https://github.com/actions/setup-python/compare/v4...v5) --- updated-dependencies: - dependency-name: actions/setup-python dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 0dc629bb..5b339b43 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -46,7 +46,7 @@ jobs: - uses: actions/checkout@v4 with: submodules: recursive - - uses: actions/setup-python@v4 + - uses: actions/setup-python@v5 with: python-version: ${{ matrix.python_version }} - name: Install From a40783249a21721d49e740824a15de95cd8ddd7d Mon Sep 17 00:00:00 2001 From: thyssentishman <131173056+thyssentishman@users.noreply.github.com> Date: Fri, 19 Jan 2024 08:29:30 +0000 Subject: [PATCH 69/78] Rename variables with reserved names (#456) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit From the GNU libc manual[0]: In addition to the names documented in this manual, reserved names include all external identifiers (global functions and variables) that begin with an underscore (‘_’) and all identifiers regardless of use that begin with either two underscores or an underscore followed by a capital letter are reserved names. This is so that the library and header files can define functions, variables, and macros for internal purposes without risk of conflict with names in user programs. [0] https://www.gnu.org/software/libc/manual/html_node/Reserved-Names.html --- orocos_kdl/src/chainiksolverpos_lma.cpp | 4 ++-- orocos_kdl/src/chainiksolverpos_lma.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/orocos_kdl/src/chainiksolverpos_lma.cpp b/orocos_kdl/src/chainiksolverpos_lma.cpp index 2fd5588b..71d2ab5e 100644 --- a/orocos_kdl/src/chainiksolverpos_lma.cpp +++ b/orocos_kdl/src/chainiksolverpos_lma.cpp @@ -49,7 +49,7 @@ inline void Twist_to_Eigen(const KDL::Twist& t,Eigen::MatrixBase& e) { ChainIkSolverPos_LMA::ChainIkSolverPos_LMA( const KDL::Chain& _chain, - const Eigen::Matrix& _L, + const Eigen::Matrix& _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()), + L(_l.cast()), T_base_jointroot(nj), T_base_jointtip(nj), q(nj), diff --git a/orocos_kdl/src/chainiksolverpos_lma.hpp b/orocos_kdl/src/chainiksolverpos_lma.hpp index c3410cf6..42d4bbf1 100644 --- a/orocos_kdl/src/chainiksolverpos_lma.hpp +++ b/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; after 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& _L, + const Eigen::Matrix& _l, double _eps=1E-5, int _maxiter=500, double _eps_joints=1E-15 From 129693e571a7822655d1f58bb0f83b385542a3d8 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Fri, 29 Mar 2024 14:01:35 +0100 Subject: [PATCH 70/78] Bump python_orocos_kdl/pybind11 from `8b03ffa` to `01ab935` (#459) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `8b03ffa` to `01ab935`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/8b03ffa7c06cd9c8a38297b1c8923695d1ff1b07...01ab935612a6800c4ad42957808d6cbd30047902) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index 8b03ffa7..01ab9356 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit 8b03ffa7c06cd9c8a38297b1c8923695d1ff1b07 +Subproject commit 01ab935612a6800c4ad42957808d6cbd30047902 From faa74b740fb5c7f331ddf6e18863197d3c24948f Mon Sep 17 00:00:00 2001 From: DasRoteSkelett Date: Tue, 7 May 2024 08:15:58 +0200 Subject: [PATCH 71/78] cmake: Allow PYTHON_SITE_PACKAGES_INSTALL_DIR set extern (#461) The variable PYTHON_SITE_PACKAGES_INSTALL_DIR is kind of hardcoded in the CMakeLists.txt to .../dist-packages. This is not true for all linux distros. dist-packages comes from debian and debian like distros. Python core uses site-packages. We use an unintrusive way and allow the variable to be passed from cmake, so you at least have the option to set a different install location (i.e. site-packages) from extern. Signed-off-by: Matthias Schoepfer --- python_orocos_kdl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index 64fdd215..e0bc2f2a 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -28,7 +28,7 @@ set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION} CACHE STRING "Python version used find_package(Python ${PYTHON_VERSION} COMPONENTS Interpreter Development REQUIRED) # 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") # This might be overridden below if built with catkin. +set(PYTHON_SITE_PACKAGES_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/python${Python_VERSION_MAJOR}.${Python_VERSION_MINOR}/dist-packages" CACHE STRING "Install location of the python package") # This might be overridden below if built with catkin. set(LIBRARY_NAME "PyKDL") # catkin-specific configuration (optional) From 03be204a38744b2045c9e4face7534459ce93d9e Mon Sep 17 00:00:00 2001 From: efferre79 Date: Mon, 10 Jun 2024 09:43:15 +0200 Subject: [PATCH 72/78] added support to boost::timer 1.83+ (#463) * added support to boost::timer 1.83+ * update also the cmake file --- orocos_kdl/examples/CMakeLists.txt | 7 ++++++- orocos_kdl/examples/chainiksolverpos_lma_demo.cpp | 14 ++++++++++++++ 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/orocos_kdl/examples/CMakeLists.txt b/orocos_kdl/examples/CMakeLists.txt index bc5cbc02..1e257496 100644 --- a/orocos_kdl/examples/CMakeLists.txt +++ b/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) diff --git a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp index 55ffe7e7..34f0bdcd 100644 --- a/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp +++ b/orocos_kdl/examples/chainiksolverpos_lma_demo.cpp @@ -56,7 +56,12 @@ estimate of shortest time per invposkin (ms) 0.155544 #include #include +#include +#if BOOST_VERSION < 108300 #include +#else +#include +#endif /** * tests the inverse kinematics on the given kinematic chain for a @@ -64,7 +69,11 @@ estimate of shortest time per invposkin (ms) 0.155544 * \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(); @@ -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; From 6daa04403cd39cf0a85bac13662158aa4afe9099 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 27 Jun 2024 13:36:50 +0200 Subject: [PATCH 73/78] Bump python_orocos_kdl/pybind11 from `01ab935` to `10889fe` (#466) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `01ab935` to `10889fe`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/01ab935612a6800c4ad42957808d6cbd30047902...10889fe897aa1c081909975252b1486e0724c46b) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index 01ab9356..10889fe8 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit 01ab935612a6800c4ad42957808d6cbd30047902 +Subproject commit 10889fe897aa1c081909975252b1486e0724c46b From 179ee2c558f14e86a8d3bed47c1714f90ea887ea Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Fri, 28 Jun 2024 08:09:55 +0200 Subject: [PATCH 74/78] Bump python_orocos_kdl/pybind11 from `10889fe` to `6b92b6e` (#467) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `10889fe` to `6b92b6e`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/10889fe897aa1c081909975252b1486e0724c46b...6b92b6e148ab2762950425f6de851f8d152d5b4f) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index 10889fe8..6b92b6e1 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit 10889fe897aa1c081909975252b1486e0724c46b +Subproject commit 6b92b6e148ab2762950425f6de851f8d152d5b4f From af9cc9520de4c660c0149e0691e33ff1359e6726 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Fri, 16 Aug 2024 10:28:28 +0200 Subject: [PATCH 75/78] Bump python_orocos_kdl/pybind11 from `6b92b6e` to `5211a17` (#469) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `6b92b6e` to `5211a17`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/6b92b6e148ab2762950425f6de851f8d152d5b4f...5211a1715a1421236ca02ae4974c57a73782043c) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index 6b92b6e1..5211a171 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit 6b92b6e148ab2762950425f6de851f8d152d5b4f +Subproject commit 5211a1715a1421236ca02ae4974c57a73782043c From b6f2af241ce7adf18e7df73c9086e619e65e21b1 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Fri, 23 Aug 2024 08:47:39 +0200 Subject: [PATCH 76/78] Bump python_orocos_kdl/pybind11 from `5211a17` to `cb9abc8` (#472) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `5211a17` to `cb9abc8`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/5211a1715a1421236ca02ae4974c57a73782043c...cb9abc80aa092995d81a1aaf4b6d61f52e337665) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index 5211a171..cb9abc80 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit 5211a1715a1421236ca02ae4974c57a73782043c +Subproject commit cb9abc80aa092995d81a1aaf4b6d61f52e337665 From d2c989d9e3f8ca19e0c0c51a2e52479de20a886b Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Sat, 21 Sep 2024 09:49:35 +0200 Subject: [PATCH 77/78] Bump python_orocos_kdl/pybind11 from `cb9abc8` to `58c382a` (#476) Bumps [python_orocos_kdl/pybind11](https://github.com/pybind/pybind11) from `cb9abc8` to `58c382a`. - [Release notes](https://github.com/pybind/pybind11/releases) - [Commits](https://github.com/pybind/pybind11/compare/cb9abc80aa092995d81a1aaf4b6d61f52e337665...58c382a8e3d7081364d2f5c62e7f429f0412743b) --- updated-dependencies: - dependency-name: python_orocos_kdl/pybind11 dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- python_orocos_kdl/pybind11 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 index cb9abc80..58c382a8 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit cb9abc80aa092995d81a1aaf4b6d61f52e337665 +Subproject commit 58c382a8e3d7081364d2f5c62e7f429f0412743b From b8c7473b4b49e17f125661d9ab7d7c52a91af522 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 18 Oct 2024 14:11:05 +0200 Subject: [PATCH 78/78] (PyKDL) fix return type and bound name (#465) --- python_orocos_kdl/PyKDL/kinfam.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python_orocos_kdl/PyKDL/kinfam.cpp b/python_orocos_kdl/PyKDL/kinfam.cpp index 3e6c5d2d..fec7eda7 100644 --- a/python_orocos_kdl/PyKDL/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/kinfam.cpp @@ -247,9 +247,9 @@ void init_kinfam(pybind11::module &m) }); m.def("SetToZero", (void (*)(Jacobian&)) &KDL::SetToZero, py::arg("jac")); - m.def("changeRefPoint", (void (*)(const Jacobian&, const Vector&, Jacobian&)) &KDL::changeRefPoint, py::arg("src"), py::arg("base"), py::arg("dest")); - m.def("changeBase", (void (*)(const Jacobian&, const Rotation&, Jacobian&)) &KDL::changeBase, py::arg("src"), py::arg("rot"), py::arg("dest")); - m.def("SetToZero", (void (*)(const Jacobian&, const Frame&, Jacobian&)) &KDL::changeRefFrame, py::arg("src"), py::arg("frame"), py::arg("dest")); + m.def("changeRefPoint", (bool (*)(const Jacobian&, const Vector&, Jacobian&)) &KDL::changeRefPoint, py::arg("src"), py::arg("base"), py::arg("dest")); + m.def("changeBase", (bool (*)(const Jacobian&, const Rotation&, Jacobian&)) &KDL::changeBase, py::arg("src"), py::arg("rot"), py::arg("dest")); + m.def("changeRefFrame", (bool (*)(const Jacobian&, const Frame&, Jacobian&)) &KDL::changeRefFrame, py::arg("src"), py::arg("frame"), py::arg("dest")); // --------------------