diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt index f9d37e02..ababd647 100644 --- a/orocos_kdl/CMakeLists.txt +++ b/orocos_kdl/CMakeLists.txt @@ -23,6 +23,8 @@ MESSAGE( STATUS "Orocos KDL version ${VERSION} (${KDL_VERSION_MAJOR}.${KDL_VERSI SET( PROJ_SOURCE_DIR ${orocos_kdl_SOURCE_DIR} ) SET( PROJ_BINARY_DIR ${orocos_kdl_BINARY_DIR} ) +include(GNUInstallDirs) + # catkin-specific configuration (optional) find_package(catkin QUIET) if(catkin_FOUND) @@ -47,20 +49,23 @@ ELSE ( NOT CMAKE_BUILD_TYPE ) MESSAGE( STATUS "Build type set to '${CMAKE_BUILD_TYPE}' by user." ) ENDIF ( NOT CMAKE_BUILD_TYPE ) -SET( KDL_CFLAGS "") +if(MSVC) + add_compile_options(/W4 /WX) +else(MSVC) + add_compile_options(-Wall -Werror=all -Wextra -Werror=extra) +endif(MSVC) find_package(Eigen3 QUIET) if(NOT EIGEN3_FOUND) include(${PROJ_SOURCE_DIR}/cmake/FindEigen3.cmake) endif() -include_directories(${EIGEN3_INCLUDE_DIR}) +include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}) SET(KDL_CFLAGS "${KDL_CFLAGS} -I\"${EIGEN3_INCLUDE_DIR}\"") # Check the platform STL containers capabilities include(cmake/CheckSTLContainers.cmake) CHECK_STL_CONTAINERS() - -# Set the default option appropriately +# Set the default option accordingly if(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) set(KDL_USE_NEW_TREE_INTERFACE_DEFAULT OFF) else(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) @@ -80,7 +85,7 @@ endif() if(KDL_USE_NEW_TREE_INTERFACE) # We need shared_ptr from boost since not all compilers are c++11 capable find_package(Boost REQUIRED) - include_directories(${Boost_INCLUDE_DIRS}) + include_directories(SYSTEM ${Boost_INCLUDE_DIRS}) endif(KDL_USE_NEW_TREE_INTERFACE) OPTION(ENABLE_TESTS OFF "Enable building of tests") @@ -112,20 +117,42 @@ export(TARGETS orocos-kdl export(PACKAGE orocos_kdl) +include(CMakePackageConfigHelpers) # Generate CMake package configuration -CONFIGURE_FILE(orocos_kdl-config.cmake.in - ${PROJECT_BINARY_DIR}/orocos_kdl-config.cmake @ONLY) -CONFIGURE_FILE(orocos_kdl-config-version.cmake.in - ${PROJECT_BINARY_DIR}/orocos_kdl-config-version.cmake @ONLY) - -INSTALL(FILES cmake/FindEigen3.cmake DESTINATION share/orocos_kdl/cmake) -INSTALL(FILES ${PROJECT_BINARY_DIR}/orocos_kdl-config.cmake DESTINATION share/orocos_kdl/cmake) -INSTALL(FILES ${PROJECT_BINARY_DIR}/orocos_kdl-config-version.cmake DESTINATION share/orocos_kdl/cmake) -INSTALL(EXPORT OrocosKDLTargets DESTINATION share/orocos_kdl/cmake) +configure_package_config_file(${CMAKE_CURRENT_SOURCE_DIR}/orocos_kdl-config.cmake.in + ${PROJECT_BINARY_DIR}/orocos_kdl-config.cmake + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/orocos_kdl +) + +write_basic_package_version_file( + ${PROJECT_BINARY_DIR}/orocos_kdl-config-version.cmake + VERSION ${KDL_VERSION} + COMPATIBILITY SameMajorVersion +) + +install( + FILES + ${PROJECT_BINARY_DIR}/orocos_kdl-config.cmake + ${PROJECT_BINARY_DIR}/orocos_kdl-config-version.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/orocos_kdl +) + +INSTALL(FILES cmake/FindEigen3.cmake DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/orocos_kdl) +INSTALL(EXPORT OrocosKDLTargets DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/orocos_kdl) # Generate pkg-config package configuration -CONFIGURE_FILE(orocos_kdl.pc.in ${CMAKE_CURRENT_BINARY_DIR}/orocos-kdl.pc @ONLY) -CONFIGURE_FILE(orocos_kdl.pc.in ${CMAKE_CURRENT_BINARY_DIR}/orocos_kdl.pc @ONLY) - -INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/orocos-kdl.pc DESTINATION lib${LIB_SUFFIX}/pkgconfig) -INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/orocos_kdl.pc DESTINATION lib${LIB_SUFFIX}/pkgconfig) +configure_package_config_file(${CMAKE_CURRENT_SOURCE_DIR}/orocos_kdl.pc.in + ${PROJECT_BINARY_DIR}/orocos_kdl.pc + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig +) +configure_package_config_file(${CMAKE_CURRENT_SOURCE_DIR}/orocos_kdl.pc.in + ${PROJECT_BINARY_DIR}/orocos-kdl.pc + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig +) + +install( + FILES + ${PROJECT_BINARY_DIR}/orocos_kdl.pc + ${PROJECT_BINARY_DIR}/orocos-kdl.pc + DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig +) diff --git a/orocos_kdl/cmake_uninstall.cmake.in b/orocos_kdl/cmake_uninstall.cmake.in deleted file mode 100644 index bcf2437c..00000000 --- a/orocos_kdl/cmake_uninstall.cmake.in +++ /dev/null @@ -1,22 +0,0 @@ -IF(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") - MESSAGE(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"") -ENDIF(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") - -FILE(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) -STRING(REGEX REPLACE "\n" ";" files "${files}") -FOREACH(file ${files}) - MESSAGE(STATUS "Uninstalling \"${file}\"") - IF(EXISTS "${file}") - EXEC_PROGRAM( - "@CMAKE_COMMAND@" ARGS "-E remove \"${file}\"" - OUTPUT_VARIABLE rm_out - RETURN_VALUE rm_retval - ) - IF("${rm_retval}" STREQUAL 0) - ELSE("${rm_retval}" STREQUAL 0) - MESSAGE(FATAL_ERROR "Problem when removing \"${file}\"") - ENDIF("${rm_retval}" STREQUAL 0) - ELSE(EXISTS "${file}") - MESSAGE(STATUS "File \"${file}\" does not exist.") - ENDIF(EXISTS "${file}") -ENDFOREACH(file) \ No newline at end of file diff --git a/orocos_kdl/orocos_kdl-config-version.cmake.in b/orocos_kdl/orocos_kdl-config-version.cmake.in deleted file mode 100644 index 796eddc1..00000000 --- a/orocos_kdl/orocos_kdl-config-version.cmake.in +++ /dev/null @@ -1,11 +0,0 @@ -set(PACKAGE_VERSION "@KDL_VERSION@") - -# Check whether the requested PACKAGE_FIND_VERSION is compatible -if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") - set(PACKAGE_VERSION_COMPATIBLE FALSE) -else() - set(PACKAGE_VERSION_COMPATIBLE TRUE) - if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") - set(PACKAGE_VERSION_EXACT TRUE) - endif() -endif() \ No newline at end of file diff --git a/orocos_kdl/src/CMakeLists.txt b/orocos_kdl/src/CMakeLists.txt index 954b8fc0..6ba372b4 100644 --- a/orocos_kdl/src/CMakeLists.txt +++ b/orocos_kdl/src/CMakeLists.txt @@ -58,7 +58,6 @@ TARGET_INCLUDE_DIRECTORIES(orocos-kdl PUBLIC SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES SOVERSION "${KDL_VERSION_MAJOR}.${KDL_VERSION_MINOR}" VERSION "${KDL_VERSION}" - COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS}" PUBLIC_HEADER "${KDL_HPPS};${CMAKE_CURRENT_BINARY_DIR}/config.h" ) diff --git a/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp b/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp index 6e01c49a..6bd57530 100644 --- a/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp +++ b/orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp @@ -92,7 +92,7 @@ namespace KDL{ return (error = E_NOERROR); } - void ChainFdSolver_RNE::RK4Integrator(unsigned int& nj, const double& t, double& dt, KDL::JntArray& q, KDL::JntArray& q_dot, + void ChainFdSolver_RNE::RK4Integrator(unsigned int& nj, const double& /*t*/, double& dt, KDL::JntArray& q, KDL::JntArray& q_dot, KDL::JntArray& torques, KDL::Wrenches& f_ext, KDL::ChainFdSolver_RNE& fdsolver, KDL::JntArray& q_dotdot, KDL::JntArray& dq, KDL::JntArray& dq_dot, KDL::JntArray& q_temp, KDL::JntArray& q_dot_temp) diff --git a/orocos_kdl/src/chainhdsolver_vereshchagin.cpp b/orocos_kdl/src/chainhdsolver_vereshchagin.cpp index 75f58e96..7d21a8d1 100644 --- a/orocos_kdl/src/chainhdsolver_vereshchagin.cpp +++ b/orocos_kdl/src/chainhdsolver_vereshchagin.cpp @@ -74,7 +74,7 @@ int ChainHdSolver_Vereshchagin::CartToJnt(const JntArray &q, const JntArray &q_d return (error = E_NOERROR); } -void ChainHdSolver_Vereshchagin::initial_upwards_sweep(const JntArray &q, const JntArray &qdot, const JntArray &qdotdot, const Wrenches& f_ext) +void ChainHdSolver_Vereshchagin::initial_upwards_sweep(const JntArray &q, const JntArray &qdot, const JntArray &/*qdotdot*/, const Wrenches& f_ext) { //if (q.rows() != nj || qdot.rows() != nj || qdotdot.rows() != nj || f_ext.size() != ns) // return -1; diff --git a/orocos_kdl/src/frameacc.hpp b/orocos_kdl/src/frameacc.hpp index a8cd3529..23d0d091 100644 --- a/orocos_kdl/src/frameacc.hpp +++ b/orocos_kdl/src/frameacc.hpp @@ -66,6 +66,7 @@ class VectorAcc Vector dv; //!< acceleration vector public: VectorAcc():p(),v(),dv() {} + VectorAcc(const VectorAcc& arg) = default; explicit VectorAcc(const Vector& _p):p(_p),v(Vector::Zero()),dv(Vector::Zero()) {} VectorAcc(const Vector& _p,const Vector& _v):p(_p),v(_v),dv(Vector::Zero()) {} VectorAcc(const Vector& _p,const Vector& _v,const Vector& _dv): @@ -114,6 +115,7 @@ class RotationAcc Vector dw; //!< angular acceration vector public: RotationAcc():R(),w() {} + RotationAcc(const RotationAcc& arg) = default; explicit RotationAcc(const Rotation& _R):R(_R),w(Vector::Zero()){} RotationAcc(const Rotation& _R,const Vector& _w,const Vector& _dw): R(_R),w(_w),dw(_dw) {} @@ -166,6 +168,7 @@ class FrameAcc VectorAcc p; //!< Translation, velocity and acceleration of origin. public: FrameAcc(){} + FrameAcc(const FrameAcc& arg) = default; explicit FrameAcc(const Frame& _T):M(_T.M),p(_T.p) {} FrameAcc(const Frame& _T,const Twist& _t,const Twist& _dt): M(_T.M,_t.rot,_dt.rot),p(_T.p,_t.vel,_dt.vel) {} @@ -205,6 +208,7 @@ class TwistAcc public: TwistAcc():vel(),rot() {}; + TwistAcc(const TwistAcc& arg) = default; TwistAcc(const VectorAcc& _vel,const VectorAcc& _rot):vel(_vel),rot(_rot) {}; IMETHOD TwistAcc& operator-=(const TwistAcc& arg); diff --git a/orocos_kdl/src/framevel.hpp b/orocos_kdl/src/framevel.hpp index b84e69c1..ca798f0a 100644 --- a/orocos_kdl/src/framevel.hpp +++ b/orocos_kdl/src/framevel.hpp @@ -92,6 +92,7 @@ class VectorVel Vector v; // velocity vector public: VectorVel():p(),v(){} + VectorVel(const VectorVel& arg) = default; VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {} explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {} @@ -154,6 +155,7 @@ class RotationVel Vector w; // rotation vector public: RotationVel():R(),w() {} + RotationVel(const RotationVel& arg) = default; explicit RotationVel(const Rotation& _R):R(_R),w(Vector::Zero()){} RotationVel(const Rotation& _R,const Vector& _w):R(_R),w(_w){} @@ -219,6 +221,7 @@ class FrameVel VectorVel p; public: FrameVel(){} + FrameVel(const FrameVel& arg) = default; explicit FrameVel(const Frame& _T): M(_T.M),p(_T.p) {} @@ -277,6 +280,7 @@ class TwistVel // = Constructors TwistVel():vel(),rot() {}; + TwistVel(const TwistVel& arg) = default; TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {}; TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {}; TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {}; diff --git a/orocos_kdl/src/jntarray.cpp b/orocos_kdl/src/jntarray.cpp index 32c875d0..99738bd5 100644 --- a/orocos_kdl/src/jntarray.cpp +++ b/orocos_kdl/src/jntarray.cpp @@ -55,15 +55,13 @@ namespace KDL data.conservativeResizeLike(Eigen::VectorXd::Zero(newSize)); } - double JntArray::operator()(unsigned int i,unsigned int j)const + double JntArray::operator()(unsigned int i,unsigned int /*j*/)const { - assert(j==0); return data(i); } - double& JntArray::operator()(unsigned int i,unsigned int j) + double& JntArray::operator()(unsigned int i,unsigned int /*j*/) { - assert(j==0); return data(i); } diff --git a/orocos_kdl/src/kinfam_io.cpp b/orocos_kdl/src/kinfam_io.cpp index d8eb4d31..ca6e41b3 100644 --- a/orocos_kdl/src/kinfam_io.cpp +++ b/orocos_kdl/src/kinfam_io.cpp @@ -29,7 +29,7 @@ std::ostream& operator <<(std::ostream& os, const Joint& joint) { <<", axis: "<>(std::istream& is, Joint& joint) { +std::istream& operator >>(std::istream& is, Joint& /*joint*/) { return is; } @@ -38,7 +38,7 @@ std::ostream& operator <<(std::ostream& os, const Segment& segment) { return os; } -std::istream& operator >>(std::istream& is, Segment& segment) { +std::istream& operator >>(std::istream& is, Segment& /*segment*/) { return is; } @@ -50,7 +50,7 @@ std::ostream& operator <<(std::ostream& os, const Chain& chain) { return os; } -std::istream& operator >>(std::istream& is, Chain& chain) { +std::istream& operator >>(std::istream& is, Chain& /*chain*/) { return is; } @@ -68,7 +68,7 @@ std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator root) { return os << "\n"; } -std::istream& operator >>(std::istream& is, Tree& tree) { +std::istream& operator >>(std::istream& is, Tree& /*tree*/) { return is; } @@ -80,7 +80,7 @@ std::ostream& operator <<(std::ostream& os, const JntArray& array) { return os; } -std::istream& operator >>(std::istream& is, JntArray& array) { +std::istream& operator >>(std::istream& is, JntArray& /*array*/) { return is; } @@ -95,7 +95,7 @@ std::ostream& operator <<(std::ostream& os, const Jacobian& jac) { return os; } -std::istream& operator >>(std::istream& is, Jacobian& jac) { +std::istream& operator >>(std::istream& is, Jacobian& /*jac*/) { return is; } std::ostream& operator <<(std::ostream& os, const JntSpaceInertiaMatrix& jntspaceinertiamatrix) { @@ -109,7 +109,7 @@ std::ostream& operator <<(std::ostream& os, const JntSpaceInertiaMatrix& jntspac return os; } -std::istream& operator >>(std::istream& is, JntSpaceInertiaMatrix& jntspaceinertiamatrix) { +std::istream& operator >>(std::istream& is, JntSpaceInertiaMatrix& /*jntspaceinertiamatrix*/) { return is; } diff --git a/orocos_kdl/src/path_composite.cpp b/orocos_kdl/src/path_composite.cpp index c93f5a29..ac3d1c4b 100644 --- a/orocos_kdl/src/path_composite.cpp +++ b/orocos_kdl/src/path_composite.cpp @@ -85,7 +85,7 @@ void Path_Composite::Add(Path* geom, bool aggregate ) { gv.insert( gv.end(),std::make_pair(geom,aggregate) ); } -double Path_Composite::LengthToS(double length) { +double Path_Composite::LengthToS(double /*length*/) { throw Error_MotionPlanning_Not_Applicable(); return 0; } diff --git a/orocos_kdl/src/path_cyclic_closed.cpp b/orocos_kdl/src/path_cyclic_closed.cpp index fa5c65f3..a8bbe5d2 100644 --- a/orocos_kdl/src/path_cyclic_closed.cpp +++ b/orocos_kdl/src/path_cyclic_closed.cpp @@ -48,7 +48,7 @@ namespace KDL { Path_Cyclic_Closed::Path_Cyclic_Closed(Path* _geom,int _times, bool _aggregate): times(_times),geom(_geom), aggregate(_aggregate) {} -double Path_Cyclic_Closed::LengthToS(double length) { +double Path_Cyclic_Closed::LengthToS(double /*length*/) { throw Error_MotionPlanning_Not_Applicable(); return 0; } diff --git a/orocos_kdl/src/path_point.cpp b/orocos_kdl/src/path_point.cpp index 854843dc..1ccae434 100644 --- a/orocos_kdl/src/path_point.cpp +++ b/orocos_kdl/src/path_point.cpp @@ -55,15 +55,15 @@ double Path_Point::LengthToS(double length) { double Path_Point::PathLength(){ return 0; } -Frame Path_Point::Pos(double s) const { +Frame Path_Point::Pos(double /*s*/) const { return F_base_start; } -Twist Path_Point::Vel(double s,double sd) const { +Twist Path_Point::Vel(double /*s*/,double /*sd*/) const { return Twist::Zero(); } -Twist Path_Point::Acc(double s,double sd,double sdd) const { +Twist Path_Point::Acc(double /*s*/,double /*sd*/,double /*sdd*/) const { return Twist::Zero(); } diff --git a/orocos_kdl/src/rigidbodyinertia.cpp b/orocos_kdl/src/rigidbodyinertia.cpp index 3bfd938d..7d98232f 100644 --- a/orocos_kdl/src/rigidbodyinertia.cpp +++ b/orocos_kdl/src/rigidbodyinertia.cpp @@ -27,7 +27,7 @@ namespace KDL{ const static bool mhi=true; - RigidBodyInertia::RigidBodyInertia(double m_,const Vector& h_,const RotationalInertia& I_,bool mhi): + RigidBodyInertia::RigidBodyInertia(double m_,const Vector& h_,const RotationalInertia& I_,bool /*mhi*/): m(m_),h(h_),I(I_) { } diff --git a/orocos_kdl/src/rotational_interpolation_sa.cpp b/orocos_kdl/src/rotational_interpolation_sa.cpp index 6b5a56b5..886eb25f 100644 --- a/orocos_kdl/src/rotational_interpolation_sa.cpp +++ b/orocos_kdl/src/rotational_interpolation_sa.cpp @@ -60,11 +60,11 @@ Rotation RotationalInterpolation_SingleAxis::Pos(double theta) const { return R_base_start* Rotation::Rot2(rot_start_end,theta); } -Vector RotationalInterpolation_SingleAxis::Vel(double theta,double thetad) const { +Vector RotationalInterpolation_SingleAxis::Vel(double /*theta*/,double thetad) const { return R_base_start * ( rot_start_end*thetad ); } -Vector RotationalInterpolation_SingleAxis::Acc(double theta,double thetad,double thetadd) const { +Vector RotationalInterpolation_SingleAxis::Acc(double /*theta*/,double /*thetad*/,double thetadd) const { return R_base_start * ( rot_start_end* thetadd); } @@ -85,4 +85,3 @@ RotationalInterpolation* RotationalInterpolation_SingleAxis::Clone() const { } } - diff --git a/orocos_kdl/src/trajectory_stationary.hpp b/orocos_kdl/src/trajectory_stationary.hpp index 162fa4e4..cc678e6f 100644 --- a/orocos_kdl/src/trajectory_stationary.hpp +++ b/orocos_kdl/src/trajectory_stationary.hpp @@ -35,13 +35,13 @@ namespace KDL { virtual double Duration() const { return duration; } - virtual Frame Pos(double time) const { + virtual Frame Pos(double /*time*/) const { return pos; } - virtual Twist Vel(double time) const { + virtual Twist Vel(double /*time*/) const { return Twist::Zero(); } - virtual Twist Acc(double time) const { + virtual Twist Acc(double /*time*/) const { return Twist::Zero(); } virtual void Write(std::ostream& os) const; diff --git a/orocos_kdl/src/utilities/error.h b/orocos_kdl/src/utilities/error.h index 785fa51d..ea63931b 100644 --- a/orocos_kdl/src/utilities/error.h +++ b/orocos_kdl/src/utilities/error.h @@ -70,7 +70,7 @@ class Error_IO : public Error { std::string msg; int typenr; public: - Error_IO(const std::string& _msg="Unspecified I/O Error",int typenr=0):msg(_msg) {} + Error_IO(const std::string& _msg="Unspecified I/O Error",int /*typenr*/=0):msg(_msg) {} virtual const char* Description() const {return msg.c_str();} virtual int GetType() const {return typenr;} }; diff --git a/orocos_kdl/src/velocityprofile_dirac.cpp b/orocos_kdl/src/velocityprofile_dirac.cpp index 23c915b5..46c7dae3 100644 --- a/orocos_kdl/src/velocityprofile_dirac.cpp +++ b/orocos_kdl/src/velocityprofile_dirac.cpp @@ -75,7 +75,7 @@ namespace KDL { return 0; } - double VelocityProfile_Dirac::Acc(double time) const { + double VelocityProfile_Dirac::Acc(double /*time*/) const { throw Error_MotionPlanning_Incompatible(); return 0; } diff --git a/orocos_kdl/src/velocityprofile_rect.cpp b/orocos_kdl/src/velocityprofile_rect.cpp index 7992c1e9..82f6b625 100644 --- a/orocos_kdl/src/velocityprofile_rect.cpp +++ b/orocos_kdl/src/velocityprofile_rect.cpp @@ -118,7 +118,7 @@ double VelocityProfile_Rectangular::Vel(double time) const { } } -double VelocityProfile_Rectangular::Acc(double time) const { +double VelocityProfile_Rectangular::Acc(double /*time*/) const { throw Error_MotionPlanning_Incompatible(); return 0; } diff --git a/orocos_kdl/src/velocityprofile_spline.cpp b/orocos_kdl/src/velocityprofile_spline.cpp index ec651ed9..de37ecbf 100644 --- a/orocos_kdl/src/velocityprofile_spline.cpp +++ b/orocos_kdl/src/velocityprofile_spline.cpp @@ -48,7 +48,7 @@ VelocityProfile_Spline::~VelocityProfile_Spline() return; } -void VelocityProfile_Spline::SetProfile(double pos1, double pos2) +void VelocityProfile_Spline::SetProfile(double /*pos1*/, double /*pos2*/) { return; } diff --git a/orocos_kdl/src/velocityprofile_trap.cpp b/orocos_kdl/src/velocityprofile_trap.cpp index 1bee674e..c39f6d9c 100644 --- a/orocos_kdl/src/velocityprofile_trap.cpp +++ b/orocos_kdl/src/velocityprofile_trap.cpp @@ -190,9 +190,4 @@ void VelocityProfile_Trap::Write(std::ostream& os) const { os << "TRAPEZOIDAL[" << maxvel << "," << maxacc <<"]"; } - - - - } - diff --git a/orocos_kdl/tests/CMakeLists.txt b/orocos_kdl/tests/CMakeLists.txt index ed045b2f..0c36ad74 100644 --- a/orocos_kdl/tests/CMakeLists.txt +++ b/orocos_kdl/tests/CMakeLists.txt @@ -5,64 +5,44 @@ IF(ENABLE_TESTS) ADD_EXECUTABLE(framestest framestest.cpp test-runner.cpp) TARGET_LINK_LIBRARIES(framestest orocos-kdl ${CPPUNIT}) - SET(TESTNAME "framestest") - SET_TARGET_PROPERTIES( framestest PROPERTIES - COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS} -DTESTNAME=\"\\\"${TESTNAME}\\\"\" ") + target_compile_definitions(framestest PRIVATE TESTNAME="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}") + target_compile_definitions(kinfamtest PRIVATE TESTNAME="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}\\\"\" ") + target_compile_definitions(solvertest PRIVATE TESTNAME="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}\\\"\" ") + target_compile_definitions(inertiatest PRIVATE TESTNAME="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}\\\"\" ") + target_compile_definitions(jacobiantest PRIVATE TESTNAME="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}\\\"\" ") + target_compile_definitions(jacobiandottest PRIVATE TESTNAME="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}\\\"\" ") + target_compile_definitions(velocityprofiletest PRIVATE TESTNAME="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}\\\"\" ") + target_compile_definitions(treeinvdyntest PRIVATE TESTNAME="treeinvdyntest") ADD_TEST(NAME treeinvdyntest COMMAND treeinvdyntest) -# ADD_EXECUTABLE(rframestest rframestest.cpp) -# TARGET_LINK_LIBRARIES(rframestest orocos-kdl) -# ADD_TEST(NAME rframestest COMMAND rframestest) -# # ADD_EXECUTABLE(rallnumbertest rallnumbertest.cpp) # TARGET_LINK_LIBRARIES(rallnumbertest orocos-kdl) # ADD_TEST(NAME rallnumbertest COMMAND rallnumbertest) diff --git a/orocos_kdl/tests/rallnumbertest.cpp b/orocos_kdl/tests/rallnumbertest.cpp index 806caa9a..67f93130 100644 --- a/orocos_kdl/tests/rallnumbertest.cpp +++ b/orocos_kdl/tests/rallnumbertest.cpp @@ -12,10 +12,10 @@ -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include diff --git a/orocos_kdl/tests/rframestest.cpp b/orocos_kdl/tests/rframestest.cpp deleted file mode 100644 index a9a68f1a..00000000 --- a/orocos_kdl/tests/rframestest.cpp +++ /dev/null @@ -1,346 +0,0 @@ -#include -#include -#include -#include -#include - - -using namespace KDL; - -void TestVector() { - KDL_CTX; - Vector v(3,4,5); - Vector v2; - Vector vt(-5,-6,-3); - KDL_DIFF( 2*v-v, v ); - KDL_DIFF( v*2-v, v ); - KDL_DIFF( v+v+v-2*v, v ); - v2=v; - KDL_DIFF( v, v2 ); - v2+=v; - KDL_DIFF( 2*v, v2 ); - v2-=v; - KDL_DIFF( v,v2); - v2.ReverseSign(); - KDL_DIFF( v,-v2); - KDL_DIFF( v*vt,-vt*v); - v2 = Vector(-5,-6,-3); - KDL_DIFF( v*v2,-v2*v); -} - -void TestVectorVel() { - KDL_CTX; - VectorVel v(Vector(3,-4,5),Vector(6,3,-5)); - VectorVel v2; - Vector vt(-4,-6,-8); - KDL_DIFF( 2*v-v, v ); - KDL_DIFF( v*2-v, v ); - KDL_DIFF( v+v+v-2*v, v ); - v2=v; - KDL_DIFF( v, v2 ); - v2+=v; - KDL_DIFF( 2*v, v2 ); - v2-=v; - KDL_DIFF( v,v2); - v2.ReverseSign(); - KDL_DIFF( v,-v2); - KDL_DIFF( v*vt,-vt*v); - v2 = VectorVel(Vector(-5,-6,-3),Vector(3,4,5)); - KDL_DIFF( v*v2,-v2*v); -} - -void TestVectorAcc() { - KDL_CTX; - VectorAcc v(Vector(3,-4,5),Vector(6,3,-5),Vector(-4,-3,-6)); - VectorAcc v2; - Vector vt(-4,-6,-8); - KDL_DIFF( 2*v-v, v ); - KDL_DIFF( v*2-v, v ); - KDL_DIFF( v+v+v-2*v, v ); - v2=v; - KDL_DIFF( v, v2 ); - v2+=v; - KDL_DIFF( 2*v, v2 ); - v2-=v; - KDL_DIFF( v,v2); - v2.ReverseSign(); - KDL_DIFF( v,-v2); - KDL_DIFF( v*vt,-vt*v); - v2 = VectorAcc(Vector(-5,-6,-3),Vector(-3,-4,-1),Vector(10,12,9)); - KDL_DIFF( v*v2,-v2*v); -} - -void TestRotation() { - KDL_CTX; - Vector v(Vector(9,4,-2)); - Vector v2; - Vector vt(2,3,4); - Rotation R; - Rotation R2; - double a; - double b; - double c; - a= -15*deg2rad; - b= 20*deg2rad; - c= -80*deg2rad; - R = Rotation(Rotation::RPY(a,b,c)); - R2=R; - KDL_DIFF( R,R2 ); - KDL_DIFF( (R*v).Norm(), v.Norm() ); - KDL_DIFF( R.Inverse(R*v), v ); - KDL_DIFF( R*R.Inverse(v), v ); - KDL_DIFF( R*Rotation::Identity(), R ); - KDL_DIFF( Rotation::Identity()*R, R ); - KDL_DIFF( R*(R*(R*v)), (R*R*R)*v ); - KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt ); - KDL_DIFF( R*R.Inverse(), Rotation::Identity() ); - KDL_DIFF( R.Inverse()*R, Rotation::Identity() ); - KDL_DIFF( R.Inverse()*v, R.Inverse(v) ); - v2=v*v-2*v; - KDL_DIFF( (v2).Norm(), ::sqrt(dot(v2,v2)) ); -} - - -void TestRotationVel() { - KDL_CTX; - VectorVel v(Vector(9,4,-2),Vector(-5,6,-2)); - VectorVel v2; - Vector vt(2,3,4); - RotationVel R; - RotationVel R2; - double a; - double b; - double c; - a= -15*deg2rad; - b= 20*deg2rad; - c= -80*deg2rad; - R = RotationVel(Rotation::RPY(a,b,c),Vector(2,4,1)); - R2=R; - KDL_DIFF( R,R2 ); - KDL_DIFF( (R*v).Norm(), v.Norm() ); - KDL_DIFF( R.Inverse(R*v), v ); - KDL_DIFF( R*R.Inverse(v), v ); - KDL_DIFF( R*Rotation::Identity(), R ); - KDL_DIFF( Rotation::Identity()*R, R ); - KDL_DIFF( R*(R*(R*v)), (R*R*R)*v ); - KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt ); - KDL_DIFF( R*R.Inverse(), RotationVel::Identity() ); - KDL_DIFF( R.Inverse()*R, RotationVel::Identity() ); - KDL_DIFF( R.Inverse()*v, R.Inverse(v) ); - v2=v*v-2*v; - KDL_DIFF( (v2).Norm(), sqrt(dot(v2,v2)) ); -} - -void TestRotationAcc() { - KDL_CTX; - VectorAcc v(Vector(9,4,-2),Vector(-5,6,-2),Vector(2,-3,-3)); - VectorAcc v2; - Vector vt(2,3,4); - RotationAcc R; - RotationAcc R2; - double a; - double b; - double c; - a= -15*deg2rad; - b= 20*deg2rad; - c= -80*deg2rad; - R = RotationAcc(Rotation::RPY(a,b,c),Vector(2,4,1),Vector(-3,-2,-1)); - R2=R; - KDL_DIFF( R,R2 ); - KDL_DIFF( (R*v).Norm(), v.Norm() ); - KDL_DIFF( R.Inverse(R*v), v ); - KDL_DIFF( R*R.Inverse(v), v ); - KDL_DIFF( R*Rotation::Identity(), R ); - KDL_DIFF( Rotation::Identity()*R, R ); - KDL_DIFF( R*(R*(R*v)), (R*R*R)*v ); - KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt ); - KDL_DIFF( R*R.Inverse(), RotationAcc::Identity() ); - KDL_DIFF( R.Inverse()*R, RotationAcc::Identity() ); - KDL_DIFF( R.Inverse()*v, R.Inverse(v) ); - v2=v*v-2*v; - KDL_DIFF( (v2).Norm(), sqrt(dot(v2,v2)) ); -} - -void TestFrame() { - KDL_CTX; - Vector v(3,4,5); - Vector vt(-1,0,-10); - Rotation R; - Frame F; - Frame F2; - F = Frame(Rotation::EulerZYX(10*deg2rad,20*deg2rad,-10*deg2rad),Vector(4,-2,1)); - - F2=F; - KDL_DIFF( F, F2 ); - KDL_DIFF( F.Inverse(F*v), v ); - KDL_DIFF( F.Inverse(F*vt), vt ); - KDL_DIFF( F*F.Inverse(v), v ); - KDL_DIFF( F*F.Inverse(vt), vt ); - KDL_DIFF( F*Frame::Identity(), F ); - KDL_DIFF( Frame::Identity()*F, F ); - KDL_DIFF( F*(F*(F*v)), (F*F*F)*v ); - KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt ); - KDL_DIFF( F*F.Inverse(), Frame::Identity() ); - KDL_DIFF( F.Inverse()*F, Frame::Identity() ); - KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) ); -} - -void TestFrameVel() { - KDL_CTX; - VectorVel v(Vector(3,4,5),Vector(-2,-4,-1)); - Vector vt(-1,0,-10); - RotationVel R; - FrameVel F; - FrameVel F2; ; - F = FrameVel( - Frame(Rotation::EulerZYX(10*deg2rad,20*deg2rad,-10*deg2rad),Vector(4,-2,1)), - Twist(Vector(2,-2,-2),Vector(-5,-3,-2)) - ); - F2=F; - KDL_DIFF( F, F2 ); - KDL_DIFF( F.Inverse(F*v), v ); - KDL_DIFF( F.Inverse(F*vt), vt ); - KDL_DIFF( F*F.Inverse(v), v ); - KDL_DIFF( F*F.Inverse(vt), vt ); - KDL_DIFF( F*Frame::Identity(), F ); - KDL_DIFF( Frame::Identity()*F, F ); - KDL_DIFF( F*(F*(F*v)), (F*F*F)*v ); - KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt ); - KDL_DIFF( F*F.Inverse(), FrameVel::Identity() ); - KDL_DIFF( F.Inverse()*F, Frame::Identity() ); - KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) ); -} - -void TestFrameAcc() { - KDL_CTX; - VectorAcc v(Vector(3,4,5),Vector(-2,-4,-1),Vector(6,4,2)); - Vector vt(-1,0,-10); - RotationAcc R; - FrameAcc F; - FrameAcc F2; - F = FrameAcc( - Frame(Rotation::EulerZYX(10*deg2rad,20*deg2rad,-10*deg2rad),Vector(4,-2,1)), - Twist(Vector(2,-2,-2),Vector(-5,-3,-2)), - Twist(Vector(5,6,2),Vector(-2,-3,1)) - ); - F2=F; - KDL_DIFF( F, F2 ); - KDL_DIFF( F.Inverse(F*v), v ); - KDL_DIFF( F.Inverse(F*vt), vt ); - KDL_DIFF( F*F.Inverse(v), v ); - KDL_DIFF( F*F.Inverse(vt), vt ); - KDL_DIFF( F*Frame::Identity(), F ); - KDL_DIFF( Frame::Identity()*F, F ); - KDL_DIFF( F*(F*(F*v)), (F*F*F)*v ); - KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt ); - KDL_DIFF( F*F.Inverse(), FrameAcc::Identity() ); - KDL_DIFF( F.Inverse()*F, Frame::Identity() ); - KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) ); -} - -void TestTwistVel() { - KDL_CTX; - // Twist - TwistVel t(VectorVel( - Vector(6,3,5), - Vector(1,4,2) - ),VectorVel( - Vector(4,-2,7), - Vector(-1,-2,-3) - ) - ); - TwistVel t2; - RotationVel R(Rotation::RPY(10*deg2rad,20*deg2rad,-15*deg2rad),Vector(-1,5,3)); - FrameVel F = FrameVel( - Frame( - Rotation::EulerZYX(-17*deg2rad,13*deg2rad,-16*deg2rad), - Vector(4,-2,1) - ), - Twist( - Vector(2,-2,-2), - Vector(-5,-3,-2) - ) - ); - - KDL_DIFF(2.0*t-t,t); - KDL_DIFF(t*2.0-t,t); - KDL_DIFF(t+t+t-2.0*t,t); - t2=t; - KDL_DIFF(t,t2); - t2+=t; - KDL_DIFF(2.0*t,t2); - t2-=t; - KDL_DIFF(t,t2); - t.ReverseSign(); - KDL_DIFF(t,-t2); - KDL_DIFF(R.Inverse(R*t),t); - KDL_DIFF(R*t,R*R.Inverse(R*t)); - KDL_DIFF(F.Inverse(F*t),t); - KDL_DIFF(F*t,F*F.Inverse(F*t)); - KDL_DIFF(doubleVel(3.14,2)*t,t*doubleVel(3.14,2)); - KDL_DIFF(t/doubleVel(3.14,2),t*(1.0/doubleVel(3.14,2))); - KDL_DIFF(t/3.14,t*(1.0/3.14)); - KDL_DIFF(-t,-1.0*t); - VectorVel p1(Vector(5,1,2),Vector(4,2,1)) ; - VectorVel p2(Vector(2,0,5),Vector(-2,7,-1)) ; - KDL_DIFF(t.RefPoint(p1+p2),t.RefPoint(p1).RefPoint(p2)); - KDL_DIFF(t,t.RefPoint(-p1).RefPoint(p1)); -} - -void TestTwistAcc() { - KDL_CTX; - // Twist - TwistAcc t( VectorAcc(Vector(6,3,5),Vector(1,4,2),Vector(5,2,1)), - VectorAcc(Vector(4,-2,7),Vector(-1,-2,-3),Vector(5,2,9) ) - ); - TwistAcc t2; - RotationAcc R(Rotation::RPY(10*deg2rad,20*deg2rad,-15*deg2rad), - Vector(-1,5,3), - Vector(2,1,3) - ) ; - FrameAcc F = FrameAcc( - Frame(Rotation::EulerZYX(-17*deg2rad,13*deg2rad,-16*deg2rad),Vector(4,-2,1)), - Twist(Vector(2,-2,-2),Vector(-5,-3,-2)), - Twist(Vector(5,4,-5),Vector(12,13,17)) - ); - - KDL_DIFF(2.0*t-t,t); - KDL_DIFF(t*2.0-t,t); - KDL_DIFF(t+t+t-2.0*t,t); - t2=t; - KDL_DIFF(t,t2); - t2+=t; - KDL_DIFF(2.0*t,t2); - t2-=t; - KDL_DIFF(t,t2); - t.ReverseSign(); - KDL_DIFF(t,-t2); - KDL_DIFF(R.Inverse(R*t),t); - KDL_DIFF(R*t,R*R.Inverse(R*t)); - KDL_DIFF(F.Inverse(F*t),t); - KDL_DIFF(F*t,F*F.Inverse(F*t)); - KDL_DIFF(doubleAcc(3.14,2,3)*t,t*doubleAcc(3.14,2,3)); - KDL_DIFF(t/doubleAcc(3.14,2,7),t*(1.0/doubleAcc(3.14,2,7))); - KDL_DIFF(t/3.14,t*(1.0/3.14)); - KDL_DIFF(-t,-1.0*t); - VectorAcc p1(Vector(5,1,2),Vector(4,2,1),Vector(2,1,3)); - VectorAcc p2(Vector(2,0,5),Vector(-2,7,-1),Vector(-3,2,-1)); - KDL_DIFF(t.RefPoint(p1+p2),t.RefPoint(p1).RefPoint(p2)); - KDL_DIFF(t,t.RefPoint(-p1).RefPoint(p1)); -} - -int main() { - KDL_CTX; - TestVector(); - TestRotation(); - TestFrame(); - TestVectorVel(); - TestRotationVel(); - TestFrameVel(); - TestVectorAcc(); - TestRotationAcc(); - TestFrameAcc(); - TestTwistVel(); - TestTwistAcc(); - return 0; -} diff --git a/orocos_kdl/tests/test-runner.cpp b/orocos_kdl/tests/test-runner.cpp index db23244f..01ae1f77 100644 --- a/orocos_kdl/tests/test-runner.cpp +++ b/orocos_kdl/tests/test-runner.cpp @@ -12,7 +12,7 @@ #include #include -int main(int argc, char** argv) +int main(int /*argc*/, char** /*argv*/) { // Get the top level suite from the registry CppUnit::Test *suite = CppUnit::TestFactoryRegistry::getRegistry().makeTest(); diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index 64fdd215..ab936b8c 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -12,9 +12,12 @@ endif() project(python_orocos_kdl VERSION 1.5.1) +add_compile_options(-Wall -Werror=all) +add_compile_options(-Wextra -Werror=extra) + # find a matching version of orocos_kdl find_package(orocos_kdl ${PROJECT_VERSION} EXACT REQUIRED) -include_directories(${orocos_kdl_INCLUDE_DIRS}) +include_directories(SYSTEM ${orocos_kdl_INCLUDE_DIRS}) link_directories(${orocos_kdl_LIBRARY_DIRS}) if(DEFINED ENV{ROS_PYTHON_VERSION}) diff --git a/python_orocos_kdl/PyKDL/kinfam.cpp b/python_orocos_kdl/PyKDL/kinfam.cpp index 3e6c5d2d..39798a21 100644 --- a/python_orocos_kdl/PyKDL/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/kinfam.cpp @@ -247,9 +247,9 @@ void init_kinfam(pybind11::module &m) }); m.def("SetToZero", (void (*)(Jacobian&)) &KDL::SetToZero, py::arg("jac")); - m.def("changeRefPoint", (void (*)(const Jacobian&, const Vector&, Jacobian&)) &KDL::changeRefPoint, py::arg("src"), py::arg("base"), py::arg("dest")); - m.def("changeBase", (void (*)(const Jacobian&, const Rotation&, Jacobian&)) &KDL::changeBase, py::arg("src"), py::arg("rot"), py::arg("dest")); - m.def("SetToZero", (void (*)(const Jacobian&, const Frame&, Jacobian&)) &KDL::changeRefFrame, py::arg("src"), py::arg("frame"), py::arg("dest")); + m.def("changeRefPoint", (bool (*)(const Jacobian&, const Vector&, Jacobian&)) &KDL::changeRefPoint, py::arg("src"), py::arg("base"), py::arg("dest")); + m.def("changeBase", (bool (*)(const Jacobian&, const Rotation&, Jacobian&)) &KDL::changeBase, py::arg("src"), py::arg("rot"), py::arg("dest")); + m.def("SetToZero", (bool (*)(const Jacobian&, const Frame&, Jacobian&)) &KDL::changeRefFrame, py::arg("src"), py::arg("frame"), py::arg("dest")); // --------------------