diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 97a79b8e..16249f09 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" @@ -12,28 +12,32 @@ jobs: strategy: fail-fast: false matrix: - os: [ubuntu-20.04] + os: [ubuntu-22.04, ubuntu-24.04] orocos_build_type: [Debug, Release] compiler: [gcc, clang] - python_version: ['3.8'] + python_version: ['3.10', '3.12'] include: - - os: ubuntu-18.04 + - os: ubuntu-24.04 orocos_build_type: Release compiler: gcc - python_version: '2' - - os: ubuntu-20.04 + python_version: '3.8' + - os: ubuntu-24.04 orocos_build_type: Release compiler: gcc python_version: '3.9' + - os: ubuntu-24.04 + orocos_build_type: Release + compiler: gcc + python_version: '3.11' env: CC: ${{ matrix.compiler }} OROCOS_BUILD_TYPE: ${{ matrix.orocos_build_type }} ROS_PYTHON_VERSION: ${{ matrix.python_version }} steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v4 with: submodules: recursive - - uses: actions/setup-python@v2 + - uses: actions/setup-python@v5 with: python-version: ${{ matrix.python_version }} - name: Install @@ -76,18 +80,6 @@ jobs: fail-fast: false matrix: include: - - env: - ROS_DISTRO: kinetic - ROS_REPO: ros - ABICHECK_URL: github:orocos/orocos_kinematics_dynamics#release-1.3 - ABICHECK_MERGE: false - branch: release-1.3 - - env: - ROS_DISTRO: melodic - ROS_REPO: ros - ABICHECK_URL: github:orocos/orocos_kinematics_dynamics#release-1.4 - ABICHECK_MERGE: false - branch: release-1.4 - env: ROS_DISTRO: noetic ROS_REPO: ros @@ -96,7 +88,7 @@ jobs: branch: release-1.5 env: ${{ matrix.env }} steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: submodules: recursive if: ${{ (github.event_name == 'push' && endsWith(github.ref, matrix.branch)) || (github.event_name == 'pull_request' && endsWith(github.base_ref, matrix.branch)) }} diff --git a/README.md b/README.md index 3de6b187..4c18ea7e 100644 --- a/README.md +++ b/README.md @@ -13,3 +13,8 @@ The C++ library is located in the `orocos_kdl` folder. The installation instruct The python bindings, are located in the `python_orocos_kdl` folder. The installation instructions can be found in [INSTALL.md](python_orocos_kdl/INSTALL.md). +Always use the same version of the C++ library and the python bindings. As a mismatch between these two can cause many issues. + +Also when using ROS/catkin, it is preferred to use the catkin installation method over the `cmake/make` method. + +There will be no ROS Noetic release. diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt index a15bd15d..f9d37e02 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) ################################################### @@ -58,7 +54,7 @@ if(NOT EIGEN3_FOUND) include(${PROJ_SOURCE_DIR}/cmake/FindEigen3.cmake) endif() include_directories(${EIGEN3_INCLUDE_DIR}) -SET(KDL_CFLAGS "${KDL_CFLAGS} -I${EIGEN3_INCLUDE_DIR}") +SET(KDL_CFLAGS "${KDL_CFLAGS} -I\"${EIGEN3_INCLUDE_DIR}\"") # Check the platform STL containers capabilities include(cmake/CheckSTLContainers.cmake) 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 5697b250..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(); @@ -90,9 +99,9 @@ void test_inverseposkin(KDL::Chain& chain) { KDL::JntArray q_sol(n); for (int trial=0;trial 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*' diff --git a/orocos_kdl/src/CMakeLists.txt b/orocos_kdl/src/CMakeLists.txt index ca63079c..954b8fc0 100644 --- a/orocos_kdl/src/CMakeLists.txt +++ b/orocos_kdl/src/CMakeLists.txt @@ -16,46 +16,44 @@ ENDIF(MSVC) CONFIGURE_FILE(config.h.in config.h @ONLY) #### Settings for rpath -IF(${CMAKE_MINIMUM_REQUIRED_VERSION} VERSION_GREATER "2.8.12") - MESSAGE(AUTHOR_WARNING "CMAKE_MINIMUM_REQUIRED_VERSION is now ${CMAKE_MINIMUM_REQUIRED_VERSION}. This check can be removed.") -ENDIF() -IF(NOT (CMAKE_VERSION VERSION_LESS 2.8.12)) - IF(NOT MSVC) - #add the option to disable RPATH - OPTION(OROCOSKDL_ENABLE_RPATH "Enable RPATH during installation" TRUE) - MARK_AS_ADVANCED(OROCOSKDL_ENABLE_RPATH) - ENDIF(NOT MSVC) +IF(NOT MSVC) + #add the option to disable RPATH + OPTION(OROCOSKDL_ENABLE_RPATH "Enable RPATH during installation" TRUE) + MARK_AS_ADVANCED(OROCOSKDL_ENABLE_RPATH) +ENDIF(NOT MSVC) - IF(OROCOSKDL_ENABLE_RPATH) - #Configure RPATH - SET(CMAKE_MACOSX_RPATH TRUE) #enable RPATH on OSX. This also suppress warnings on CMake >= 3.0 - # when building, don't use the install RPATH already - # (but later on when installing) - SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) - #build directory by default is built with RPATH - SET(CMAKE_SKIP_BUILD_RPATH FALSE) +IF(OROCOSKDL_ENABLE_RPATH) + #Configure RPATH + SET(CMAKE_MACOSX_RPATH TRUE) #enable RPATH on OSX. This also suppress warnings on CMake >= 3.0 + # when building, don't use the install RPATH already + # (but later on when installing) + SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) + #build directory by default is built with RPATH + SET(CMAKE_SKIP_BUILD_RPATH FALSE) - #This is relative RPATH for libraries built in the same project - #I assume that the directory is - # - install_dir/something for binaries - # - install_dir/lib for libraries - LIST(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir) - IF("${isSystemDir}" STREQUAL "-1") - FILE(RELATIVE_PATH _rel_path "${CMAKE_INSTALL_PREFIX}/bin" "${CMAKE_INSTALL_PREFIX}/lib") - IF (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") - SET(CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}") - ELSE() - SET(CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}") - ENDIF() - ENDIF("${isSystemDir}" STREQUAL "-1") - # add the automatically determined parts of the RPATH - # which point to directories outside the build tree to the install RPATH - SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) #very important! - ENDIF() + #This is relative RPATH for libraries built in the same project + #I assume that the directory is + # - install_dir/something for binaries + # - install_dir/lib for libraries + LIST(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir) + IF("${isSystemDir}" STREQUAL "-1") + FILE(RELATIVE_PATH _rel_path "${CMAKE_INSTALL_PREFIX}/bin" "${CMAKE_INSTALL_PREFIX}/lib") + IF (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + SET(CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}") + ELSE() + SET(CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}") + ENDIF() + ENDIF("${isSystemDir}" STREQUAL "-1") + # add the automatically determined parts of the RPATH + # which point to directories outside the build tree to the install RPATH + SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) #very important! ENDIF() #####end RPATH -ADD_LIBRARY(orocos-kdl ${LIB_TYPE} ${KDL_SRCS} config.h) +ADD_LIBRARY(orocos-kdl ${LIB_TYPE} ${KDL_SRCS}) + +TARGET_INCLUDE_DIRECTORIES(orocos-kdl PUBLIC + "$") SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES SOVERSION "${KDL_VERSION_MAJOR}.${KDL_VERSION_MINOR}" @@ -65,22 +63,15 @@ SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES ) #### Settings for rpath disabled (back-compatibility) -IF(${CMAKE_MINIMUM_REQUIRED_VERSION} VERSION_GREATER "2.8.12") - MESSAGE(AUTHOR_WARNING "CMAKE_MINIMUM_REQUIRED_VERSION is now ${CMAKE_MINIMUM_REQUIRED_VERSION}. This check can be removed.") -ENDIF() -IF(CMAKE_VERSION VERSION_LESS 2.8.12) +IF(NOT OROCOSKDL_ENABLE_RPATH) SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}") -ELSE() - IF(NOT OROCOSKDL_ENABLE_RPATH) - SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES - INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}") - ENDIF() ENDIF() #####end RPATH # Needed so that the generated config.h can be used -INCLUDE_DIRECTORIES(${CMAKE_CURRENT_BINARY_DIR}) +TARGET_INCLUDE_DIRECTORIES(orocos-kdl PUBLIC + "$") TARGET_LINK_LIBRARIES(orocos-kdl ${Boost_LIBRARIES}) INSTALL(TARGETS orocos-kdl 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/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 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_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/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 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 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 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 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/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..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: @@ -235,10 +275,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(){ 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/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 diff --git a/orocos_kdl/src/tree.cpp b/orocos_kdl/src/tree.cpp index 6d8943de..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=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; } diff --git a/orocos_kdl/src/treeiksolverpos_online.cpp b/orocos_kdl/src/treeiksolverpos_online.cpp index 3e3d1318..594c3a05 100644 --- a/orocos_kdl/src/treeiksolverpos_online.cpp +++ b/orocos_kdl/src/treeiksolverpos_online.cpp @@ -35,12 +35,9 @@ TreeIkSolverPos_Online::TreeIkSolverPos_Online(const double& 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), 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/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(); } 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 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/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/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; 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..2a3ab4a6 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(3450679443808348711u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(v2), static_cast(11093822414574u)); 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(3373832976806748309u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(t2), static_cast(730713428471863u)); } 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(13897938943539516747u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(w2), static_cast(730713428471863u)); } 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(5526237894416316219u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(Rotation()), static_cast(8386870752212395617u)); } 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); @@ -633,6 +650,24 @@ 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 + 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)); // 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)); // Only for testing purposes, shouldn't use static function of instances + + CPPUNIT_ASSERT_EQUAL(std::hash{}(F), static_cast(6112004106257185417u)); + CPPUNIT_ASSERT_EQUAL(std::hash{}(Frame()), static_cast(8387572672274540708u)); } JntArray CreateRandomJntArray(int size) 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); diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index 09897cb3..cc452225 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -1,7 +1,11 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.12.0) 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() @@ -21,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} EXACT 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) @@ -46,7 +49,13 @@ else() set(PYTHON_MODULE_EXTENSION ".so") endif() -add_subdirectory(pybind11) +find_package(pybind11 2.6 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 diff --git a/python_orocos_kdl/INSTALL.md b/python_orocos_kdl/INSTALL.md index c3f2d5de..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=2), 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/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 a6852caa..9cbee1b9 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; @@ -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); @@ -108,7 +109,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 +130,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; @@ -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); @@ -183,7 +185,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 +205,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; @@ -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); @@ -259,7 +262,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 +287,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 +296,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 +317,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,15 +363,16 @@ 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()); 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__ @@ -409,7 +413,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 +425,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,20 +440,21 @@ 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, 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()); 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__ diff --git a/python_orocos_kdl/PyKDL/framevel.cpp b/python_orocos_kdl/PyKDL/framevel.cpp index bf8609bd..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); @@ -152,7 +153,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, @@ -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); @@ -237,7 +239,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 +281,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); @@ -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__ diff --git a/python_orocos_kdl/PyKDL/kinfam.cpp b/python_orocos_kdl/PyKDL/kinfam.cpp index d489aabd..fec7eda7 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<>()); @@ -85,8 +82,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 +111,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 +132,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 +155,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 +169,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 +212,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 +229,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 +238,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 +246,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", (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")); // -------------------- @@ -260,25 +257,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 +284,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 +300,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 +325,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); @@ -338,8 +335,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); // -------------------- @@ -380,8 +378,8 @@ 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 + 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")); @@ -419,14 +417,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); @@ -451,9 +449,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); @@ -473,7 +471,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")); // ------------------------------ @@ -507,7 +505,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")); // ------------------------------ 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 787d2c88..58c382a8 160000 --- a/python_orocos_kdl/pybind11 +++ b/python_orocos_kdl/pybind11 @@ -1 +1 @@ -Subproject commit 787d2c88cafa4d07fb38c9519c485a86323cfcf4 +Subproject commit 58c382a8e3d7081364d2f5c62e7f429f0412743b 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/framestest.py b/python_orocos_kdl/tests/framestest.py index eeed0531..cd6abe1a 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 @@ -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 @@ -110,12 +115,16 @@ 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), 3373832976806748309) + 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 @@ -184,12 +194,15 @@ 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 self.assertTrue(not Equal(w, -w)) # Doesn't work for zero wrench + # Hash + self.assertEqual(hash(w), hash(13897938943539516747)) # 551895977443887016 + self.assertEqual(hash(Wrench()), 730713428471863) + # Members v1 = Vector(1, 2, 3) v2 = Vector(4, 5, 6) @@ -228,6 +241,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 +260,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 +308,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 +358,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) @@ -346,6 +370,23 @@ def testFrame(self): self.assertEqual(Frame(f).M, f.M) self.assertEqual(Frame(f).p, f.p) + # Denavit-Hartenberg + 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)) + # 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, + 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)) + # 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, 5, 6, 7, 9, 10, 11), diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 614501a2..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 @@ -32,6 +31,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 +65,11 @@ 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) @@ -247,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)) @@ -302,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)