pax_global_header00006660000000000000000000000064137207675740014534gustar00rootroot0000000000000052 comment=735d562454ee623ec4b4a4108d26b024e8420d58 kdl_parser-1.14.1/000077500000000000000000000000001372076757400137465ustar00rootroot00000000000000kdl_parser-1.14.1/README.md000066400000000000000000000007251372076757400152310ustar00rootroot00000000000000# KDL Parser This contains packages for converting from URDF to a representation in KDL. See the ROS wiki for API documentation and tutorials. * [`kdl_parser`](http://wiki.ros.org/kdl_parser) * [`kdl_parser_py`](http://wiki.ros.org/kdl_parser_py) This was originally part of the [`ros/robot_model`](https://github.com/ros/robot_model) repository. It has been moved to this repo as described by [`ros/robot_model#195`](https://github.com/ros/robot_model/issues/195) kdl_parser-1.14.1/kdl_parser/000077500000000000000000000000001372076757400160745ustar00rootroot00000000000000kdl_parser-1.14.1/kdl_parser/CHANGELOG.rst000066400000000000000000000064541372076757400201260ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package kdl_parser ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.14.1 (2020-08-24) ------------------- 1.14.0 (2020-04-13) ------------------- * Used keys for Orocos (`#38 `_) * Set C++ standard to 14 (`#37 `_) * Contributors: Alejandro Hernández Cordero, James Xu, Shane Loretz 1.13.1 (2018-07-23) ------------------- * Fix up missing link tags in some XML files. (`#15 `_) * Contributors: Chris Lalancette 1.13.0 (2018-04-05) ------------------- * kdl_parser: switch from TinyXML to TinyXML2 (`#4 `_) * Style fixes from ros2 (`#11 `_) * Make rostest a test_depend (`#3 `_) * update links now that this is in its own repo * Contributors: Chris Lalancette, Dmitry Rozhkov, Mikael Arguedas 1.12.10 (2017-05-17) -------------------- * Use result of find_package(orocos_kdl) properly (`#200 `_) orocos_kdl_LIBRARY_DIRS was not set 1.12.9 (2017-04-26) ------------------- 1.12.8 (2017-03-27) ------------------- * add Chris and Shane as maintainers (`#184 `_) * fix missed mandatory -std=c++11 flag (`#181 `_) collada_parser,kdl_parser,urdf: add c++11 flag, collada_parser: replace typeof with ansi __typeof\_\_ builded/tested on gentoo Thanks den4ix for the contribution! * Contributors: Denis Romanchuk, William Woodall 1.12.7 (2017-01-26) ------------------- 1.12.6 (2017-01-04) ------------------- * Now using ``urdf::*ShredPtr`` instead of ``boost::shared_ptr`` (`#144 `_) * Contributors: Jochen Sprickerhof 1.12.5 (2016-10-27) ------------------- * fix segfault: safely handle empty robot model (`#154 `_) * Contributors: Robert Haschke 1.12.4 (2016-08-23) ------------------- 1.12.3 (2016-06-10) ------------------- 1.12.2 (2016-04-12) ------------------- 1.12.1 (2016-04-10) ------------------- 1.11.8 (2015-09-11) ------------------- 1.11.7 (2015-04-22) ------------------- 1.11.6 (2014-11-30) ------------------- * add version dependency on orocos_kdl >= 1.3.0 * Contributors: William Woodall 1.11.5 (2014-07-24) ------------------- * Update KDL SegmentMap interface to optionally use shared pointers The KDL Tree API optionally uses shared pointers on platforms where the STL containers don't support incomplete types. * Contributors: Brian Jensen 1.11.4 (2014-07-07) ------------------- 1.11.3 (2014-06-24) ------------------- * kdl_parser: Adding kdl library explicitly so that dependees can find it * Contributors: Jonathan Bohren 1.11.2 (2014-03-22) ------------------- 1.11.1 (2014-03-20) ------------------- 1.11.0 (2014-02-21) ------------------- * fix test at kdl_parser * Contributors: YoheiKakiuchi 1.10.18 (2013-12-04) -------------------- * add DEPENDS for kdl_parser * Contributors: Ioan Sucan 1.10.16 (2013-11-18) -------------------- * check for CATKIN_ENABLE_TESTING 1.10.15 (2013-08-17) -------------------- * fix `#30 `_ kdl_parser-1.14.1/kdl_parser/CMakeLists.txt000066400000000000000000000057601372076757400206440ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.7.2) project(kdl_parser) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(catkin QUIET COMPONENTS rosconsole cmake_modules ) if(NOT catkin_FOUND) # use local copies of FindTinyXML.cmake and FindTinyXML2.cmake from # 'cmake_modules' (https://github.com/ros/cmake_modules) list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake) endif() find_package(urdfdom REQUIRED) include_directories(${urdfdom_INCLUDE_DIRS}) find_package(orocos_kdl REQUIRED) find_package(TinyXML REQUIRED) find_package(TinyXML2 REQUIRED) # check for rosconsole # We check additionally for catkin to distinguish between an "official" ROS distribution # and the one provided in the distribution's repository. find_package(rosconsole QUIET) if(rosconsole_FOUND AND catkin_FOUND) add_definitions(-DHAS_ROS) endif() find_package(urdf QUIET) if(urdf_FOUND) add_definitions(-DHAS_URDF) include_directories(${urdf_INCLUDE_DIRS}) endif() include_directories(include ${orocos_kdl_INCLUDE_DIRS} ${TinyXML_INCLUDE_DIRS} ${TinyXML2_INCLUDE_DIRS}) if(catkin_FOUND) link_directories(${catkin_LIBRARY_DIRS}) include_directories(${catkin_INCLUDE_DIRS}) catkin_package( LIBRARIES ${PROJECT_NAME} ${orocos_kdl_LIBRARIES} INCLUDE_DIRS include CATKIN_DEPENDS rosconsole urdf DEPENDS orocos_kdl TinyXML TinyXML2 ) endif() add_library(${PROJECT_NAME} SHARED src/kdl_parser.cpp) target_link_libraries(${PROJECT_NAME} ${TinyXML_LIBRARIES} ${TinyXML2_LIBRARIES} ${orocos_kdl_LIBRARIES} ) target_link_libraries(${PROJECT_NAME} ${urdfdom_LIBRARIES}) if(catkin_FOUND) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) endif() if(urdf_FOUND) target_link_libraries(${PROJECT_NAME} ${urdf_LIBRARIES}) endif() if(WIN32) target_compile_definitions(${PROJECT_NAME} PRIVATE "KDL_PARSER_BUILDING_DLL") endif() add_executable(check_kdl_parser src/check_kdl_parser.cpp ) target_link_libraries(check_kdl_parser ${PROJECT_NAME}) if(catkin_FOUND AND CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS roscpp rostest) add_rostest_gtest(test_kdl_parser test/test_kdl_parser.launch test/test_kdl_parser.cpp ) target_link_libraries(test_kdl_parser ${PROJECT_NAME}) add_rostest_gtest(test_inertia_rpy test/test_inertia_rpy.launch test/test_inertia_rpy.cpp ) target_link_libraries(test_inertia_rpy ${PROJECT_NAME}) endif() if(catkin_FOUND) # How does CATKIN do this? #rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_kdl_parser.launch) # Install library install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) else() install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include) endif() kdl_parser-1.14.1/kdl_parser/cmake/000077500000000000000000000000001372076757400171545ustar00rootroot00000000000000kdl_parser-1.14.1/kdl_parser/cmake/FindTinyXML.cmake000066400000000000000000000054151372076757400222700ustar00rootroot00000000000000################################################################################################## # # CMake script for finding TinyXML. # # Input variables: # # - TinyXML_ROOT_DIR (optional): When specified, header files and libraries will be searched for in # ${TinyXML_ROOT_DIR}/include # ${TinyXML_ROOT_DIR}/libs # respectively, and the default CMake search order will be ignored. When unspecified, the default # CMake search order is used. # This variable can be specified either as a CMake or environment variable. If both are set, # preference is given to the CMake variable. # Use this variable for finding packages installed in a nonstandard location, or for enforcing # that one of multiple package installations is picked up. # # # Cache variables (not intended to be used in CMakeLists.txt files) # # - TinyXML_INCLUDE_DIR: Absolute path to package headers. # - TinyXML_LIBRARY: Absolute path to library. # # # Output variables: # # - TinyXML_FOUND: Boolean that indicates if the package was found # - TinyXML_INCLUDE_DIRS: Paths to the necessary header files # - TinyXML_LIBRARIES: Package libraries # # # Example usage: # # find_package(TinyXML) # if(NOT TinyXML_FOUND) # # Error handling # endif() # ... # include_directories(${TinyXML_INCLUDE_DIRS} ...) # ... # target_link_libraries(my_target ${TinyXML_LIBRARIES}) # ################################################################################################## # Get package location hint from environment variable (if any) if(NOT TinyXML_ROOT_DIR AND DEFINED ENV{TinyXML_ROOT_DIR}) set(TinyXML_ROOT_DIR "$ENV{TinyXML_ROOT_DIR}" CACHE PATH "TinyXML base directory location (optional, used for nonstandard installation paths)") endif() # Search path for nonstandard package locations if(TinyXML_ROOT_DIR) set(TinyXML_INCLUDE_PATH PATHS "${TinyXML_ROOT_DIR}/include" NO_DEFAULT_PATH) set(TinyXML_LIBRARY_PATH PATHS "${TinyXML_ROOT_DIR}/lib" NO_DEFAULT_PATH) endif() # Find headers and libraries find_path(TinyXML_INCLUDE_DIR NAMES tinyxml.h PATH_SUFFIXES "tinyxml" ${TinyXML_INCLUDE_PATH}) find_library(TinyXML_LIBRARY NAMES tinyxml PATH_SUFFIXES "tinyxml" ${TinyXML_LIBRARY_PATH}) mark_as_advanced(TinyXML_INCLUDE_DIR TinyXML_LIBRARY) # Output variables generation include(FindPackageHandleStandardArgs) find_package_handle_standard_args(TinyXML DEFAULT_MSG TinyXML_LIBRARY TinyXML_INCLUDE_DIR) set(TinyXML_FOUND ${TINYXML_FOUND}) # Enforce case-correctness: Set appropriately cased variable... unset(TINYXML_FOUND) # ...and unset uppercase variable generated by find_package_handle_standard_args if(TinyXML_FOUND) set(TinyXML_INCLUDE_DIRS ${TinyXML_INCLUDE_DIR}) set(TinyXML_LIBRARIES ${TinyXML_LIBRARY}) endif() kdl_parser-1.14.1/kdl_parser/cmake/FindTinyXML2.cmake000066400000000000000000000054721372076757400223550ustar00rootroot00000000000000################################################################################################## # # CMake script for finding TinyXML2. # # Input variables: # # - TinyXML2_ROOT_DIR (optional): When specified, header files and libraries will be searched for in # ${TinyXML2_ROOT_DIR}/include # ${TinyXML2_ROOT_DIR}/libs # respectively, and the default CMake search order will be ignored. When unspecified, the default # CMake search order is used. # This variable can be specified either as a CMake or environment variable. If both are set, # preference is given to the CMake variable. # Use this variable for finding packages installed in a nonstandard location, or for enforcing # that one of multiple package installations is picked up. # # # Cache variables (not intended to be used in CMakeLists.txt files) # # - TinyXML2_INCLUDE_DIR: Absolute path to package headers. # - TinyXML2_LIBRARY: Absolute path to library. # # # Output variables: # # - TinyXML2_FOUND: Boolean that indicates if the package was found # - TinyXML2_INCLUDE_DIRS: Paths to the necessary header files # - TinyXML2_LIBRARIES: Package libraries # # # Example usage: # # find_package(TinyXML2) # if(NOT TinyXML2_FOUND) # # Error handling # endif() # ... # include_directories(${TinyXML2_INCLUDE_DIRS} ...) # ... # target_link_libraries(my_target ${TinyXML2_LIBRARIES}) # ################################################################################################## # Get package location hint from environment variable (if any) if(NOT TinyXML2_ROOT_DIR AND DEFINED ENV{TinyXML2_ROOT_DIR}) set(TinyXML2_ROOT_DIR "$ENV{TinyXML2_ROOT_DIR}" CACHE PATH "TinyXML2 base directory location (optional, used for nonstandard installation paths)") endif() # Search path for nonstandard package locations if(TinyXML2_ROOT_DIR) set(TinyXML2_INCLUDE_PATH PATHS "${TinyXML2_ROOT_DIR}/include" NO_DEFAULT_PATH) set(TinyXML2_LIBRARY_PATH PATHS "${TinyXML2_ROOT_DIR}/lib" NO_DEFAULT_PATH) endif() # Find headers and libraries find_path(TinyXML2_INCLUDE_DIR NAMES tinyxml2.h PATH_SUFFIXES "tinyxml2" ${TinyXML2_INCLUDE_PATH}) find_library(TinyXML2_LIBRARY NAMES tinyxml2 PATH_SUFFIXES "tinyxml2" ${TinyXML2_LIBRARY_PATH}) mark_as_advanced(TinyXML2_INCLUDE_DIR TinyXML2_LIBRARY) # Output variables generation include(FindPackageHandleStandardArgs) find_package_handle_standard_args(TinyXML2 DEFAULT_MSG TinyXML2_LIBRARY TinyXML2_INCLUDE_DIR) set(TinyXML2_FOUND ${TINYXML2_FOUND}) # Enforce case-correctness: Set appropriately cased variable... unset(TINYXML2_FOUND) # ...and unset uppercase variable generated by find_package_handle_standard_args if(TinyXML2_FOUND) set(TinyXML2_INCLUDE_DIRS ${TinyXML2_INCLUDE_DIR}) set(TinyXML2_LIBRARIES ${TinyXML2_LIBRARY}) endif() kdl_parser-1.14.1/kdl_parser/include/000077500000000000000000000000001372076757400175175ustar00rootroot00000000000000kdl_parser-1.14.1/kdl_parser/include/kdl_parser/000077500000000000000000000000001372076757400216455ustar00rootroot00000000000000kdl_parser-1.14.1/kdl_parser/include/kdl_parser/kdl_parser.hpp000066400000000000000000000077021372076757400245120ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* Author: Wim Meeussen */ #ifndef KDL_PARSER__KDL_PARSER_HPP_ #define KDL_PARSER__KDL_PARSER_HPP_ #include #include #include #include #include // NOLINT #include "kdl_parser/visibility_control.hpp" namespace kdl_parser { /** Constructs a KDL tree from a file, given the file name * \param file The filename from where to read the xml * \param tree The resulting KDL Tree * returns true on success, false on failure */ KDL_PARSER_PUBLIC bool treeFromFile(const std::string & file, KDL::Tree & tree); /** Constructs a KDL tree from the parameter server, given the parameter name * \param param the name of the parameter on the parameter server * \param tree The resulting KDL Tree * returns true on success, false on failure or if built without ROS */ KDL_PARSER_PUBLIC bool treeFromParam(const std::string & param, KDL::Tree & tree); /** Constructs a KDL tree from a string containing xml * \param xml A string containing the xml description of the robot * \param tree The resulting KDL Tree * returns true on success, false on failure */ KDL_PARSER_PUBLIC bool treeFromString(const std::string & xml, KDL::Tree & tree); /** Constructs a KDL tree from a TinyXML2 document * \param[in] xml_doc The document containing the xml description of the robot * \param[out] tree The resulting KDL Tree * \return true on success, false on failure */ KDL_PARSER_PUBLIC bool treeFromXml(const tinyxml2::XMLDocument * xml_doc, KDL::Tree & tree); /** Constructs a KDL tree from a TinyXML document * \param[in] xml_doc The document containing the xml description of the robot * \param[out] tree The resulting KDL Tree * returns true on success, false on failure */ KDL_PARSER_PUBLIC KDL_PARSER_DEPRECATED("TinyXML API is deprecated, use the TinyXML2 version instead") bool treeFromXml(TiXmlDocument * xml_doc, KDL::Tree & tree); /** Constructs a KDL tree from a URDF robot model * \param robot_model The URDF robot model * \param tree The resulting KDL Tree * returns true on success, false on failure */ KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree); } // namespace kdl_parser #endif // KDL_PARSER__KDL_PARSER_HPP_ kdl_parser-1.14.1/kdl_parser/include/kdl_parser/visibility_control.hpp000066400000000000000000000067451372076757400263210ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2017, Open Source Robotics Foundation, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* This header must be included by all kdl_parser headers which declare symbols * which are defined in the kdl_parser library. When not building the kdl_parser * library, i.e. when using the headers in other package's code, the contents * of this header change the visibility of certain symbols which the kdl_parser * library cannot have, but the consuming code must have inorder to link. */ #ifndef KDL_PARSER__VISIBILITY_CONTROL_HPP_ #define KDL_PARSER__VISIBILITY_CONTROL_HPP_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ #define KDL_PARSER_EXPORT __attribute__ ((dllexport)) #define KDL_PARSER_IMPORT __attribute__ ((dllimport)) #define KDL_PARSER_DEPRECATED(msg) __attribute__((deprecated(msg))) #else #define KDL_PARSER_EXPORT __declspec(dllexport) #define KDL_PARSER_IMPORT __declspec(dllimport) #define KDL_PARSER_DEPRECATED(msg) __declspec(deprecated(msg)) #endif #ifdef KDL_PARSER_BUILDING_DLL #define KDL_PARSER_PUBLIC KDL_PARSER_EXPORT #else #define KDL_PARSER_PUBLIC KDL_PARSER_IMPORT #endif #define KDL_PARSER_PUBLIC_TYPE KDL_PARSER_PUBLIC #define KDL_PARSER_LOCAL #else #define KDL_PARSER_EXPORT __attribute__ ((visibility("default"))) #define KDL_PARSER_IMPORT #if __GNUC__ >= 4 #define KDL_PARSER_PUBLIC __attribute__ ((visibility("default"))) #define KDL_PARSER_LOCAL __attribute__ ((visibility("hidden"))) #else #define KDL_PARSER_PUBLIC #define KDL_PARSER_LOCAL #endif #define KDL_PARSER_PUBLIC_TYPE #define KDL_PARSER_DEPRECATED(msg) __attribute__((deprecated(msg))) #endif #endif // KDL_PARSER__VISIBILITY_CONTROL_HPP_ kdl_parser-1.14.1/kdl_parser/package.xml000066400000000000000000000026671372076757400202240ustar00rootroot00000000000000 kdl_parser 1.14.1 The Kinematics and Dynamics Library (KDL) defines a tree structure to represent the kinematic and dynamic parameters of a robot mechanism. kdl_parser provides tools to construct a KDL tree from an XML robot representation in URDF. Wim Meeussen Ioan Sucan Jackie Kay Chris Lalancette Shane Loretz BSD http://ros.org/wiki/kdl_parser https://github.com/ros/kdl_parser https://github.com/ros/kdl_parser/issues catkin liborocos-kdl-dev cmake_modules liburdfdom-headers-dev rosconsole liburdfdom-headers-dev rosconsole tinyxml tinyxml2 urdf roscpp rostest kdl_parser-1.14.1/kdl_parser/src/000077500000000000000000000000001372076757400166635ustar00rootroot00000000000000kdl_parser-1.14.1/kdl_parser/src/check_kdl_parser.cpp000066400000000000000000000063311372076757400226550ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* Author: Wim Meeussen */ #include #include #include "kdl_parser/kdl_parser.hpp" #include #include #include #include void printLink(const KDL::SegmentMap::const_iterator & link, const std::string & prefix) { std::cout << prefix << "- Segment " << GetTreeElementSegment(link->second).getName() << " has " << GetTreeElementChildren(link->second).size() << " children" << std::endl; for (unsigned int i = 0; i < GetTreeElementChildren(link->second).size(); i++) { printLink(GetTreeElementChildren(link->second)[i], prefix + " "); } } int main(int argc, char ** argv) { if (argc < 2) { std::cerr << "Expect xml file to parse" << std::endl; return -1; } urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDFFile(argv[1]); if (!robot_model) { std::cerr << "Could not generate robot model" << std::endl; return false; } KDL::Tree my_tree; if (!kdl_parser::treeFromUrdfModel(*robot_model, my_tree)) { std::cerr << "Could not extract kdl tree" << std::endl; return false; } // walk through tree std::cout << " ======================================" << std::endl; std::cout << " Tree has " << my_tree.getNrOfSegments() << " link(s) and a root link" << std::endl; std::cout << " ======================================" << std::endl; KDL::SegmentMap::const_iterator root = my_tree.getRootSegment(); printLink(root, ""); } kdl_parser-1.14.1/kdl_parser/src/kdl_parser.cpp000066400000000000000000000175071372076757400215270ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* Author: Wim Meeussen */ #include "kdl_parser/kdl_parser.hpp" #include #include #include #include #include #ifdef HAS_ROS #include #else // forward ROS warnings and errors to stderr #define ROS_DEBUG(...) fprintf(stdout, __VA_ARGS__); #define ROS_ERROR(...) fprintf(stderr, __VA_ARGS__); #define ROS_WARN(...) fprintf(stderr, __VA_ARGS__); #endif #ifdef HAS_URDF #include #include #endif namespace kdl_parser { // construct vector KDL::Vector toKdl(urdf::Vector3 v) { return KDL::Vector(v.x, v.y, v.z); } // construct rotation KDL::Rotation toKdl(urdf::Rotation r) { return KDL::Rotation::Quaternion(r.x, r.y, r.z, r.w); } // construct pose KDL::Frame toKdl(urdf::Pose p) { return KDL::Frame(toKdl(p.rotation), toKdl(p.position)); } // construct joint KDL::Joint toKdl(urdf::JointSharedPtr jnt) { KDL::Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform); switch (jnt->type) { case urdf::Joint::FIXED: { return KDL::Joint(jnt->name, KDL::Joint::None); } case urdf::Joint::REVOLUTE: { KDL::Vector axis = toKdl(jnt->axis); return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis); } case urdf::Joint::CONTINUOUS: { KDL::Vector axis = toKdl(jnt->axis); return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis); } case urdf::Joint::PRISMATIC: { KDL::Vector axis = toKdl(jnt->axis); return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::TransAxis); } default: { ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str()); return KDL::Joint(jnt->name, KDL::Joint::None); } } return KDL::Joint(); } // construct inertia KDL::RigidBodyInertia toKdl(urdf::InertialSharedPtr i) { KDL::Frame origin = toKdl(i->origin); // the mass is frame independent double kdl_mass = i->mass; // kdl and urdf both specify the com position in the reference frame of the link KDL::Vector kdl_com = origin.p; // kdl specifies the inertia matrix in the reference frame of the link, // while the urdf specifies the inertia matrix in the inertia reference frame KDL::RotationalInertia urdf_inertia = KDL::RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz); // Rotation operators are not defined for rotational inertia, // so we use the RigidBodyInertia operators (with com = 0) as a workaround KDL::RigidBodyInertia kdl_inertia_wrt_com_workaround = origin.M * KDL::RigidBodyInertia(0, KDL::Vector::Zero(), urdf_inertia); // Note that the RigidBodyInertia constructor takes the 3d inertia wrt the com // while the getRotationalInertia method returns the 3d inertia wrt the frame origin // (but having com = Vector::Zero() in kdl_inertia_wrt_com_workaround they match) KDL::RotationalInertia kdl_inertia_wrt_com = kdl_inertia_wrt_com_workaround.getRotationalInertia(); return KDL::RigidBodyInertia(kdl_mass, kdl_com, kdl_inertia_wrt_com); } // recursive function to walk through tree bool addChildrenToTree(urdf::LinkConstSharedPtr root, KDL::Tree & tree) { std::vector children = root->child_links; ROS_DEBUG("Link %s had %zu children", root->name.c_str(), children.size()); // constructs the optional inertia KDL::RigidBodyInertia inert(0); if (root->inertial) { inert = toKdl(root->inertial); } // constructs the kdl joint KDL::Joint jnt = toKdl(root->parent_joint); // construct the kdl segment KDL::Segment sgm(root->name, jnt, toKdl( root->parent_joint->parent_to_joint_origin_transform), inert); // add segment to tree tree.addSegment(sgm, root->parent_joint->parent_link_name); // recurslively add all children for (size_t i = 0; i < children.size(); i++) { if (!addChildrenToTree(children[i], tree)) { return false; } } return true; } bool treeFromFile(const std::string & file, KDL::Tree & tree) { const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDFFile(file); return kdl_parser::treeFromUrdfModel(*robot_model, tree); } bool treeFromParam(const std::string & param, KDL::Tree & tree) { #if defined(HAS_ROS) && defined(HAS_URDF) urdf::Model robot_model; if (!robot_model.initParam(param)){ ROS_ERROR("Could not generate robot model"); return false; } return treeFromUrdfModel(robot_model, tree); #else return false; #endif } bool treeFromString(const std::string & xml, KDL::Tree & tree) { const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDF(xml); if (!robot_model) { ROS_ERROR("Could not generate robot model"); return false; } return kdl_parser::treeFromUrdfModel(*robot_model, tree); } bool treeFromXml(const tinyxml2::XMLDocument * xml_doc, KDL::Tree & tree) { if (!xml_doc) { ROS_ERROR("Could not parse the xml document"); return false; } tinyxml2::XMLPrinter printer; xml_doc->Print(&printer); return treeFromString(printer.CStr(), tree); } bool treeFromXml(TiXmlDocument * xml_doc, KDL::Tree & tree) { if (!xml_doc) { ROS_ERROR("Could not parse the xml document"); return false; } std::stringstream ss; ss << *xml_doc; return treeFromString(ss.str(), tree); } bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree) { if (!robot_model.getRoot()) { return false; } tree = KDL::Tree(robot_model.getRoot()->name); // warn if root link has inertia. KDL does not support this if (robot_model.getRoot()->inertial) { ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not " "support a root link with an inertia. As a workaround, you can add an extra " "dummy link to your URDF.", robot_model.getRoot()->name.c_str()); } // add all children for (size_t i = 0; i < robot_model.getRoot()->child_links.size(); i++) { if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) { return false; } } return true; } } // namespace kdl_parser kdl_parser-1.14.1/kdl_parser/test/000077500000000000000000000000001372076757400170535ustar00rootroot00000000000000kdl_parser-1.14.1/kdl_parser/test/pr2_desc.xml000066400000000000000000004176051372076757400213130ustar00rootroot00000000000000 true 1000.0 true 1.0 5 power_state 10.0 87.78 -474 525 15.52 16.41 640 640 1 0.0 0.0 0.0 false -129.998394137 129.998394137 0.05 10.0 0.01 20 0.005 true 20 base_scan base_laser_link -79.2380952381 79.2380952381 -79.2380952381 -79.2380952381 79.2380952381 -79.2380952381 -79.2380952381 79.2380952381 -79.2380952381 -79.2380952381 79.2380952381 -79.2380952381 true base_link_geom 100.0 true 100.0 base_bumper true 100.0 base_link base_pose_ground_truth 0.01 map 25.7 25.7 0 0 0 0 base_footprint torso_lift_link_geom 100.0 true 100.0 torso_lift_bumper -52143.33 true 100.0 imu_link torso_lift_imu/data 2.89e-08 0 0 0 0 0 0 6.0 6.0 R8G8B8 2448 2050 45 0.1 100 20.0 true 20.0 /prosilica/image_raw /prosilica/camera_info /prosilica/request_image high_def_frame 1224.5 1224.5 1025.5 2955 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 640 480 BAYER_BGGR8 90 0.1 100 25.0 true 25.0 wide_stereo/left/image_raw wide_stereo/left/camera_info wide_stereo_optical_frame 0 320.5 320.5 240.5 320 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue 640 480 BAYER_BGGR8 90 0.1 100 25.0 true 25.0 wide_stereo/right/image_raw wide_stereo/right/camera_info wide_stereo_optical_frame 0.09 320.5 320.5 240.5 320 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue 640 480 L8 45 0.1 100 25.0 true 25.0 narrow_stereo/left/image_raw narrow_stereo/left/camera_info narrow_stereo_optical_frame 0 320.5 320.5 240.5 772.55 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue 640 480 L8 45 0.1 100 25.0 true 25.0 narrow_stereo/right/image_raw narrow_stereo/right/camera_info narrow_stereo_optical_frame 0.09 320.5 320.5 240.5 772.55 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue 640 640 1 0.0 0.0 0.0 false -79.9999999086 79.9999999086 0.05 10.0 0.01 40 0.005 true 40 tilt_scan laser_tilt_link -6.05 true 32.6525111499 r_shoulder_pan_link_geom 100.0 true 100.0 r_shoulder_pan_bumper true r_shoulder_lift_link_geom 100.0 true 100.0 r_r_shoulder_lift_bumper true 63.1552452977 61.8948225713 r_upper_arm_link_geom 100.0 true 100.0 r_upper_arm_bumper true true -90.5142857143 -1.0 r_elbow_flex_link_geom 100.0 true 100.0 r_elbow_flex_bumper true -36.167452007 true r_forearm_link_geom 100.0 true 100.0 r_forearm_bumper true r_wrist_flex_link_geom 100.0 true 100.0 r_wrist_flex_bumper true r_wrist_roll_link_geom 100.0 true 100.0 r_wrist_roll_bumper true r_gripper_l_finger_link_geom 100.0 true 100.0 r_gripper_l_finger_bumper true r_gripper_r_finger_link_geom 100.0 true 100.0 r_gripper_r_finger_bumper true false r_gripper_l_finger_tip_link_geom 100.0 true 100.0 r_gripper_l_finger_tip_bumper true false r_gripper_r_finger_tip_link_geom 100.0 true 100.0 r_gripper_r_finger_tip_bumper true 100.0 r_gripper_l_finger_link r_gripper_l_finger_pose_ground_truth 0.0 base_link true 100.0 r_gripper_l_finger_link r_gripper_l_finger_force_ground_truth r_gripper_l_finger_link r_gripper_r_parallel_link r_gripper_r_finger_tip_link r_gripper_r_finger_tip_link 0 0 1 -0.018 -0.021 0 r_gripper_l_parallel_link r_gripper_l_finger_tip_link r_gripper_l_finger_tip_link 0 0 1 -0.018 0.021 0 r_gripper_l_finger_tip_link r_gripper_r_finger_tip_link r_gripper_r_finger_tip_link 0 1 0 true true true r_gripper_palm_link_geom 100.0 true 100.0 r_gripper_palm_bumper true 100.0 r_gripper_palm_link r_gripper_palm_pose_ground_truth 0 0 0 0 0 0 0.0 map true 32.6525111499 l_shoulder_pan_link_geom 100.0 true 100.0 l_shoulder_pan_bumper true l_shoulder_lift_link_geom 100.0 true 100.0 l_r_shoulder_lift_bumper true 63.1552452977 61.8948225713 l_upper_arm_link_geom 100.0 true 100.0 l_upper_arm_bumper true true -90.5142857143 -1.0 l_elbow_flex_link_geom 100.0 true 100.0 l_elbow_flex_bumper true -36.167452007 true l_forearm_link_geom 100.0 true 100.0 l_forearm_bumper true l_wrist_flex_link_geom 100.0 true 100.0 l_wrist_flex_bumper true l_wrist_roll_link_geom 100.0 true 100.0 l_wrist_roll_bumper true l_gripper_l_finger_link_geom 100.0 true 100.0 l_gripper_l_finger_bumper true l_gripper_r_finger_link_geom 100.0 true 100.0 l_gripper_r_finger_bumper true false l_gripper_l_finger_tip_link_geom 100.0 true 100.0 l_gripper_l_finger_tip_bumper true false l_gripper_r_finger_tip_link_geom 100.0 true 100.0 l_gripper_r_finger_tip_bumper true 100.0 l_gripper_l_finger_link l_gripper_l_finger_pose_ground_truth 0.0 base_link true 100.0 l_gripper_l_finger_link l_gripper_l_finger_force_ground_truth l_gripper_l_finger_link l_gripper_r_parallel_link l_gripper_r_finger_tip_link l_gripper_r_finger_tip_link 0 0 1 -0.018 -0.021 0 l_gripper_l_parallel_link l_gripper_l_finger_tip_link l_gripper_l_finger_tip_link 0 0 1 -0.018 0.021 0 l_gripper_l_finger_tip_link l_gripper_r_finger_tip_link l_gripper_r_finger_tip_link 0 1 0 true true true l_gripper_palm_link_geom 100.0 true 100.0 l_gripper_palm_bumper true 100.0 l_gripper_palm_link l_gripper_palm_pose_ground_truth 0 0 0 0 0 0 0.0 map 640 480 L8 90 0.1 100 25.0 true 25.0 l_forearm_cam/image_raw l_forearm_cam/camera_info l_forearm_cam_optical_frame 0 320.5 320.5 240.5 320 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue 640 480 L8 90 0.1 100 25.0 true 25.0 r_forearm_cam/image_raw r_forearm_cam/camera_info r_forearm_cam_optical_frame 0 320.5 320.5 240.5 320 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue kdl_parser-1.14.1/kdl_parser/test/pr2_desc_bracket.xml000066400000000000000000000013411372076757400227700ustar00rootroot00000000000000 kdl_parser-1.14.1/kdl_parser/test/pr2_desc_bracket2.xml000066400000000000000000000013401372076757400230510ustar00rootroot00000000000000 kdl_parser-1.14.1/kdl_parser/test/pr2_desc_vector.xml000066400000000000000000000014261372076757400226630ustar00rootroot00000000000000 kdl_parser-1.14.1/kdl_parser/test/testInertiaRPYmodel1.urdf000066400000000000000000000016341372076757400237310ustar00rootroot00000000000000 kdl_parser-1.14.1/kdl_parser/test/testInertiaRPYmodel2.urdf000066400000000000000000000016531372076757400237330ustar00rootroot00000000000000 kdl_parser-1.14.1/kdl_parser/test/test_inertia_rpy.cpp000066400000000000000000000077521372076757400231560ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2015 Open Source Robotics Foundation, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* Author: Jackie Kay */ #include #include #include #include #include #include #include #include "kdl_parser/kdl_parser.hpp" int g_argc; char ** g_argv; class TestInertiaRPY : public testing::Test { public: protected: /// constructor TestInertiaRPY() { } /// Destructor ~TestInertiaRPY() { } }; TEST_F(TestInertiaRPY, test_torques) { // workaround for segfault issue with parsing 2 trees instantiated on the stack KDL::Tree * tree_1 = new KDL::Tree; KDL::Tree * tree_2 = new KDL::Tree; KDL::JntArray torques_1; KDL::JntArray torques_2; { ASSERT_TRUE(kdl_parser::treeFromFile(g_argv[1], *tree_1)); KDL::Vector gravity(0, 0, -9.81); KDL::Chain chain; std::cout << "number of joints: " << tree_1->getNrOfJoints() << std::endl; std::cout << "number of segments: " << tree_1->getNrOfSegments() << std::endl; ASSERT_TRUE(tree_1->getChain("base_link", "link2", chain)); KDL::ChainIdSolver_RNE solver(chain, gravity); KDL::JntArray q(chain.getNrOfJoints()); KDL::JntArray qdot(chain.getNrOfJoints()); KDL::JntArray qdotdot(chain.getNrOfJoints()); std::vector wrenches(chain.getNrOfJoints()); solver.CartToJnt(q, qdot, qdotdot, wrenches, torques_1); delete tree_1; tree_1 = NULL; } { ASSERT_TRUE(kdl_parser::treeFromFile(g_argv[2], *tree_2)); KDL::Vector gravity(0, 0, -9.81); KDL::Chain chain; ASSERT_TRUE(tree_2->getChain("base_link", "link2", chain)); KDL::ChainIdSolver_RNE solver(chain, gravity); KDL::JntArray q(chain.getNrOfJoints()); KDL::JntArray qdot(chain.getNrOfJoints()); KDL::JntArray qdotdot(chain.getNrOfJoints()); std::vector wrenches(chain.getNrOfJoints()); solver.CartToJnt(q, qdot, qdotdot, wrenches, torques_2); delete tree_2; tree_2 = NULL; } ASSERT_TRUE(torques_1 == torques_2); SUCCEED(); } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test_kdl_parser"); for (size_t i = 0; i < argc; ++i) { std::cout << argv[i] << std::endl; } g_argc = argc; g_argv = argv; return RUN_ALL_TESTS(); } kdl_parser-1.14.1/kdl_parser/test/test_inertia_rpy.launch000066400000000000000000000003471372076757400236370ustar00rootroot00000000000000 kdl_parser-1.14.1/kdl_parser/test/test_kdl_parser.cpp000066400000000000000000000060151372076757400227460ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* Author: Wim Meeussen */ #include #include #include "kdl_parser/kdl_parser.hpp" int g_argc; char ** g_argv; class TestParser : public testing::Test { public: KDL::Tree my_tree; protected: /// constructor TestParser() { } /// Destructor ~TestParser() { } }; TEST_F(TestParser, test) { for (int i = 1; i < g_argc - 2; i++) { ROS_ERROR("Testing file %s", g_argv[i]); ASSERT_FALSE(kdl_parser::treeFromFile(g_argv[i], my_tree)); } ASSERT_TRUE(kdl_parser::treeFromFile(g_argv[g_argc - 1], my_tree)); ASSERT_EQ(8, my_tree.getNrOfJoints()); ASSERT_EQ(16, my_tree.getNrOfSegments()); ASSERT_TRUE(my_tree.getSegment("dummy_link") == my_tree.getRootSegment()); ASSERT_EQ((unsigned int)1, my_tree.getRootSegment()->second.children.size()); ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment()); ASSERT_EQ(10.0, my_tree.getSegment("base_link")->second.segment.getInertia().getMass()); ASSERT_NEAR(1.000, my_tree.getSegment( "base_link")->second.segment.getInertia().getRotationalInertia().data[0], 0.001); SUCCEED(); } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test_kdl_parser"); g_argc = argc; g_argv = argv; return RUN_ALL_TESTS(); } kdl_parser-1.14.1/kdl_parser/test/test_kdl_parser.launch000066400000000000000000000002201372076757400234260ustar00rootroot00000000000000 kdl_parser-1.14.1/kdl_parser/test/test_robot.urdf000066400000000000000000000256231372076757400221310ustar00rootroot00000000000000 kdl_parser-1.14.1/kdl_parser_py/000077500000000000000000000000001372076757400166045ustar00rootroot00000000000000kdl_parser-1.14.1/kdl_parser_py/CHANGELOG.rst000066400000000000000000000121161372076757400206260ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package kdl_parser_py ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.14.1 (2020-08-24) ------------------- * Drop CATKIN_IGNORE (`#42 `_) * Contributors: Jochen Sprickerhof 1.14.0 (2020-04-13) ------------------- * Used keys for Orocos (`#38 `_) * Contributors: Alejandro Hernández Cordero 1.13.1 (2018-07-23) ------------------- * Remove the declaration of a library from kdl_parser_py. (`#14 `_) * Remove unused kdl_parser_py.urdf. (`#17 `_) * Contributors: Chris Lalancette 1.13.0 (2018-04-05) ------------------- * Make rostest a test_depend (`#3 `_) * update links now that this is in its own repo * Contributors: Chris Lalancette, Mikael Arguedas 1.12.10 (2017-05-17) -------------------- 1.12.9 (2017-04-26) ------------------- * Unset origin xyz or rpy is now properly interpreted as zero vector (`#187 `_) * Contributors: eugene-katsevman 1.12.8 (2017-03-27) ------------------- * Switch a couple more packages over to Chris and Shane. * Contributors: Chris Lalancette 1.12.7 (2017-01-26) ------------------- 1.12.6 (2017-01-04) ------------------- 1.12.5 (2016-10-27) ------------------- 1.12.4 (2016-08-23) ------------------- 1.12.3 (2016-06-10) ------------------- 1.12.2 (2016-04-12) ------------------- 1.12.1 (2016-04-10) ------------------- * Remove cmake_modules dependency * Contributors: Jackie Kay 1.12.0 (2016-04-04) ------------------- 1.11.10 (2016-02-23) -------------------- * another patch release * Remove cmake_modules dependency * Contributors: Jackie Kay 1.11.9 (2016-02-22) ------------------- * Changelog * kdl_parser_py: run_depend on urdfdom_py * Remove dependency in urdf_parser_py * kdl_parser: Adding python kdl parser * Contributors: Jackie Kay, Jonathan Bohren, Steven Peters 1.11.8 (2015-09-11) ------------------- 1.11.7 (2015-04-22) ------------------- 1.11.6 (2014-11-30 11:17) ------------------------- 1.11.5 (2014-07-24) ------------------- 1.11.4 (2014-07-07) ------------------- 1.11.3 (2014-06-24) ------------------- 1.11.2 (2014-03-22) ------------------- 1.11.1 (2014-03-20) ------------------- 1.11.0 (2014-02-21) ------------------- 1.10.21 (2014-11-30 11:15) -------------------------- 1.10.20 (2014-08-01) -------------------- 1.10.19 (2014-02-15) -------------------- 1.10.18 (2013-12-04) -------------------- 1.10.17 (2013-11-22) -------------------- 1.10.16 (2013-11-18) -------------------- 1.10.15 (2013-08-17) -------------------- 1.10.14 (2013-07-26) -------------------- 1.10.13 (2013-07-17) -------------------- 1.10.12 (2013-07-02 08:41) -------------------------- 1.10.11 (2013-05-21) -------------------- 1.10.10 (2013-05-20) -------------------- 1.10.9 (2013-04-21) ------------------- 1.10.8 (2013-04-18 12:24) ------------------------- 1.10.7 (2013-04-18 10:19) ------------------------- 1.10.6 (2013-04-18 01:56) ------------------------- 1.10.5 (2013-04-18 01:46) ------------------------- 1.10.4 (2013-04-12) ------------------- 1.10.3 (2013-03-13 22:22) ------------------------- 1.10.2 (2013-03-13 17:34) ------------------------- 1.10.1 (2013-03-13 17:15) ------------------------- 1.10.0 (2013-03-11 19:48) ------------------------- 1.9.36 (2013-07-02 08:38) ------------------------- 1.9.35 (2013-04-29) ------------------- 1.9.34 (2013-04-18 18:17) ------------------------- 1.9.33 (2013-03-11 19:49) ------------------------- 1.9.32 (2012-12-22) ------------------- 1.9.31 (2012-12-18) ------------------- 1.9.30 (2012-12-14) ------------------- 1.9.29 (2012-12-05) ------------------- 1.9.28 (2012-11-07) ------------------- 1.9.27 (2012-11-06) ------------------- 1.9.26 (2012-11-05) ------------------- 1.9.25 (2012-10-29) ------------------- 1.9.24 (2012-10-25) ------------------- 1.9.23 (2012-10-14 15:26) ------------------------- 1.9.22 (2012-10-14 13:13) ------------------------- 1.9.21 (2012-10-14 12:25) ------------------------- 1.9.20 (2012-10-14 02:13) ------------------------- 1.9.19 (2012-10-13) ------------------- 1.9.18 (2012-10-07) ------------------- 1.9.17 (2012-10-06 21:27) ------------------------- 1.9.16 (2012-10-06 21:22) ------------------------- 1.9.15 (2012-10-06 20:47) ------------------------- 1.9.14 (2012-10-06 19:20) ------------------------- 1.9.13 (2012-09-16 16:51) ------------------------- 1.9.12 (2012-09-16 02:25) ------------------------- 1.9.11 (2012-09-15 13:45) ------------------------- 1.9.10 (2012-09-15 12:27) ------------------------- 1.9.9 (2012-09-12 14:38) ------------------------ 1.9.8 (2012-09-12 14:27) ------------------------ 1.9.7 (2012-09-11) ------------------ 1.9.6 (2012-09-07) ------------------ 1.9.5 (2012-09-06) ------------------ 1.9.4 (2012-09-04) ------------------ 1.9.3 (2012-09-03) ------------------ 1.9.2 (2012-08-14 20:34) ------------------------ 1.9.1 (2012-08-14 20:33) ------------------------ 1.9.0 (2012-08-02) ------------------ kdl_parser-1.14.1/kdl_parser_py/CMakeLists.txt000066400000000000000000000006151372076757400213460ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(kdl_parser_py) find_package(catkin REQUIRED) catkin_package( CATKIN_DEPENDS urdfdom_py ) catkin_python_setup() catkin_install_python(PROGRAMS ${PROJECT_NAME}/urdf.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) if(CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS rostest) add_rostest(test/test_kdl_parser.launch) endif() kdl_parser-1.14.1/kdl_parser_py/kdl_parser_py/000077500000000000000000000000001372076757400214425ustar00rootroot00000000000000kdl_parser-1.14.1/kdl_parser_py/kdl_parser_py/__init__.py000066400000000000000000000000001372076757400235410ustar00rootroot00000000000000kdl_parser-1.14.1/kdl_parser_py/kdl_parser_py/urdf.py000066400000000000000000000074661372076757400227710ustar00rootroot00000000000000from __future__ import print_function import urdf_parser_py.urdf as urdf import PyKDL as kdl def treeFromFile(filename): """ Construct a PyKDL.Tree from an URDF file. :param filename: URDF file path """ with open(filename) as urdf_file: return treeFromUrdfModel(urdf.URDF.from_xml_string(urdf_file.read())) def treeFromParam(param): """ Construct a PyKDL.Tree from an URDF in a ROS parameter. :param param: Parameter name, ``str`` """ return treeFromUrdfModel(urdf.URDF.from_parameter_server(param)) def treeFromString(xml): """ Construct a PyKDL.Tree from an URDF xml string. :param xml: URDF xml string, ``str`` """ return treeFromUrdfModel(urdf.URDF.from_xml_string(xml)) def _toKdlPose(pose): # URDF might have RPY OR XYZ unspecified. Both default to zeros rpy = pose.rpy if pose and pose.rpy and len(pose.rpy) == 3 else [0, 0, 0] xyz = pose.xyz if pose and pose.xyz and len(pose.xyz) == 3 else [0, 0, 0] return kdl.Frame( kdl.Rotation.RPY(*rpy), kdl.Vector(*xyz)) def _toKdlInertia(i): # kdl specifies the inertia in the reference frame of the link, the urdf # specifies the inertia in the inertia reference frame origin = _toKdlPose(i.origin) inertia = i.inertia return origin.M * kdl.RigidBodyInertia( i.mass, origin.p, kdl.RotationalInertia(inertia.ixx, inertia.iyy, inertia.izz, inertia.ixy, inertia.ixz, inertia.iyz)); def _toKdlJoint(jnt): fixed = lambda j,F: kdl.Joint(j.name, getattr(kdl.Joint, 'None')) rotational = lambda j,F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.RotAxis) translational = lambda j,F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.TransAxis) type_map = { 'fixed': fixed, 'revolute': rotational, 'continuous': rotational, 'prismatic': translational, 'floating': fixed, 'planar': fixed, 'unknown': fixed, } return type_map[jnt.type](jnt, _toKdlPose(jnt.origin)) def _add_children_to_tree(robot_model, root, tree): # constructs the optional inertia inert = kdl.RigidBodyInertia(0) if root.inertial: inert = _toKdlInertia(root.inertial) # constructs the kdl joint (parent_joint_name, parent_link_name) = robot_model.parent_map[root.name] parent_joint = robot_model.joint_map[parent_joint_name] # construct the kdl segment sgm = kdl.Segment( root.name, _toKdlJoint(parent_joint), _toKdlPose(parent_joint.origin), inert) # add segment to tree if not tree.addSegment(sgm, parent_link_name): return False if root.name not in robot_model.child_map: return True children = [robot_model.link_map[l] for (j,l) in robot_model.child_map[root.name]] # recurslively add all children for child in children: if not _add_children_to_tree(robot_model, child, tree): return False return True; def treeFromUrdfModel(robot_model, quiet=False): """ Construct a PyKDL.Tree from an URDF model from urdf_parser_python. :param robot_model: URDF xml string, ``str`` :param quiet: If true suppress messages to stdout, ``bool`` """ root = robot_model.link_map[robot_model.get_root()] if root.inertial and not quiet: print("The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF." % root.name); ok = True tree = kdl.Tree(root.name) # add all children for (joint,child) in robot_model.child_map[root.name]: if not _add_children_to_tree(robot_model, robot_model.link_map[child], tree): ok = False break return (ok, tree) kdl_parser-1.14.1/kdl_parser_py/package.xml000066400000000000000000000032071372076757400207230ustar00rootroot00000000000000 kdl_parser_py 1.14.1 The Kinematics and Dynamics Library (KDL) defines a tree structure to represent the kinematic and dynamic parameters of a robot mechanism. kdl_parser_py provides Python tools to construct a KDL tree from an XML robot representation in URDF. Jonathan Bohren Jackie Kay Chris Lalancette Shane Loretz BSD http://ros.org/wiki/kdl_parser_py https://github.com/ros/kdl_parser https://github.com/ros/kdl_parser/issues catkin python-setuptools python3-setuptools python-catkin-pkg python3-catkin-pkg urdfdom_py urdfdom_py python3-pykdl rostest kdl_parser-1.14.1/kdl_parser_py/setup.py000066400000000000000000000002741372076757400203210ustar00rootroot00000000000000#!/usr/bin/env python from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['kdl_parser_py'] ) setup(**d) kdl_parser-1.14.1/kdl_parser_py/test/000077500000000000000000000000001372076757400175635ustar00rootroot00000000000000kdl_parser-1.14.1/kdl_parser_py/test/test.urdf000066400000000000000000000146301372076757400214300ustar00rootroot00000000000000 kdl_parser-1.14.1/kdl_parser_py/test/test_kdl_parser.launch000066400000000000000000000002231372076757400241410ustar00rootroot00000000000000 kdl_parser-1.14.1/kdl_parser_py/test/test_kdl_parser.py000077500000000000000000000025321372076757400233270ustar00rootroot00000000000000#!/usr/bin/env python import sys import kdl_parser_py.urdf import unittest import rostest PKG = "kdl_parser_py" NAME = "test_kdl_parser" class TestKdlParser(unittest.TestCase): def runTest(self): filename = None if (len(sys.argv) > 1): filename = sys.argv[1] else: self.fail("Expected filename!") (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename) self.assertTrue(ok) # KDL doesn't count fixed joints (since they aren't kinematic) self.assertEqual(tree.getNrOfJoints(), 8) # KDL doesn't count base link (since it's attached by fixed links self.assertEqual(tree.getNrOfSegments(), 10) chain = tree.getChain("base_link", "right_gripper") self.assertEqual(chain.getNrOfSegments(), 2) self.assertEqual(chain.getNrOfJoints(), 2) self.assertEqual(chain.getSegment(0).getName(), "gripper_pole") self.assertEqual(chain.getSegment(0).getJoint().getName(), "gripper_extension") self.assertEqual(chain.getSegment(1).getName(), "right_gripper") self.assertEqual(chain.getSegment(1).getJoint().getName(), "right_gripper_joint") inertia = chain.getSegment(1).getInertia() self.assertAlmostEqual(inertia.getCOG().z(), 3.0) if __name__ == '__main__': rostest.run(PKG, NAME, TestKdlParser, sys.argv)