urdfdom-0.4.1/000077500000000000000000000000001265476635200131745ustar00rootroot00000000000000urdfdom-0.4.1/.travis.yml000066400000000000000000000006031265476635200153040ustar00rootroot00000000000000sudo: required dist: trusty # Force travis to use its minimal image with default Python settings language: generic compiler: - clang - gcc script: "./.travis/build" before_install: - sudo apt-get update -qq - sudo apt-get install -qq libboost-system-dev libboost-thread-dev libboost-test-dev libtinyxml-dev python-yaml python-mock matrix: allow_failures: - compiler: clang urdfdom-0.4.1/CMakeLists.txt000066400000000000000000000062061265476635200157400ustar00rootroot00000000000000cmake_minimum_required( VERSION 2.8 FATAL_ERROR ) project (urdfdom CXX C) set (URDF_MAJOR_VERSION 0) set (URDF_MINOR_VERSION 4) set (URDF_PATCH_VERSION 0) set (URDF_VERSION ${URDF_MAJOR_VERSION}.${URDF_MINOR_VERSION}.${URDF_PATCH_VERSION}) set (URDF_MAJOR_MINOR_VERSION ${URDF_MAJOR_VERSION}.${URDF_MINOR_VERSION}) message (STATUS "${PROJECT_NAME} version ${URDF_VERSION}") include(GNUInstallDirs) # hack: by default this would be 'lib/x86_64-linux-gnu' set(CMAKE_INSTALL_LIBDIR lib) # set the default build type if (NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() # If compiler support symbol visibility, enable it. include(CheckCCompilerFlag) check_c_compiler_flag(-fvisibility=hidden HAS_VISIBILITY) if (HAS_VISIBILITY) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fvisibility=hidden") endif() # This shouldn't be necessary, but there has been trouble # with MSVC being set off, but MSVCXX ON. if(MSVC OR MSVC90 OR MSVC10) set(MSVC ON) endif (MSVC OR MSVC90 OR MSVC10) set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") find_package(TinyXML REQUIRED) include_directories(SYSTEM ${TinyXML_INCLUDE_DIRS}) find_package(urdfdom_headers 0.4 REQUIRED) include_directories(SYSTEM ${urdfdom_headers_INCLUDE_DIRS}) find_package(console_bridge 0.3 REQUIRED) include_directories(SYSTEM ${console_bridge_INCLUDE_DIRS}) link_directories(${console_bridge_LIBRARY_DIRS}) find_package(Boost REQUIRED system thread unit_test_framework) include_directories(${Boost_INCLUDE_DIR}) link_directories(${Boost_LIBRARY_DIRS}) #In Visual Studio a special postfix for #libraries compiled in debug is used if(MSVC) set(CMAKE_DEBUG_POSTFIX "d") endif(MSVC) enable_testing() add_subdirectory(urdf_parser) if(WIN32 AND NOT CYGWIN) set(CMAKE_CONFIG_INSTALL_DIR CMake) else() set(CMAKE_CONFIG_INSTALL_DIR ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME}/cmake/) endif() set(PKG_NAME ${PROJECT_NAME}) set(PKG_LIBRARIES urdfdom_sensor urdfdom_model_state urdfdom_model urdfdom_world) set(PKG_DEPENDS urdfdom_headers console_bridge) set(cmake_conf_file "cmake/urdfdom-config.cmake") configure_file("${CMAKE_CURRENT_SOURCE_DIR}/${cmake_conf_file}.in" "${CMAKE_BINARY_DIR}/${cmake_conf_file}" @ONLY) install(FILES ${CMAKE_BINARY_DIR}/${cmake_conf_file} DESTINATION ${CMAKE_CONFIG_INSTALL_DIR} COMPONENT cmake) # Make the package config file if (NOT MSVC) set(PKG_DESC "Unified Robot Description Format") set(PKG_DEPENDS "urdfdom_headers console_bridge") # make the list separated by spaces instead of ; set(PKG_URDF_LIBS "-lurdfdom_sensor -lurdfdom_model_state -lurdfdom_model -lurdfdom_world") set(pkg_conf_file "cmake/pkgconfig/urdfdom.pc") configure_file("${CMAKE_CURRENT_SOURCE_DIR}/${pkg_conf_file}.in" "${CMAKE_BINARY_DIR}/${pkg_conf_file}" @ONLY) install(FILES ${CMAKE_BINARY_DIR}/${pkg_conf_file} DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig/ COMPONENT pkgconfig) endif() add_subdirectory(urdf_parser_py) message(STATUS "Configuration successful. Type make to compile urdfdom") SET_DIRECTORY_PROPERTIES(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES ${CMAKE_BINARY_DIR}/cmake/urdfdom-config.cmake ADDITIONAL_MAKE_CLEAN_FILES ${CMAKE_BINARY_DIR}/cmake/pkgconfig/urdfdom.pc) urdfdom-0.4.1/LICENSE000066400000000000000000000033561265476635200142100ustar00rootroot00000000000000/********************************************************************* * 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. *********************************************************************/ urdfdom-0.4.1/README.md000066400000000000000000000044201265476635200144530ustar00rootroot00000000000000urdfdom =========== The URDF (U-Robot Description Format) library provides core data structures and a simple XML parsers for populating the class data structures from an URDF file. For now, the details of the URDF specifications reside on http://ros.org/wiki/urdf ### Build Status [![Build Status](https://travis-ci.org/ros/urdfdom.png)](https://travis-ci.org/ros/urdfdom) ### Using with ROS If you choose to check this repository out for use with ROS, be aware that the necessary ``package.xml`` is not included in this repo but instead is added in during the ROS release process. To emulate this, pull the appropriate file into this repository using the following format. Be sure to replace the ALLCAPS words with the apropriate terms: ``` wget https://raw.github.com/ros-gbp/urdfdom-release/debian/ROS_DISTRO/UBUNTU_DISTRO/urdfdom/package.xml ``` For example: ``` wget https://raw.github.com/ros-gbp/urdfdom-release/debian/hydro/precise/urdfdom/package.xml ``` ### Installing from Source with ROS Debians **Warning: this will break ABI compatibility with future /opt/ros updates through the debian package manager. This is a hack, use at your own risk.** If you want to install urdfdom from source, but not install all of ROS from source, you can follow these loose guidelines. This is not best practice for installing, but works. This version is for ROS Hydro but should be easily customized for future version of ROS: ``` sudo mv /opt/ros/hydro/include/urdf_parser/ /opt/ros/hydro/include/_urdf_parser/ sudo mv /opt/ros/hydro/lib/liburdfdom_model.so /opt/ros/hydro/lib/_liburdfdom_model.so sudo mv /opt/ros/hydro/lib/liburdfdom_model_state.so /opt/ros/hydro/lib/_liburdfdom_model_state.so sudo mv /opt/ros/hydro/lib/liburdfdom_sensor.so /opt/ros/hydro/lib/_liburdfdom_sensor.so sudo mv /opt/ros/hydro/lib/liburdfdom_world.so /opt/ros/hydro/lib/_liburdfdom_world.so sudo mv /opt/ros/hydro/lib/pkgconfig/urdfdom.pc /opt/ros/hydro/lib/pkgconfig/_urdfdom.pc sudo mv /opt/ros/hydro/share/urdfdom/cmake/urdfdom-config.cmake /opt/ros/hydro/share/urdfdom/cmake/_urdfdom-config.cmake cd ~/ros/urdfdom # where the git repo was checked out mkdir -p build cd build cmake ../ -DCMAKE_INSTALL_PREFIX=/opt/ros/hydro make -j8 sudo make install # now rebuild your catkin workspace cd ~/ros/ws_catkin catkin_make ``` urdfdom-0.4.1/cmake/000077500000000000000000000000001265476635200142545ustar00rootroot00000000000000urdfdom-0.4.1/cmake/FindTinyXML.cmake000066400000000000000000000054151265476635200173700ustar00rootroot00000000000000################################################################################################## # # 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() urdfdom-0.4.1/cmake/pkgconfig/000077500000000000000000000000001265476635200162235ustar00rootroot00000000000000urdfdom-0.4.1/cmake/pkgconfig/urdfdom.pc.in000066400000000000000000000004601265476635200206140ustar00rootroot00000000000000# This file was generated by CMake for @PROJECT_NAME@ prefix=@CMAKE_INSTALL_PREFIX@ exec_prefix=${prefix} libdir=${prefix}/lib includedir=${prefix}/include Name: @PKG_NAME@ Description: @PKG_DESC@ Version: @URDF_VERSION@ Requires: @PKG_DEPENDS@ Libs: -L${libdir} @PKG_URDF_LIBS@ Cflags: -I${includedir} urdfdom-0.4.1/cmake/urdfdom-config.cmake.in000066400000000000000000000014031265476635200205640ustar00rootroot00000000000000if (@PKG_NAME@_CONFIG_INCLUDED) return() endif() set(@PKG_NAME@_CONFIG_INCLUDED TRUE) set(@PKG_NAME@_INCLUDE_DIRS "@CMAKE_INSTALL_PREFIX@/include" "@Boost_INCLUDE_DIR@" "@TinyXML_INCLUDE_DIRS@") foreach(lib @PKG_LIBRARIES@) set(onelib "${lib}-NOTFOUND") find_library(onelib ${lib} PATHS "@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@" NO_DEFAULT_PATH ) if(NOT onelib) message(FATAL_ERROR "Library '${lib}' in package @PKG_NAME@ is not installed properly") endif() list(APPEND @PKG_NAME@_LIBRARIES ${onelib}) endforeach() foreach(dep @PKG_DEPENDS@) if(NOT ${dep}_FOUND) find_package(${dep}) endif() list(APPEND @PKG_NAME@_INCLUDE_DIRS ${${dep}_INCLUDE_DIRS}) list(APPEND @PKG_NAME@_LIBRARIES ${${dep}_LIBRARIES}) endforeach() urdfdom-0.4.1/urdf_parser/000077500000000000000000000000001265476635200155105ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser/CMakeLists.txt000066400000000000000000000042471265476635200202570ustar00rootroot00000000000000include_directories(include) add_library(urdfdom_world SHARED src/pose.cpp src/model.cpp src/link.cpp src/joint.cpp src/world.cpp) target_link_libraries(urdfdom_world ${TinyXML_LIBRARIES} ${console_bridge_LIBRARIES} ${Boost_LIBRARIES}) set_target_properties(urdfdom_world PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERSION}) add_library(urdfdom_model SHARED src/pose.cpp src/model.cpp src/link.cpp src/joint.cpp) target_link_libraries(urdfdom_model ${TinyXML_LIBRARIES} ${console_bridge_LIBRARIES} ${Boost_LIBRARIES}) set_target_properties(urdfdom_model PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERSION}) add_library(urdfdom_sensor SHARED src/urdf_sensor.cpp) target_link_libraries(urdfdom_sensor urdfdom_model ${TinyXML_LIBRARIES} ${console_bridge_LIBRARIES} ${Boost_LIBRARIES}) set_target_properties(urdfdom_sensor PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERSION}) add_library(urdfdom_model_state SHARED src/urdf_model_state.cpp src/twist.cpp) target_link_libraries(urdfdom_model_state ${TinyXML_LIBRARIES} ${console_bridge_LIBRARIES} ${Boost_LIBRARIES}) set_target_properties(urdfdom_model_state PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERSION}) # -------------------------------- add_executable(check_urdf src/check_urdf.cpp) target_link_libraries(check_urdf urdfdom_model urdfdom_world) add_executable(urdf_to_graphiz src/urdf_to_graphiz.cpp) target_link_libraries(urdf_to_graphiz urdfdom_model) # urdf_mem_test is a binary for testing, not a unit test add_executable(urdf_mem_test test/memtest.cpp) target_link_libraries(urdf_mem_test urdfdom_model) # unit test to fix geometry problems add_executable(urdf_unit_test test/urdf_unit_test.cpp) target_link_libraries(urdf_unit_test urdfdom_model ${Boost_LIBRARIES}) add_test(urdf_unit_test urdf_unit_test) INSTALL(TARGETS urdfdom_model DESTINATION ${CMAKE_INSTALL_LIBDIR}) INSTALL(TARGETS urdfdom_world DESTINATION ${CMAKE_INSTALL_LIBDIR}) INSTALL(TARGETS check_urdf urdf_to_graphiz urdf_mem_test DESTINATION ${CMAKE_INSTALL_BINDIR}) INSTALL(TARGETS urdfdom_sensor DESTINATION ${CMAKE_INSTALL_LIBDIR}) INSTALL(TARGETS urdfdom_model_state DESTINATION ${CMAKE_INSTALL_LIBDIR}) INSTALL(DIRECTORY include/urdf_parser DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) urdfdom-0.4.1/urdf_parser/include/000077500000000000000000000000001265476635200171335ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser/include/urdf_parser/000077500000000000000000000000001265476635200214475ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser/include/urdf_parser/exportdecl.h000066400000000000000000000070071265476635200237750ustar00rootroot00000000000000/********************************************************************* * 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: Thomas Moulard */ #ifndef URDFDOM_EXPORTDECL_H # define URDFDOM_EXPORTDECL_H // Handle portable symbol export. // Defining manually which symbol should be exported is required // under Windows whether MinGW or MSVC is used. // // The headers then have to be able to work in two different modes: // - dllexport when one is building the library, // - dllimport for clients using the library. // // On Linux, set the visibility accordingly. If C++ symbol visibility // is handled by the compiler, see: http://gcc.gnu.org/wiki/Visibility # if defined _WIN32 || defined __CYGWIN__ // On Microsoft Windows, use dllimport and dllexport to tag symbols. # define URDFDOM_DLLIMPORT __declspec(dllimport) # define URDFDOM_DLLEXPORT __declspec(dllexport) # define URDFDOM_DLLLOCAL # else // On Linux, for GCC >= 4, tag symbols using GCC extension. # if __GNUC__ >= 4 # define URDFDOM_DLLIMPORT __attribute__ ((visibility("default"))) # define URDFDOM_DLLEXPORT __attribute__ ((visibility("default"))) # define URDFDOM_DLLLOCAL __attribute__ ((visibility("hidden"))) # else // Otherwise (GCC < 4 or another compiler is used), export everything. # define URDFDOM_DLLIMPORT # define URDFDOM_DLLEXPORT # define URDFDOM_DLLLOCAL # endif // __GNUC__ >= 4 # endif // defined _WIN32 || defined __CYGWIN__ # ifdef URDFDOM_STATIC // If one is using the library statically, get rid of // extra information. # define URDFDOM_DLLAPI # define URDFDOM_LOCAL # else // Depending on whether one is building or using the // library define DLLAPI to import or export. # ifdef URDFDOM_EXPORTS # define URDFDOM_DLLAPI URDFDOM_DLLEXPORT # else # define URDFDOM_DLLAPI URDFDOM_DLLIMPORT # endif // URDFDOM_EXPORTS # define URDFDOM_LOCAL URDFDOM_DLLLOCAL # endif // URDFDOM_STATIC #endif //! URDFDOM_EXPORTDECL_H urdfdom-0.4.1/urdf_parser/include/urdf_parser/urdf_parser.h000066400000000000000000000053701265476635200241410ustar00rootroot00000000000000/********************************************************************* * 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 URDF_PARSER_URDF_PARSER_H #define URDF_PARSER_URDF_PARSER_H #include #include #include #include #include #include #include "exportdecl.h" namespace urdf_export_helpers { URDFDOM_DLLAPI std::string values2str(unsigned int count, const double *values, double (*conv)(double) = NULL); URDFDOM_DLLAPI std::string values2str(urdf::Vector3 vec); URDFDOM_DLLAPI std::string values2str(urdf::Rotation rot); URDFDOM_DLLAPI std::string values2str(urdf::Color c); URDFDOM_DLLAPI std::string values2str(double d); } namespace urdf{ URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDF(const std::string &xml_string); URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFile(const std::string &path); URDFDOM_DLLAPI TiXmlDocument* exportURDF(ModelInterfaceSharedPtr &model); URDFDOM_DLLAPI TiXmlDocument* exportURDF(const ModelInterface &model); URDFDOM_DLLAPI bool parsePose(Pose&, TiXmlElement*); } #endif urdfdom-0.4.1/urdf_parser/src/000077500000000000000000000000001265476635200162775ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser/src/check_urdf.cpp000066400000000000000000000066711265476635200211120ustar00rootroot00000000000000/********************************************************************* * 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 "urdf_parser/urdf_parser.h" #include #include using namespace urdf; void printTree(LinkConstSharedPtr link,int level = 0) { level+=2; int count = 0; for (std::vector::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) { if (*child) { for(int j=0;jname << std::endl; // first grandchild printTree(*child,level); } else { for(int j=0;jname << " has a null child!" << *child << std::endl; } } } int main(int argc, char** argv) { if (argc < 2){ std::cerr << "Expect URDF xml file to parse" << std::endl; return -1; } std::string xml_string; std::fstream xml_file(argv[1], std::fstream::in); while ( xml_file.good() ) { std::string line; std::getline( xml_file, line); xml_string += (line + "\n"); } xml_file.close(); ModelInterfaceSharedPtr robot = parseURDF(xml_string); if (!robot){ std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; return -1; } std::cout << "robot name is: " << robot->getName() << std::endl; // get info from parser std::cout << "---------- Successfully Parsed XML ---------------" << std::endl; // get root link LinkConstSharedPtr root_link=robot->getRoot(); if (!root_link) return -1; std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl; // print entire tree printTree(root_link); return 0; } urdfdom-0.4.1/urdf_parser/src/joint.cpp000066400000000000000000000471261265476635200201400ustar00rootroot00000000000000/********************************************************************* * Software Ligcense 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: John Hsu */ #include #include #include #include #include #include namespace urdf{ bool parsePose(Pose &pose, TiXmlElement* xml); bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config) { jd.clear(); // Get joint damping const char* damping_str = config->Attribute("damping"); if (damping_str == NULL){ CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: no damping, defaults to 0"); jd.damping = 0; } else { try { jd.damping = boost::lexical_cast(damping_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("damping value (%s) is not a float: %s",damping_str, e.what()); return false; } } // Get joint friction const char* friction_str = config->Attribute("friction"); if (friction_str == NULL){ CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: no friction, defaults to 0"); jd.friction = 0; } else { try { jd.friction = boost::lexical_cast(friction_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("friction value (%s) is not a float: %s",friction_str, e.what()); return false; } } if (damping_str == NULL && friction_str == NULL) { CONSOLE_BRIDGE_logError("joint dynamics element specified with no damping and no friction"); return false; } else{ CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: damping %f and friction %f", jd.damping, jd.friction); return true; } } bool parseJointLimits(JointLimits &jl, TiXmlElement* config) { jl.clear(); // Get lower joint limit const char* lower_str = config->Attribute("lower"); if (lower_str == NULL){ CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no lower, defaults to 0"); jl.lower = 0; } else { try { jl.lower = boost::lexical_cast(lower_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("lower value (%s) is not a float: %s", lower_str, e.what()); return false; } } // Get upper joint limit const char* upper_str = config->Attribute("upper"); if (upper_str == NULL){ CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no upper, , defaults to 0"); jl.upper = 0; } else { try { jl.upper = boost::lexical_cast(upper_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("upper value (%s) is not a float: %s",upper_str, e.what()); return false; } } // Get joint effort limit const char* effort_str = config->Attribute("effort"); if (effort_str == NULL){ CONSOLE_BRIDGE_logError("joint limit: no effort"); return false; } else { try { jl.effort = boost::lexical_cast(effort_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("effort value (%s) is not a float: %s",effort_str, e.what()); return false; } } // Get joint velocity limit const char* velocity_str = config->Attribute("velocity"); if (velocity_str == NULL){ CONSOLE_BRIDGE_logError("joint limit: no velocity"); return false; } else { try { jl.velocity = boost::lexical_cast(velocity_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("velocity value (%s) is not a float: %s",velocity_str, e.what()); return false; } } return true; } bool parseJointSafety(JointSafety &js, TiXmlElement* config) { js.clear(); // Get soft_lower_limit joint limit const char* soft_lower_limit_str = config->Attribute("soft_lower_limit"); if (soft_lower_limit_str == NULL) { CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no soft_lower_limit, using default value"); js.soft_lower_limit = 0; } else { try { js.soft_lower_limit = boost::lexical_cast(soft_lower_limit_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("soft_lower_limit value (%s) is not a float: %s",soft_lower_limit_str, e.what()); return false; } } // Get soft_upper_limit joint limit const char* soft_upper_limit_str = config->Attribute("soft_upper_limit"); if (soft_upper_limit_str == NULL) { CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no soft_upper_limit, using default value"); js.soft_upper_limit = 0; } else { try { js.soft_upper_limit = boost::lexical_cast(soft_upper_limit_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("soft_upper_limit value (%s) is not a float: %s",soft_upper_limit_str, e.what()); return false; } } // Get k_position_ safety "position" gain - not exactly position gain const char* k_position_str = config->Attribute("k_position"); if (k_position_str == NULL) { CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no k_position, using default value"); js.k_position = 0; } else { try { js.k_position = boost::lexical_cast(k_position_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("k_position value (%s) is not a float: %s",k_position_str, e.what()); return false; } } // Get k_velocity_ safety velocity gain const char* k_velocity_str = config->Attribute("k_velocity"); if (k_velocity_str == NULL) { CONSOLE_BRIDGE_logError("joint safety: no k_velocity"); return false; } else { try { js.k_velocity = boost::lexical_cast(k_velocity_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("k_velocity value (%s) is not a float: %s",k_velocity_str, e.what()); return false; } } return true; } bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config) { jc.clear(); // Get rising edge position const char* rising_position_str = config->Attribute("rising"); if (rising_position_str == NULL) { CONSOLE_BRIDGE_logDebug("urdfdom.joint_calibration: no rising, using default value"); jc.rising.reset(); } else { try { jc.rising.reset(new double(boost::lexical_cast(rising_position_str))); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("risingvalue (%s) is not a float: %s",rising_position_str, e.what()); return false; } } // Get falling edge position const char* falling_position_str = config->Attribute("falling"); if (falling_position_str == NULL) { CONSOLE_BRIDGE_logDebug("urdfdom.joint_calibration: no falling, using default value"); jc.falling.reset(); } else { try { jc.falling.reset(new double(boost::lexical_cast(falling_position_str))); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("fallingvalue (%s) is not a float: %s",falling_position_str, e.what()); return false; } } return true; } bool parseJointMimic(JointMimic &jm, TiXmlElement* config) { jm.clear(); // Get name of joint to mimic const char* joint_name_str = config->Attribute("joint"); if (joint_name_str == NULL) { CONSOLE_BRIDGE_logError("joint mimic: no mimic joint specified"); return false; } else jm.joint_name = joint_name_str; // Get mimic multiplier const char* multiplier_str = config->Attribute("multiplier"); if (multiplier_str == NULL) { CONSOLE_BRIDGE_logDebug("urdfdom.joint_mimic: no multiplier, using default value of 1"); jm.multiplier = 1; } else { try { jm.multiplier = boost::lexical_cast(multiplier_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("multiplier value (%s) is not a float: %s",multiplier_str, e.what()); return false; } } // Get mimic offset const char* offset_str = config->Attribute("offset"); if (offset_str == NULL) { CONSOLE_BRIDGE_logDebug("urdfdom.joint_mimic: no offset, using default value of 0"); jm.offset = 0; } else { try { jm.offset = boost::lexical_cast(offset_str); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("offset value (%s) is not a float: %s",offset_str, e.what()); return false; } } return true; } bool parseJoint(Joint &joint, TiXmlElement* config) { joint.clear(); // Get Joint Name const char *name = config->Attribute("name"); if (!name) { CONSOLE_BRIDGE_logError("unnamed joint found"); return false; } joint.name = name; // Get transform from Parent Link to Joint Frame TiXmlElement *origin_xml = config->FirstChildElement("origin"); if (!origin_xml) { CONSOLE_BRIDGE_logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str()); joint.parent_to_joint_origin_transform.clear(); } else { if (!parsePose(joint.parent_to_joint_origin_transform, origin_xml)) { joint.parent_to_joint_origin_transform.clear(); CONSOLE_BRIDGE_logError("Malformed parent origin element for joint [%s]", joint.name.c_str()); return false; } } // Get Parent Link TiXmlElement *parent_xml = config->FirstChildElement("parent"); if (parent_xml) { const char *pname = parent_xml->Attribute("link"); if (!pname) { CONSOLE_BRIDGE_logInform("no parent link name specified for Joint link [%s]. this might be the root?", joint.name.c_str()); } else { joint.parent_link_name = std::string(pname); } } // Get Child Link TiXmlElement *child_xml = config->FirstChildElement("child"); if (child_xml) { const char *pname = child_xml->Attribute("link"); if (!pname) { CONSOLE_BRIDGE_logInform("no child link name specified for Joint link [%s].", joint.name.c_str()); } else { joint.child_link_name = std::string(pname); } } // Get Joint type const char* type_char = config->Attribute("type"); if (!type_char) { CONSOLE_BRIDGE_logError("joint [%s] has no type, check to see if it's a reference.", joint.name.c_str()); return false; } std::string type_str = type_char; if (type_str == "planar") joint.type = Joint::PLANAR; else if (type_str == "floating") joint.type = Joint::FLOATING; else if (type_str == "revolute") joint.type = Joint::REVOLUTE; else if (type_str == "continuous") joint.type = Joint::CONTINUOUS; else if (type_str == "prismatic") joint.type = Joint::PRISMATIC; else if (type_str == "fixed") joint.type = Joint::FIXED; else { CONSOLE_BRIDGE_logError("Joint [%s] has no known type [%s]", joint.name.c_str(), type_str.c_str()); return false; } // Get Joint Axis if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED) { // axis TiXmlElement *axis_xml = config->FirstChildElement("axis"); if (!axis_xml){ CONSOLE_BRIDGE_logDebug("urdfdom: no axis elemement for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str()); joint.axis = Vector3(1.0, 0.0, 0.0); } else{ if (axis_xml->Attribute("xyz")){ try { joint.axis.init(axis_xml->Attribute("xyz")); } catch (ParseError &e) { joint.axis.clear(); CONSOLE_BRIDGE_logError("Malformed axis element for joint [%s]: %s", joint.name.c_str(), e.what()); return false; } } } } // Get limit TiXmlElement *limit_xml = config->FirstChildElement("limit"); if (limit_xml) { joint.limits.reset(new JointLimits()); if (!parseJointLimits(*joint.limits, limit_xml)) { CONSOLE_BRIDGE_logError("Could not parse limit element for joint [%s]", joint.name.c_str()); joint.limits.reset(); return false; } } else if (joint.type == Joint::REVOLUTE) { CONSOLE_BRIDGE_logError("Joint [%s] is of type REVOLUTE but it does not specify limits", joint.name.c_str()); return false; } else if (joint.type == Joint::PRISMATIC) { CONSOLE_BRIDGE_logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str()); return false; } // Get safety TiXmlElement *safety_xml = config->FirstChildElement("safety_controller"); if (safety_xml) { joint.safety.reset(new JointSafety()); if (!parseJointSafety(*joint.safety, safety_xml)) { CONSOLE_BRIDGE_logError("Could not parse safety element for joint [%s]", joint.name.c_str()); joint.safety.reset(); return false; } } // Get calibration TiXmlElement *calibration_xml = config->FirstChildElement("calibration"); if (calibration_xml) { joint.calibration.reset(new JointCalibration()); if (!parseJointCalibration(*joint.calibration, calibration_xml)) { CONSOLE_BRIDGE_logError("Could not parse calibration element for joint [%s]", joint.name.c_str()); joint.calibration.reset(); return false; } } // Get Joint Mimic TiXmlElement *mimic_xml = config->FirstChildElement("mimic"); if (mimic_xml) { joint.mimic.reset(new JointMimic()); if (!parseJointMimic(*joint.mimic, mimic_xml)) { CONSOLE_BRIDGE_logError("Could not parse mimic element for joint [%s]", joint.name.c_str()); joint.mimic.reset(); return false; } } // Get Dynamics TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); if (prop_xml) { joint.dynamics.reset(new JointDynamics()); if (!parseJointDynamics(*joint.dynamics, prop_xml)) { CONSOLE_BRIDGE_logError("Could not parse joint_dynamics element for joint [%s]", joint.name.c_str()); joint.dynamics.reset(); return false; } } return true; } /* exports */ bool exportPose(Pose &pose, TiXmlElement* xml); bool exportJointDynamics(JointDynamics &jd, TiXmlElement* xml) { TiXmlElement *dynamics_xml = new TiXmlElement("dynamics"); dynamics_xml->SetAttribute("damping", urdf_export_helpers::values2str(jd.damping) ); dynamics_xml->SetAttribute("friction", urdf_export_helpers::values2str(jd.friction) ); xml->LinkEndChild(dynamics_xml); return true; } bool exportJointLimits(JointLimits &jl, TiXmlElement* xml) { TiXmlElement *limit_xml = new TiXmlElement("limit"); limit_xml->SetAttribute("effort", urdf_export_helpers::values2str(jl.effort) ); limit_xml->SetAttribute("velocity", urdf_export_helpers::values2str(jl.velocity) ); limit_xml->SetAttribute("lower", urdf_export_helpers::values2str(jl.lower) ); limit_xml->SetAttribute("upper", urdf_export_helpers::values2str(jl.upper) ); xml->LinkEndChild(limit_xml); return true; } bool exportJointSafety(JointSafety &js, TiXmlElement* xml) { TiXmlElement *safety_xml = new TiXmlElement("safety_controller"); safety_xml->SetAttribute("k_position", urdf_export_helpers::values2str(js.k_position) ); safety_xml->SetAttribute("k_velocity", urdf_export_helpers::values2str(js.k_velocity) ); safety_xml->SetAttribute("soft_lower_limit", urdf_export_helpers::values2str(js.soft_lower_limit) ); safety_xml->SetAttribute("soft_upper_limit", urdf_export_helpers::values2str(js.soft_upper_limit) ); xml->LinkEndChild(safety_xml); return true; } bool exportJointCalibration(JointCalibration &jc, TiXmlElement* xml) { if (jc.falling || jc.rising) { TiXmlElement *calibration_xml = new TiXmlElement("calibration"); if (jc.falling) calibration_xml->SetAttribute("falling", urdf_export_helpers::values2str(*jc.falling) ); if (jc.rising) calibration_xml->SetAttribute("rising", urdf_export_helpers::values2str(*jc.rising) ); //calibration_xml->SetAttribute("reference_position", urdf_export_helpers::values2str(jc.reference_position) ); xml->LinkEndChild(calibration_xml); } return true; } bool exportJointMimic(JointMimic &jm, TiXmlElement* xml) { if (!jm.joint_name.empty()) { TiXmlElement *mimic_xml = new TiXmlElement("mimic"); mimic_xml->SetAttribute("offset", urdf_export_helpers::values2str(jm.offset) ); mimic_xml->SetAttribute("multiplier", urdf_export_helpers::values2str(jm.multiplier) ); mimic_xml->SetAttribute("joint", jm.joint_name ); xml->LinkEndChild(mimic_xml); } return true; } bool exportJoint(Joint &joint, TiXmlElement* xml) { TiXmlElement * joint_xml = new TiXmlElement("joint"); joint_xml->SetAttribute("name", joint.name); if (joint.type == urdf::Joint::PLANAR) joint_xml->SetAttribute("type", "planar"); else if (joint.type == urdf::Joint::FLOATING) joint_xml->SetAttribute("type", "floating"); else if (joint.type == urdf::Joint::REVOLUTE) joint_xml->SetAttribute("type", "revolute"); else if (joint.type == urdf::Joint::CONTINUOUS) joint_xml->SetAttribute("type", "continuous"); else if (joint.type == urdf::Joint::PRISMATIC) joint_xml->SetAttribute("type", "prismatic"); else if (joint.type == urdf::Joint::FIXED) joint_xml->SetAttribute("type", "fixed"); else CONSOLE_BRIDGE_logError("ERROR: Joint [%s] type [%d] is not a defined type.\n",joint.name.c_str(), joint.type); // origin exportPose(joint.parent_to_joint_origin_transform, joint_xml); // axis TiXmlElement * axis_xml = new TiXmlElement("axis"); axis_xml->SetAttribute("xyz", urdf_export_helpers::values2str(joint.axis)); joint_xml->LinkEndChild(axis_xml); // parent TiXmlElement * parent_xml = new TiXmlElement("parent"); parent_xml->SetAttribute("link", joint.parent_link_name); joint_xml->LinkEndChild(parent_xml); // child TiXmlElement * child_xml = new TiXmlElement("child"); child_xml->SetAttribute("link", joint.child_link_name); joint_xml->LinkEndChild(child_xml); if (joint.dynamics) exportJointDynamics(*(joint.dynamics), joint_xml); if (joint.limits) exportJointLimits(*(joint.limits), joint_xml); if (joint.safety) exportJointSafety(*(joint.safety), joint_xml); if (joint.calibration) exportJointCalibration(*(joint.calibration), joint_xml); if (joint.mimic) exportJointMimic(*(joint.mimic), joint_xml); xml->LinkEndChild(joint_xml); return true; } } urdfdom-0.4.1/urdf_parser/src/link.cpp000066400000000000000000000436051265476635200177500ustar00rootroot00000000000000/********************************************************************* * 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 #include #include #include #include #include namespace urdf{ bool parsePose(Pose &pose, TiXmlElement* xml); bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok) { bool has_rgb = false; bool has_filename = false; material.clear(); if (!config->Attribute("name")) { CONSOLE_BRIDGE_logError("Material must contain a name attribute"); return false; } material.name = config->Attribute("name"); // texture TiXmlElement *t = config->FirstChildElement("texture"); if (t) { if (t->Attribute("filename")) { material.texture_filename = t->Attribute("filename"); has_filename = true; } } // color TiXmlElement *c = config->FirstChildElement("color"); if (c) { if (c->Attribute("rgba")) { try { material.color.init(c->Attribute("rgba")); has_rgb = true; } catch (ParseError &e) { material.color.clear(); CONSOLE_BRIDGE_logError(std::string("Material [" + material.name + "] has malformed color rgba values: " + e.what()).c_str()); } } } if (!has_rgb && !has_filename) { if (!only_name_is_ok) // no need for an error if only name is ok { if (!has_rgb) CONSOLE_BRIDGE_logError(std::string("Material ["+material.name+"] color has no rgba").c_str()); if (!has_filename) CONSOLE_BRIDGE_logError(std::string("Material ["+material.name+"] not defined in file").c_str()); } return false; } return true; } bool parseSphere(Sphere &s, TiXmlElement *c) { s.clear(); s.type = Geometry::SPHERE; if (!c->Attribute("radius")) { CONSOLE_BRIDGE_logError("Sphere shape must have a radius attribute"); return false; } try { s.radius = boost::lexical_cast(c->Attribute("radius")); } catch (boost::bad_lexical_cast &e) { std::stringstream stm; stm << "radius [" << c->Attribute("radius") << "] is not a valid float: " << e.what(); CONSOLE_BRIDGE_logError(stm.str().c_str()); return false; } return true; } bool parseBox(Box &b, TiXmlElement *c) { b.clear(); b.type = Geometry::BOX; if (!c->Attribute("size")) { CONSOLE_BRIDGE_logError("Box shape has no size attribute"); return false; } try { b.dim.init(c->Attribute("size")); } catch (ParseError &e) { b.dim.clear(); CONSOLE_BRIDGE_logError(e.what()); return false; } return true; } bool parseCylinder(Cylinder &y, TiXmlElement *c) { y.clear(); y.type = Geometry::CYLINDER; if (!c->Attribute("length") || !c->Attribute("radius")) { CONSOLE_BRIDGE_logError("Cylinder shape must have both length and radius attributes"); return false; } try { y.length = boost::lexical_cast(c->Attribute("length")); } catch (boost::bad_lexical_cast &/*e*/) { std::stringstream stm; stm << "length [" << c->Attribute("length") << "] is not a valid float"; CONSOLE_BRIDGE_logError(stm.str().c_str()); return false; } try { y.radius = boost::lexical_cast(c->Attribute("radius")); } catch (boost::bad_lexical_cast &/*e*/) { std::stringstream stm; stm << "radius [" << c->Attribute("radius") << "] is not a valid float"; CONSOLE_BRIDGE_logError(stm.str().c_str()); return false; } return true; } bool parseMesh(Mesh &m, TiXmlElement *c) { m.clear(); m.type = Geometry::MESH; if (!c->Attribute("filename")) { CONSOLE_BRIDGE_logError("Mesh must contain a filename attribute"); return false; } m.filename = c->Attribute("filename"); if (c->Attribute("scale")) { try { m.scale.init(c->Attribute("scale")); } catch (ParseError &e) { m.scale.clear(); CONSOLE_BRIDGE_logError("Mesh scale was specified, but could not be parsed: %s", e.what()); return false; } } else { m.scale.x = m.scale.y = m.scale.z = 1; } return true; } GeometrySharedPtr parseGeometry(TiXmlElement *g) { GeometrySharedPtr geom; if (!g) return geom; TiXmlElement *shape = g->FirstChildElement(); if (!shape) { CONSOLE_BRIDGE_logError("Geometry tag contains no child element."); return geom; } std::string type_name = shape->ValueStr(); if (type_name == "sphere") { Sphere *s = new Sphere(); geom.reset(s); if (parseSphere(*s, shape)) return geom; } else if (type_name == "box") { Box *b = new Box(); geom.reset(b); if (parseBox(*b, shape)) return geom; } else if (type_name == "cylinder") { Cylinder *c = new Cylinder(); geom.reset(c); if (parseCylinder(*c, shape)) return geom; } else if (type_name == "mesh") { Mesh *m = new Mesh(); geom.reset(m); if (parseMesh(*m, shape)) return geom; } else { CONSOLE_BRIDGE_logError("Unknown geometry type '%s'", type_name.c_str()); return geom; } return GeometrySharedPtr(); } bool parseInertial(Inertial &i, TiXmlElement *config) { i.clear(); // Origin TiXmlElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(i.origin, o)) return false; } TiXmlElement *mass_xml = config->FirstChildElement("mass"); if (!mass_xml) { CONSOLE_BRIDGE_logError("Inertial element must have a mass element"); return false; } if (!mass_xml->Attribute("value")) { CONSOLE_BRIDGE_logError("Inertial: mass element must have value attribute"); return false; } try { i.mass = boost::lexical_cast(mass_xml->Attribute("value")); } catch (boost::bad_lexical_cast &/*e*/) { std::stringstream stm; stm << "Inertial: mass [" << mass_xml->Attribute("value") << "] is not a float"; CONSOLE_BRIDGE_logError(stm.str().c_str()); return false; } TiXmlElement *inertia_xml = config->FirstChildElement("inertia"); if (!inertia_xml) { CONSOLE_BRIDGE_logError("Inertial element must have inertia element"); return false; } if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") && inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") && inertia_xml->Attribute("izz"))) { CONSOLE_BRIDGE_logError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes"); return false; } try { i.ixx = boost::lexical_cast(inertia_xml->Attribute("ixx")); i.ixy = boost::lexical_cast(inertia_xml->Attribute("ixy")); i.ixz = boost::lexical_cast(inertia_xml->Attribute("ixz")); i.iyy = boost::lexical_cast(inertia_xml->Attribute("iyy")); i.iyz = boost::lexical_cast(inertia_xml->Attribute("iyz")); i.izz = boost::lexical_cast(inertia_xml->Attribute("izz")); } catch (boost::bad_lexical_cast &/*e*/) { std::stringstream stm; stm << "Inertial: one of the inertia elements is not a valid double:" << " ixx [" << inertia_xml->Attribute("ixx") << "]" << " ixy [" << inertia_xml->Attribute("ixy") << "]" << " ixz [" << inertia_xml->Attribute("ixz") << "]" << " iyy [" << inertia_xml->Attribute("iyy") << "]" << " iyz [" << inertia_xml->Attribute("iyz") << "]" << " izz [" << inertia_xml->Attribute("izz") << "]"; CONSOLE_BRIDGE_logError(stm.str().c_str()); return false; } return true; } bool parseVisual(Visual &vis, TiXmlElement *config) { vis.clear(); // Origin TiXmlElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(vis.origin, o)) return false; } // Geometry TiXmlElement *geom = config->FirstChildElement("geometry"); vis.geometry = parseGeometry(geom); if (!vis.geometry) return false; const char *name_char = config->Attribute("name"); if (name_char) vis.name = name_char; // Material TiXmlElement *mat = config->FirstChildElement("material"); if (mat) { // get material name if (!mat->Attribute("name")) { CONSOLE_BRIDGE_logError("Visual material must contain a name attribute"); return false; } vis.material_name = mat->Attribute("name"); // try to parse material element in place vis.material.reset(new Material()); if (!parseMaterial(*vis.material, mat, true)) { CONSOLE_BRIDGE_logDebug("urdfdom: material has only name, actual material definition may be in the model"); } } return true; } bool parseCollision(Collision &col, TiXmlElement* config) { col.clear(); // Origin TiXmlElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(col.origin, o)) return false; } // Geometry TiXmlElement *geom = config->FirstChildElement("geometry"); col.geometry = parseGeometry(geom); if (!col.geometry) return false; const char *name_char = config->Attribute("name"); if (name_char) col.name = name_char; return true; } bool parseLink(Link &link, TiXmlElement* config) { link.clear(); const char *name_char = config->Attribute("name"); if (!name_char) { CONSOLE_BRIDGE_logError("No name given for the link."); return false; } link.name = std::string(name_char); // Inertial (optional) TiXmlElement *i = config->FirstChildElement("inertial"); if (i) { link.inertial.reset(new Inertial()); if (!parseInertial(*link.inertial, i)) { CONSOLE_BRIDGE_logError("Could not parse inertial element for Link [%s]", link.name.c_str()); return false; } } // Multiple Visuals (optional) for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) { VisualSharedPtr vis; vis.reset(new Visual()); if (parseVisual(*vis, vis_xml)) { link.visual_array.push_back(vis); } else { vis.reset(); CONSOLE_BRIDGE_logError("Could not parse visual element for Link [%s]", link.name.c_str()); return false; } } // Visual (optional) // Assign the first visual to the .visual ptr, if it exists if (!link.visual_array.empty()) link.visual = link.visual_array[0]; // Multiple Collisions (optional) for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) { CollisionSharedPtr col; col.reset(new Collision()); if (parseCollision(*col, col_xml)) { link.collision_array.push_back(col); } else { col.reset(); CONSOLE_BRIDGE_logError("Could not parse collision element for Link [%s]", link.name.c_str()); return false; } } // Collision (optional) // Assign the first collision to the .collision ptr, if it exists if (!link.collision_array.empty()) link.collision = link.collision_array[0]; return true; } /* exports */ bool exportPose(Pose &pose, TiXmlElement* xml); bool exportMaterial(Material &material, TiXmlElement *xml) { TiXmlElement *material_xml = new TiXmlElement("material"); material_xml->SetAttribute("name", material.name); TiXmlElement* texture = new TiXmlElement("texture"); if (!material.texture_filename.empty()) texture->SetAttribute("filename", material.texture_filename); material_xml->LinkEndChild(texture); TiXmlElement* color = new TiXmlElement("color"); color->SetAttribute("rgba", urdf_export_helpers::values2str(material.color)); material_xml->LinkEndChild(color); xml->LinkEndChild(material_xml); return true; } bool exportSphere(Sphere &s, TiXmlElement *xml) { // e.g. add TiXmlElement *sphere_xml = new TiXmlElement("sphere"); sphere_xml->SetAttribute("radius", urdf_export_helpers::values2str(s.radius)); xml->LinkEndChild(sphere_xml); return true; } bool exportBox(Box &b, TiXmlElement *xml) { // e.g. add TiXmlElement *box_xml = new TiXmlElement("box"); box_xml->SetAttribute("size", urdf_export_helpers::values2str(b.dim)); xml->LinkEndChild(box_xml); return true; } bool exportCylinder(Cylinder &y, TiXmlElement *xml) { // e.g. add TiXmlElement *cylinder_xml = new TiXmlElement("cylinder"); cylinder_xml->SetAttribute("radius", urdf_export_helpers::values2str(y.radius)); cylinder_xml->SetAttribute("length", urdf_export_helpers::values2str(y.length)); xml->LinkEndChild(cylinder_xml); return true; } bool exportMesh(Mesh &m, TiXmlElement *xml) { // e.g. add TiXmlElement *mesh_xml = new TiXmlElement("mesh"); if (!m.filename.empty()) mesh_xml->SetAttribute("filename", m.filename); mesh_xml->SetAttribute("scale", urdf_export_helpers::values2str(m.scale)); xml->LinkEndChild(mesh_xml); return true; } bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml) { TiXmlElement *geometry_xml = new TiXmlElement("geometry"); if (urdf::dynamic_pointer_cast(geom)) { exportSphere((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); } else if (urdf::dynamic_pointer_cast(geom)) { exportBox((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); } else if (urdf::dynamic_pointer_cast(geom)) { exportCylinder((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); } else if (urdf::dynamic_pointer_cast(geom)) { exportMesh((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); } else { CONSOLE_BRIDGE_logError("geometry not specified, I'll make one up for you!"); Sphere *s = new Sphere(); s->radius = 0.03; geom.reset(s); exportSphere((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); } xml->LinkEndChild(geometry_xml); return true; } bool exportInertial(Inertial &i, TiXmlElement *xml) { // adds // // // // TiXmlElement *inertial_xml = new TiXmlElement("inertial"); TiXmlElement *mass_xml = new TiXmlElement("mass"); mass_xml->SetAttribute("value", urdf_export_helpers::values2str(i.mass)); inertial_xml->LinkEndChild(mass_xml); exportPose(i.origin, inertial_xml); TiXmlElement *inertia_xml = new TiXmlElement("inertia"); inertia_xml->SetAttribute("ixx", urdf_export_helpers::values2str(i.ixx)); inertia_xml->SetAttribute("ixy", urdf_export_helpers::values2str(i.ixy)); inertia_xml->SetAttribute("ixz", urdf_export_helpers::values2str(i.ixz)); inertia_xml->SetAttribute("iyy", urdf_export_helpers::values2str(i.iyy)); inertia_xml->SetAttribute("iyz", urdf_export_helpers::values2str(i.iyz)); inertia_xml->SetAttribute("izz", urdf_export_helpers::values2str(i.izz)); inertial_xml->LinkEndChild(inertia_xml); xml->LinkEndChild(inertial_xml); return true; } bool exportVisual(Visual &vis, TiXmlElement *xml) { // // // // // // // TiXmlElement * visual_xml = new TiXmlElement("visual"); exportPose(vis.origin, visual_xml); exportGeometry(vis.geometry, visual_xml); if (vis.material) exportMaterial(*vis.material, visual_xml); xml->LinkEndChild(visual_xml); return true; } bool exportCollision(Collision &col, TiXmlElement* xml) { // // // // // // // TiXmlElement * collision_xml = new TiXmlElement("collision"); exportPose(col.origin, collision_xml); exportGeometry(col.geometry, collision_xml); xml->LinkEndChild(collision_xml); return true; } bool exportLink(Link &link, TiXmlElement* xml) { TiXmlElement * link_xml = new TiXmlElement("link"); link_xml->SetAttribute("name", link.name); if (link.inertial) exportInertial(*link.inertial, link_xml); for (std::size_t i = 0 ; i < link.visual_array.size() ; ++i) exportVisual(*link.visual_array[i], link_xml); for (std::size_t i = 0 ; i < link.collision_array.size() ; ++i) exportCollision(*link.collision_array[i], link_xml); xml->LinkEndChild(link_xml); return true; } } urdfdom-0.4.1/urdf_parser/src/model.cpp000066400000000000000000000220231265476635200201020ustar00rootroot00000000000000/********************************************************************* * 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 "urdf_parser/urdf_parser.h" #include #include namespace urdf{ bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok); bool parseLink(Link &link, TiXmlElement *config); bool parseJoint(Joint &joint, TiXmlElement *config); ModelInterfaceSharedPtr parseURDFFile(const std::string &path) { std::ifstream stream( path.c_str() ); if (!stream) { CONSOLE_BRIDGE_logError(("File " + path + " does not exist").c_str()); return ModelInterfaceSharedPtr(); } std::string xml_str((std::istreambuf_iterator(stream)), std::istreambuf_iterator()); return urdf::parseURDF( xml_str ); } ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) { ModelInterfaceSharedPtr model(new ModelInterface); model->clear(); TiXmlDocument xml_doc; xml_doc.Parse(xml_string.c_str()); if (xml_doc.Error()) { CONSOLE_BRIDGE_logError(xml_doc.ErrorDesc()); xml_doc.ClearError(); model.reset(); return model; } TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot"); if (!robot_xml) { CONSOLE_BRIDGE_logError("Could not find the 'robot' element in the xml file"); model.reset(); return model; } // Get robot name const char *name = robot_xml->Attribute("name"); if (!name) { CONSOLE_BRIDGE_logError("No name given for the robot."); model.reset(); return model; } model->name_ = std::string(name); // Get all Material elements for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) { MaterialSharedPtr material; material.reset(new Material); try { parseMaterial(*material, material_xml, false); // material needs to be fully defined here if (model->getMaterial(material->name)) { CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str()); material.reset(); model.reset(); return model; } else { model->materials_.insert(make_pair(material->name,material)); CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str()); } } catch (ParseError &/*e*/) { CONSOLE_BRIDGE_logError("material xml is not initialized correctly"); material.reset(); model.reset(); return model; } } // Get all Link elements for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) { LinkSharedPtr link; link.reset(new Link); try { parseLink(*link, link_xml); if (model->getLink(link->name)) { CONSOLE_BRIDGE_logError("link '%s' is not unique.", link->name.c_str()); model.reset(); return model; } else { // set link visual material CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material", link->name.c_str()); if (link->visual) { if (!link->visual->material_name.empty()) { if (model->getMaterial(link->visual->material_name)) { CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str()); link->visual->material = model->getMaterial( link->visual->material_name.c_str() ); } else { if (link->visual->material) { CONSOLE_BRIDGE_logDebug("urdfdom: link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str()); model->materials_.insert(make_pair(link->visual->material->name,link->visual->material)); } else { CONSOLE_BRIDGE_logError("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str()); model.reset(); return model; } } } } model->links_.insert(make_pair(link->name,link)); CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str()); } } catch (ParseError &/*e*/) { CONSOLE_BRIDGE_logError("link xml is not initialized correctly"); model.reset(); return model; } } if (model->links_.empty()){ CONSOLE_BRIDGE_logError("No link elements found in urdf file"); model.reset(); return model; } // Get all Joint elements for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) { JointSharedPtr joint; joint.reset(new Joint); if (parseJoint(*joint, joint_xml)) { if (model->getJoint(joint->name)) { CONSOLE_BRIDGE_logError("joint '%s' is not unique.", joint->name.c_str()); model.reset(); return model; } else { model->joints_.insert(make_pair(joint->name,joint)); CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str()); } } else { CONSOLE_BRIDGE_logError("joint xml is not initialized correctly"); model.reset(); return model; } } // every link has children links and joints, but no parents, so we create a // local convenience data structure for keeping child->parent relations std::map parent_link_tree; parent_link_tree.clear(); // building tree: name mapping try { model->initTree(parent_link_tree); } catch(ParseError &e) { CONSOLE_BRIDGE_logError("Failed to build tree: %s", e.what()); model.reset(); return model; } // find the root link try { model->initRoot(parent_link_tree); } catch(ParseError &e) { CONSOLE_BRIDGE_logError("Failed to find root link: %s", e.what()); model.reset(); return model; } return model; } bool exportMaterial(Material &material, TiXmlElement *config); bool exportLink(Link &link, TiXmlElement *config); bool exportJoint(Joint &joint, TiXmlElement *config); TiXmlDocument* exportURDF(const ModelInterface &model) { TiXmlDocument *doc = new TiXmlDocument(); TiXmlElement *robot = new TiXmlElement("robot"); robot->SetAttribute("name", model.name_); doc->LinkEndChild(robot); for (std::map::const_iterator m=model.materials_.begin(); m!=model.materials_.end(); m++) { CONSOLE_BRIDGE_logDebug("urdfdom: exporting material [%s]\n",m->second->name.c_str()); exportMaterial(*(m->second), robot); } for (std::map::const_iterator l=model.links_.begin(); l!=model.links_.end(); l++) { CONSOLE_BRIDGE_logDebug("urdfdom: exporting link [%s]\n",l->second->name.c_str()); exportLink(*(l->second), robot); } for (std::map::const_iterator j=model.joints_.begin(); j!=model.joints_.end(); j++) { CONSOLE_BRIDGE_logDebug("urdfdom: exporting joint [%s]\n",j->second->name.c_str()); exportJoint(*(j->second), robot); } return doc; } TiXmlDocument* exportURDF(ModelInterfaceSharedPtr &model) { return exportURDF(*model); } } urdfdom-0.4.1/urdf_parser/src/pose.cpp000066400000000000000000000074721265476635200177630ustar00rootroot00000000000000/********************************************************************* * 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, John Hsu */ #include #include #include #include #include #include #include #include namespace urdf_export_helpers { std::string values2str(unsigned int count, const double *values, double (*conv)(double)) { std::stringstream ss; for (unsigned int i = 0 ; i < count ; i++) { if (i > 0) ss << " "; ss << (conv ? conv(values[i]) : values[i]); } return ss.str(); } std::string values2str(urdf::Vector3 vec) { double xyz[3]; xyz[0] = vec.x; xyz[1] = vec.y; xyz[2] = vec.z; return values2str(3, xyz); } std::string values2str(urdf::Rotation rot) { double rpy[3]; rot.getRPY(rpy[0], rpy[1], rpy[2]); return values2str(3, rpy); } std::string values2str(urdf::Color c) { double rgba[4]; rgba[0] = c.r; rgba[1] = c.g; rgba[2] = c.b; rgba[3] = c.a; return values2str(4, rgba); } std::string values2str(double d) { return values2str(1, &d); } } namespace urdf{ bool parsePose(Pose &pose, TiXmlElement* xml) { pose.clear(); if (xml) { const char* xyz_str = xml->Attribute("xyz"); if (xyz_str != NULL) { try { pose.position.init(xyz_str); } catch (ParseError &e) { CONSOLE_BRIDGE_logError(e.what()); return false; } } const char* rpy_str = xml->Attribute("rpy"); if (rpy_str != NULL) { try { pose.rotation.init(rpy_str); } catch (ParseError &e) { CONSOLE_BRIDGE_logError(e.what()); return false; } } } return true; } bool exportPose(Pose &pose, TiXmlElement* xml) { TiXmlElement *origin = new TiXmlElement("origin"); std::string pose_xyz_str = urdf_export_helpers::values2str(pose.position); std::string pose_rpy_str = urdf_export_helpers::values2str(pose.rotation); origin->SetAttribute("xyz", pose_xyz_str); origin->SetAttribute("rpy", pose_rpy_str); xml->LinkEndChild(origin); return true; } } urdfdom-0.4.1/urdf_parser/src/twist.cpp000066400000000000000000000053441265476635200201630ustar00rootroot00000000000000/********************************************************************* * 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: John Hsu */ #include #include #include #include #include #include #include namespace urdf{ bool parseTwist(Twist &twist, TiXmlElement* xml) { twist.clear(); if (xml) { const char* linear_char = xml->Attribute("linear"); if (linear_char != NULL) { try { twist.linear.init(linear_char); } catch (ParseError &e) { twist.linear.clear(); CONSOLE_BRIDGE_logError("Malformed linear string [%s]: %s", linear_char, e.what()); return false; } } const char* angular_char = xml->Attribute("angular"); if (angular_char != NULL) { try { twist.angular.init(angular_char); } catch (ParseError &e) { twist.angular.clear(); CONSOLE_BRIDGE_logError("Malformed angular [%s]: %s", angular_char, e.what()); return false; } } } return true; } } urdfdom-0.4.1/urdf_parser/src/urdf_model_state.cpp000066400000000000000000000116431265476635200223300ustar00rootroot00000000000000/********************************************************************* * 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: John Hsu */ #include #include #include #include #include #include #include namespace urdf{ bool parseModelState(ModelState &ms, TiXmlElement* config) { ms.clear(); const char *name_char = config->Attribute("name"); if (!name_char) { CONSOLE_BRIDGE_logError("No name given for the model_state."); return false; } ms.name = std::string(name_char); const char *time_stamp_char = config->Attribute("time_stamp"); if (time_stamp_char) { try { double sec = boost::lexical_cast(time_stamp_char); ms.time_stamp.set(sec); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Parsing time stamp [%s] failed: %s", time_stamp_char, e.what()); return false; } } TiXmlElement *joint_state_elem = config->FirstChildElement("joint_state"); if (joint_state_elem) { JointStateSharedPtr joint_state; joint_state.reset(new JointState()); const char *joint_char = joint_state_elem->Attribute("joint"); if (joint_char) joint_state->joint = std::string(joint_char); else { CONSOLE_BRIDGE_logError("No joint name given for the model_state."); return false; } // parse position const char *position_char = joint_state_elem->Attribute("position"); if (position_char) { std::vector pieces; boost::split( pieces, position_char, boost::is_any_of(" ")); for (unsigned int i = 0; i < pieces.size(); ++i){ if (pieces[i] != ""){ try { joint_state->position.push_back(boost::lexical_cast(pieces[i].c_str())); } catch (boost::bad_lexical_cast &/*e*/) { throw ParseError("position element ("+ pieces[i] +") is not a valid float"); } } } } // parse velocity const char *velocity_char = joint_state_elem->Attribute("velocity"); if (velocity_char) { std::vector pieces; boost::split( pieces, velocity_char, boost::is_any_of(" ")); for (unsigned int i = 0; i < pieces.size(); ++i){ if (pieces[i] != ""){ try { joint_state->velocity.push_back(boost::lexical_cast(pieces[i].c_str())); } catch (boost::bad_lexical_cast &/*e*/) { throw ParseError("velocity element ("+ pieces[i] +") is not a valid float"); } } } } // parse effort const char *effort_char = joint_state_elem->Attribute("effort"); if (effort_char) { std::vector pieces; boost::split( pieces, effort_char, boost::is_any_of(" ")); for (unsigned int i = 0; i < pieces.size(); ++i){ if (pieces[i] != ""){ try { joint_state->effort.push_back(boost::lexical_cast(pieces[i].c_str())); } catch (boost::bad_lexical_cast &/*e*/) { throw ParseError("effort element ("+ pieces[i] +") is not a valid float"); } } } } // add to vector ms.joint_states.push_back(joint_state); } return false; } } urdfdom-0.4.1/urdf_parser/src/urdf_sensor.cpp000066400000000000000000000236221265476635200213410ustar00rootroot00000000000000/********************************************************************* * 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: John Hsu */ #include #include #include #include #include #include #include namespace urdf{ bool parsePose(Pose &pose, TiXmlElement* xml); bool parseCamera(Camera &camera, TiXmlElement* config) { camera.clear(); camera.type = VisualSensor::CAMERA; TiXmlElement *image = config->FirstChildElement("image"); if (image) { const char* width_char = image->Attribute("width"); if (width_char) { try { camera.width = boost::lexical_cast(width_char); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Camera image width [%s] is not a valid int: %s", width_char, e.what()); return false; } } else { CONSOLE_BRIDGE_logError("Camera sensor needs an image width attribute"); return false; } const char* height_char = image->Attribute("height"); if (height_char) { try { camera.height = boost::lexical_cast(height_char); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Camera image height [%s] is not a valid int: %s", height_char, e.what()); return false; } } else { CONSOLE_BRIDGE_logError("Camera sensor needs an image height attribute"); return false; } const char* format_char = image->Attribute("format"); if (format_char) camera.format = std::string(format_char); else { CONSOLE_BRIDGE_logError("Camera sensor needs an image format attribute"); return false; } const char* hfov_char = image->Attribute("hfov"); if (hfov_char) { try { camera.hfov = boost::lexical_cast(hfov_char); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Camera image hfov [%s] is not a valid float: %s", hfov_char, e.what()); return false; } } else { CONSOLE_BRIDGE_logError("Camera sensor needs an image hfov attribute"); return false; } const char* near_char = image->Attribute("near"); if (near_char) { try { camera.near = boost::lexical_cast(near_char); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Camera image near [%s] is not a valid float: %s", near_char, e.what()); return false; } } else { CONSOLE_BRIDGE_logError("Camera sensor needs an image near attribute"); return false; } const char* far_char = image->Attribute("far"); if (far_char) { try { camera.far = boost::lexical_cast(far_char); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Camera image far [%s] is not a valid float: %s", far_char, e.what()); return false; } } else { CONSOLE_BRIDGE_logError("Camera sensor needs an image far attribute"); return false; } } else { CONSOLE_BRIDGE_logError("Camera sensor has no element"); return false; } return true; } bool parseRay(Ray &ray, TiXmlElement* config) { ray.clear(); ray.type = VisualSensor::RAY; TiXmlElement *horizontal = config->FirstChildElement("horizontal"); if (horizontal) { const char* samples_char = horizontal->Attribute("samples"); if (samples_char) { try { ray.horizontal_samples = boost::lexical_cast(samples_char); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what()); return false; } } const char* resolution_char = horizontal->Attribute("resolution"); if (resolution_char) { try { ray.horizontal_resolution = boost::lexical_cast(resolution_char); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Ray horizontal resolution [%s] is not a valid float: %s", resolution_char, e.what()); return false; } } const char* min_angle_char = horizontal->Attribute("min_angle"); if (min_angle_char) { try { ray.horizontal_min_angle = boost::lexical_cast(min_angle_char); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Ray horizontal min_angle [%s] is not a valid float: %s", min_angle_char, e.what()); return false; } } const char* max_angle_char = horizontal->Attribute("max_angle"); if (max_angle_char) { try { ray.horizontal_max_angle = boost::lexical_cast(max_angle_char); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Ray horizontal max_angle [%s] is not a valid float: %s", max_angle_char, e.what()); return false; } } } TiXmlElement *vertical = config->FirstChildElement("vertical"); if (vertical) { const char* samples_char = vertical->Attribute("samples"); if (samples_char) { try { ray.vertical_samples = boost::lexical_cast(samples_char); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what()); return false; } } const char* resolution_char = vertical->Attribute("resolution"); if (resolution_char) { try { ray.vertical_resolution = boost::lexical_cast(resolution_char); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Ray vertical resolution [%s] is not a valid float: %s", resolution_char, e.what()); return false; } } const char* min_angle_char = vertical->Attribute("min_angle"); if (min_angle_char) { try { ray.vertical_min_angle = boost::lexical_cast(min_angle_char); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Ray vertical min_angle [%s] is not a valid float: %s", min_angle_char, e.what()); return false; } } const char* max_angle_char = vertical->Attribute("max_angle"); if (max_angle_char) { try { ray.vertical_max_angle = boost::lexical_cast(max_angle_char); } catch (boost::bad_lexical_cast &e) { CONSOLE_BRIDGE_logError("Ray vertical max_angle [%s] is not a valid float: %s", max_angle_char, e.what()); return false; } } } return false; } VisualSensorSharedPtr parseVisualSensor(TiXmlElement *g) { VisualSensorSharedPtr visual_sensor; // get sensor type TiXmlElement *sensor_xml; if (g->FirstChildElement("camera")) { Camera *camera = new Camera(); visual_sensor.reset(camera); sensor_xml = g->FirstChildElement("camera"); if (!parseCamera(*camera, sensor_xml)) visual_sensor.reset(); } else if (g->FirstChildElement("ray")) { Ray *ray = new Ray(); visual_sensor.reset(ray); sensor_xml = g->FirstChildElement("ray"); if (!parseRay(*ray, sensor_xml)) visual_sensor.reset(); } else { CONSOLE_BRIDGE_logError("No know sensor types [camera|ray] defined in block"); } return visual_sensor; } bool parseSensor(Sensor &sensor, TiXmlElement* config) { sensor.clear(); const char *name_char = config->Attribute("name"); if (!name_char) { CONSOLE_BRIDGE_logError("No name given for the sensor."); return false; } sensor.name = std::string(name_char); // parse parent_link_name const char *parent_link_name_char = config->Attribute("parent_link_name"); if (!parent_link_name_char) { CONSOLE_BRIDGE_logError("No parent_link_name given for the sensor."); return false; } sensor.parent_link_name = std::string(parent_link_name_char); // parse origin TiXmlElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(sensor.origin, o)) return false; } // parse sensor sensor.sensor = parseVisualSensor(config); return true; } } urdfdom-0.4.1/urdf_parser/src/urdf_to_graphiz.cpp000066400000000000000000000104741265476635200221770ustar00rootroot00000000000000/********************************************************************* * 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: * * * Redstributions 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 "urdf_parser/urdf_parser.h" #include #include using namespace urdf; using namespace std; void addChildLinkNames(LinkConstSharedPtr link, ofstream& os) { os << "\"" << link->name << "\" [label=\"" << link->name << "\"];" << endl; for (std::vector::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) addChildLinkNames(*child, os); } void addChildJointNames(LinkConstSharedPtr link, ofstream& os) { double r, p, y; for (std::vector::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){ (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y); os << "\"" << link->name << "\" -> \"" << (*child)->parent_joint->name << "\" [label=\"xyz: " << (*child)->parent_joint->parent_to_joint_origin_transform.position.x << " " << (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " " << (*child)->parent_joint->parent_to_joint_origin_transform.position.z << " " << "\\nrpy: " << r << " " << p << " " << y << "\"]" << endl; os << "\"" << (*child)->parent_joint->name << "\" -> \"" << (*child)->name << "\"" << endl; addChildJointNames(*child, os); } } void printTree(LinkConstSharedPtr link, string file) { std::ofstream os; os.open(file.c_str()); os << "digraph G {" << endl; os << "node [shape=box];" << endl; addChildLinkNames(link, os); os << "node [shape=ellipse, color=blue, fontcolor=blue];" << endl; addChildJointNames(link, os); os << "}" << endl; os.close(); } int main(int argc, char** argv) { if (argc != 2){ std::cerr << "Usage: urdf_to_graphiz input.xml" << std::endl; return -1; } // get the entire file std::string xml_string; std::fstream xml_file(argv[1], std::fstream::in); while ( xml_file.good() ) { std::string line; std::getline( xml_file, line); xml_string += (line + "\n"); } xml_file.close(); ModelInterfaceSharedPtr robot = parseURDF(xml_string); if (!robot){ std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; return -1; } string output = robot->getName(); // print entire tree to file printTree(robot->getRoot(), output+".gv"); cout << "Created file " << output << ".gv" << endl; string command = "dot -Tpdf "+output+".gv -o "+output+".pdf"; if (system(command.c_str()) != -1) cout << "Created file " << output << ".pdf" << endl; else cout << "There was an error executing '" << command << "'" << endl; return 0; } urdfdom-0.4.1/urdf_parser/src/world.cpp000066400000000000000000000046241265476635200201400ustar00rootroot00000000000000/********************************************************************* * 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 #include #include #include #include #include #include namespace urdf{ bool parseWorld(World &/*world*/, TiXmlElement* /*config*/) { // to be implemented return true; } bool exportWorld(World &world, TiXmlElement* xml) { TiXmlElement * world_xml = new TiXmlElement("world"); world_xml->SetAttribute("name", world.name); // to be implemented // exportModels(*world.models, world_xml); xml->LinkEndChild(world_xml); return true; } } urdfdom-0.4.1/urdf_parser/test/000077500000000000000000000000001265476635200164675ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser/test/memtest.cpp000066400000000000000000000006311265476635200206510ustar00rootroot00000000000000#include "urdf_parser/urdf_parser.h" #include #include int main(int argc, char** argv){ while (true){ std::string xml_string; std::fstream xml_file(argv[1], std::fstream::in); while ( xml_file.good() ) { std::string line; std::getline( xml_file, line); xml_string += (line + "\n"); } xml_file.close(); urdf::parseURDF(xml_string); } } urdfdom-0.4.1/urdf_parser/test/urdf_unit_test.cpp000066400000000000000000000074061265476635200222400ustar00rootroot00000000000000#include "urdf_model/pose.h" #include #include #include #include // the name of our test module #define BOOST_TEST_MODULE URDF_UNIT_TEST // needed for automatic generation of the main() #define BOOST_TEST_DYN_LINK #include bool quat_are_near(urdf::Rotation left, urdf::Rotation right) { static const double epsilon = 1e-3; // quite loose epsilon double l[4], r[4]; left.getQuaternion(l[0], l[1], l[2], l[3]); right.getQuaternion(r[0], r[1], r[2], r[3]); return (std::abs(l[0] - r[0]) < epsilon && std::abs(l[1] - r[1]) < epsilon && std::abs(l[2] - r[2]) < epsilon && std::abs(l[3] - r[3]) < epsilon) || (std::abs(l[0] + r[0]) < epsilon && std::abs(l[1] + r[1]) < epsilon && std::abs(l[2] + r[2]) < epsilon && std::abs(l[3] + r[3]) < epsilon); } std::ostream &operator<<(std::ostream &os, const urdf::Rotation& rot) { double roll, pitch, yaw; double x, y, z, w; rot.getRPY(roll, pitch, yaw); rot.getQuaternion(x, y, z, w); os << std::setprecision(9) << "x: " << x << " y: " << y << " z: " << z << " w: " << w << " roll: " << roll << " pitch: " << pitch << " yaw: "<< yaw; return os; } void check_get_set_rpy_is_idempotent(double x, double y, double z, double w) { urdf::Rotation rot0; rot0.setFromQuaternion(x, y, z, w); double roll, pitch, yaw; rot0.getRPY(roll, pitch, yaw); urdf::Rotation rot1; rot1.setFromRPY(roll, pitch, yaw); if (true) { std::cout << "\n" << "before " << rot0 << "\n" << "after " << rot1 << "\n" << "ok " << quat_are_near(rot0, rot1) << "\n"; } BOOST_CHECK(quat_are_near(rot0, rot1)); } void check_get_set_rpy_is_idempotent_from_rpy(double r, double p, double y) { urdf::Rotation rot0; rot0.setFromRPY(r, p, y); double roll, pitch, yaw; rot0.getRPY(roll, pitch, yaw); urdf::Rotation rot1; rot1.setFromRPY(roll, pitch, yaw); bool ok = quat_are_near(rot0, rot1); if (!ok) { std::cout << "initial rpy: " << r << " " << p << " " << y << "\n" << "before " << rot0 << "\n" << "after " << rot1 << "\n" << "ok " << ok << "\n"; } BOOST_CHECK(ok); } BOOST_AUTO_TEST_CASE(test_rotation_get_set_rpy_idempotent) { double x0 = 0.5, y0 = -0.5, z0 = 0.5, w0 = 0.5; check_get_set_rpy_is_idempotent(x0, y0, z0, w0); double delta = 2.2e-8; check_get_set_rpy_is_idempotent(x0, y0, z0+delta, w0-delta); // Checking consistency (in quaternion space) of set/get rpy check_get_set_rpy_is_idempotent_from_rpy(0.0,-M_PI/2,0.0); // More complete consistency check of set/get rpy // We define a list of angles (some totally random, // some instead are cornercase such as 0.0 or M_PI). // Then we check the consistency for all possible // permutations with repetition (nrOfAngles^3) std::vector testAngles; testAngles.push_back(0.0); testAngles.push_back(M_PI/4); testAngles.push_back(M_PI/3); testAngles.push_back(M_PI/2); testAngles.push_back(M_PI); testAngles.push_back(-M_PI/4); testAngles.push_back(-M_PI/3); testAngles.push_back(-M_PI/2); testAngles.push_back(-M_PI); testAngles.push_back(1.0); testAngles.push_back(1.5); testAngles.push_back(2.0); testAngles.push_back(-1.0); testAngles.push_back(-1.5); testAngles.push_back(-2.0); for(size_t rIdx = 0; rIdx < testAngles.size(); rIdx++ ) { for(size_t pIdx = 0; pIdx < testAngles.size(); pIdx++ ) { for(size_t yIdx = 0; yIdx < testAngles.size(); yIdx++ ) { check_get_set_rpy_is_idempotent_from_rpy(testAngles[rIdx], testAngles[pIdx], testAngles[yIdx]); } } } } urdfdom-0.4.1/urdf_parser_py/000077500000000000000000000000001265476635200162205ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser_py/.gitignore000066400000000000000000000000061265476635200202040ustar00rootroot00000000000000*.pyc urdfdom-0.4.1/urdf_parser_py/CMakeLists.txt000066400000000000000000000010671265476635200207640ustar00rootroot00000000000000find_program(PYTHON "python") if (NOT PYTHON) return() endif() set(SETUP_PY "${CMAKE_CURRENT_SOURCE_DIR}/setup.py") install(CODE "execute_process(COMMAND \"${PYTHON}\" \"${SETUP_PY}\" build --build-base \"${CMAKE_CURRENT_BINARY_DIR}/pybuild\" install --install-layout deb --prefix \"${CMAKE_INSTALL_PREFIX}\" WORKING_DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}\")") add_test(NAME urdf_parser_py COMMAND /usr/bin/env PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR}/src ${PYTHON} test_urdf.py WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/test) urdfdom-0.4.1/urdf_parser_py/README.md000066400000000000000000000061111265476635200174760ustar00rootroot00000000000000# urdf_parser_py ## Development Setup You must manually run `setup.py`. For catkin development, you can install to $ws/../build/lib/pythonX.Y/dist-packages via devel_prefix=$(cd $(catkin_find --first-only)/.. && pwd) cd ../urdf_parser_py python setup.py install --install-layout deb --prefix $devel_prefix Not yet sure how to get it to generate catkin-like development installs, which uses `__init__.py` to point to the development source directory. ## Authors * Thomas Moulard - `urdfpy` implementation, integration * David Lu - `urdf_python` implementation, integration * Kelsey Hawkins - `urdf_parser_python` implementation, integration * Antonio El Khoury - bugfixes * Eric Cousineau - reflection (serialization?) changes ## Reflection (or just Serialization?) This an attempt to generalize the structure of the URDF via reflection to make it easier to extend. This concept is taken from Gazebo's SDF structure, and was done with SDF in mind to a) make an SDF parser and b) make a simple converter between URDF and SDF. ### Changes * Features: * Transmission and basic Gazebo nodes. * General aggregate type, for preserving the order of aggregate types (i.e., the URDF robot model, and for the future, SDF models, links with multiple visuals / collisions, etc). This allows for basic checks to see if "scalar" elements are defined multiple times. (We were affected by this at one point with a pose defined twice with different values, screwing up the loading / saving of a model with this API). * Dumping to YAML, used for printing to string. * Doesn't preserve ordering because of it, but someone posted an issue about this on pyyaml's issue tracker, and I think a few solutions were posted (including one from the author) * XML Parsing: minidom has been swapped out with lxml.etree, but it should not be hard to change that back. Maybe Sax could be used for event-driven parsing. * API: * Loading methods rely primarily on instance methods rather than static methods, mirroring Gazebo's SDF construct-then-load method * Renamed static `parse_xml()` to `from_xml()`, and renamed `load_*` methods to `from_*` if they are static ### Todo 1. Develop a Python SDF API in a `sdf` module. * Maybe make the package itself be `robot_model_py` so that the respective modules would be `robot_model_py.urdf_parser` and `robot_model_py.sdf_parser`? * Parse Gazebo's SDF definition files at some point? For speed's sake, parse it and have it generate code to use? 2. Make a direct, two-way URDF <-> SDF converter. * Gazebo has the ability to load URDFs and save SDFs, but it lumps everything together and (I think) adds some "noise" from OpenDE for positions. 3. Make the names a little clearer, especially the fact that `from_xml` and `to_xml` write to a node, but do not create a new one. 4. Figure out good policy for handling default methods. If saving to XML, write out default values, or leave them out for brevity (and to leave it open for change)? Might be best to add that as an option. 5. Find a lightweight package that can handle the reflection aspect more elegantly. Enthought traits? IPython's spinoff of traits? urdfdom-0.4.1/urdf_parser_py/scripts/000077500000000000000000000000001265476635200177075ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser_py/scripts/display_urdf000077500000000000000000000011431265476635200223210ustar00rootroot00000000000000#!/usr/bin/env python import sys import argparse from urdf_parser_py.urdf import URDF parser = argparse.ArgumentParser(usage='Load an URDF file') parser.add_argument('file', type=argparse.FileType('r'), nargs='?', default=None, help='File to load. Use - for stdin') parser.add_argument('-o', '--output', type=argparse.FileType('w'), default=None, help='Dump file to XML') args = parser.parse_args() if args.file is None: robot = URDF.from_parameter_server() else: robot = URDF.from_xml_string(args.file.read()) print(robot) if args.output is not None: args.output.write(robot.to_xml_string()) urdfdom-0.4.1/urdf_parser_py/setup.py000066400000000000000000000014001265476635200177250ustar00rootroot00000000000000#!/usr/bin/env python from distutils.core import setup d = {'author': u'Thomas Moulard , David Lu , Kelsey Hawkins , Antonio El Khoury , Eric Cousineau ', 'description': 'The urdf_parser_py package contains a Python implementation of the\nurdf_parser modeling various aspects of robot information, specified in the\nXml Robot Description Format (URDF).', 'license': 'BSD', 'maintainer': u'Thomas Moulard', 'maintainer_email': 'thomas.moulard@gmail.com', 'name': 'urdf_parser_py', 'package_dir': {'': 'src'}, 'packages': ['urdf_parser_py', 'urdf_parser_py.xml_reflection'], 'url': 'http://ros.org/wiki/urdf_parser_py', 'version': '0.3.0'} setup(**d) urdfdom-0.4.1/urdf_parser_py/src/000077500000000000000000000000001265476635200170075ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser_py/src/urdf_parser_py/000077500000000000000000000000001265476635200220335ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser_py/src/urdf_parser_py/__init__.py000066400000000000000000000000001265476635200241320ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser_py/src/urdf_parser_py/sdf.py000066400000000000000000000061221265476635200231620ustar00rootroot00000000000000from urdf_parser_py.xml_reflection.basics import * import urdf_parser_py.xml_reflection as xmlr # What is the scope of plugins? Model, World, Sensor? xmlr.start_namespace('sdf') class Pose(xmlr.Object): def __init__(self, vec=None, extra=None): self.xyz = None self.rpy = None if vec is not None: assert isinstance(vec, list) count = len(vec) if len == 3: xyz = vec else: self.from_vec(vec) elif extra is not None: assert xyz is None, "Cannot specify 6-length vector and 3-length vector" assert len(extra) == 3, "Invalid length" self.rpy = extra def from_vec(self, vec): assert len(vec) == 6, "Invalid length" self.xyz = vec[:3] self.rpy = vec[3:6] def as_vec(self): xyz = self.xyz if self.xyz else [0, 0, 0] rpy = self.rpy if self.rpy else [0, 0, 0] return xyz + rpy def read_xml(self, node): # Better way to do this? Define type? vec = get_type('vector6').read_xml(node) self.load_vec(vec) def write_xml(self, node): vec = self.as_vec() get_type('vector6').write_xml(node, vec) def check_valid(self): assert self.xyz is not None or self.rpy is not None name_attribute = xmlr.Attribute('name', str) pose_element = xmlr.Element('pose', Pose, False) class Entity(xmlr.Object): def __init__(self, name = None, pose = None): self.name = name self.pose = pose xmlr.reflect(Entity, params = [ name_attribute, pose_element ]) class Inertia(xmlr.Object): KEYS = ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz'] def __init__(self, ixx=0.0, ixy=0.0, ixz=0.0, iyy=0.0, iyz=0.0, izz=0.0): self.ixx = ixx self.ixy = ixy self.ixz = ixz self.iyy = iyy self.iyz = iyz self.izz = izz def to_matrix(self): return [ [self.ixx, self.ixy, self.ixz], [self.ixy, self.iyy, self.iyz], [self.ixz, self.iyz, self.izz]] xmlr.reflect(Inertia, params = [xmlr.Element(key, float) for key in Inertia.KEYS]) # Pretty much copy-paste... Better method? # Use multiple inheritance to separate the objects out so they are unique? class Inertial(xmlr.Object): def __init__(self, mass = 0.0, inertia = None, pose=None): self.mass = mass self.inertia = inertia self.pose = pose xmlr.reflect(Inertial, params = [ xmlr.Element('mass', float), xmlr.Element('inertia', Inertia), pose_element ]) class Link(Entity): def __init__(self, name = None, pose = None, inertial = None, kinematic = False): Entity.__init__(self, name, pose) self.inertial = inertial self.kinematic = kinematic xmlr.reflect(Link, parent_cls = Entity, params = [ xmlr.Element('inertial', Inertial), xmlr.Attribute('kinematic', bool, False), xmlr.AggregateElement('visual', Visual, var = 'visuals'), xmlr.AggregateElement('collision', Collision, var = 'collisions') ]) class Model(Entity): def __init__(self, name = None, pose=None): Entity.__init__(self, name, pose) self.links = [] self.joints = [] self.plugins = [] xmlr.reflect(Model, parent_cls = Entity, params = [ xmlr.AggregateElement('link', Link, var = 'links'), xmlr.AggregateElement('joint', Joint, var = 'joints'), xmlr.AggregateElement('plugin', Plugin, var = 'plugins') ]) xmlr.end_namespace('sdf')urdfdom-0.4.1/urdf_parser_py/src/urdf_parser_py/urdf.py000066400000000000000000000305641265476635200233550ustar00rootroot00000000000000from urdf_parser_py.xml_reflection.basics import * import urdf_parser_py.xml_reflection as xmlr # Add a 'namespace' for names so that things don't conflict between URDF and SDF? # A type registry? How to scope that? Just make a 'global' type pointer? # Or just qualify names? urdf.geometric, sdf.geometric xmlr.start_namespace('urdf') xmlr.add_type('element_link', xmlr.SimpleElementType('link', str)) xmlr.add_type('element_xyz', xmlr.SimpleElementType('xyz', 'vector3')) verbose = True class Pose(xmlr.Object): def __init__(self, xyz=None, rpy=None): self.xyz = xyz self.rpy = rpy def check_valid(self): assert self.xyz is not None or self.rpy is not None # Aliases for backwards compatibility @property def rotation(self): return self.rpy @rotation.setter def rotation(self, value): self.rpy = value @property def position(self): return self.xyz @position.setter def position(self, value): self.xyz = value xmlr.reflect(Pose, params = [ xmlr.Attribute('xyz', 'vector3', False), xmlr.Attribute('rpy', 'vector3', False) ]) # Common stuff name_attribute = xmlr.Attribute('name', str) origin_element = xmlr.Element('origin', Pose, False) class Color(xmlr.Object): def __init__(self, *args): # What about named colors? count = len(args) if count == 4 or count == 3: self.rgba = args elif count == 1: self.rgba = args[0] elif count == 0: self.rgba = None if self.rgba is not None: if len(self.rgba) == 3: self.rgba += [1.] if len(self.rgba) != 4: raise Exception('Invalid color argument count') xmlr.reflect(Color, params = [ xmlr.Attribute('rgba', 'vector4') ]) class JointDynamics(xmlr.Object): def __init__(self, damping=None, friction=None): self.damping = damping self.friction = friction xmlr.reflect(JointDynamics, params = [ xmlr.Attribute('damping', float, False), xmlr.Attribute('friction', float, False) ]) class Box(xmlr.Object): def __init__(self, size = None): self.size = size xmlr.reflect(Box, params = [ xmlr.Attribute('size', 'vector3') ]) class Cylinder(xmlr.Object): def __init__(self, radius = 0.0, length = 0.0): self.radius = radius self.length = length xmlr.reflect(Cylinder, params = [ xmlr.Attribute('radius', float), xmlr.Attribute('length', float) ]) class Sphere(xmlr.Object): def __init__(self, radius=0.0): self.radius = radius xmlr.reflect(Sphere, params = [ xmlr.Attribute('radius', float) ]) class Mesh(xmlr.Object): def __init__(self, filename = None, scale = None): self.filename = filename self.scale = scale xmlr.reflect(Mesh, params = [ xmlr.Attribute('filename', str), xmlr.Attribute('scale', 'vector3', required=False) ]) class GeometricType(xmlr.ValueType): def __init__(self): self.factory = xmlr.FactoryType('geometric', { 'box': Box, 'cylinder': Cylinder, 'sphere': Sphere, 'mesh': Mesh }) def from_xml(self, node): children = xml_children(node) assert len(children) == 1, 'One element only for geometric' return self.factory.from_xml(children[0]) def write_xml(self, node, obj): name = self.factory.get_name(obj) child = node_add(node, name) obj.write_xml(child) xmlr.add_type('geometric', GeometricType()) class Collision(xmlr.Object): def __init__(self, geometry = None, origin = None): self.geometry = geometry self.origin = origin xmlr.reflect(Collision, params = [ origin_element, xmlr.Element('geometry', 'geometric') ]) class Texture(xmlr.Object): def __init__(self, filename = None): self.filename = filename xmlr.reflect(Texture, params = [ xmlr.Attribute('filename', str) ]) class Material(xmlr.Object): def __init__(self, name=None, color=None, texture=None): self.name = name self.color = color self.texture = texture def check_valid(self): if self.color is None and self.texture is None: xmlr.on_error("Material has neither a color nor texture") xmlr.reflect(Material, params = [ name_attribute, xmlr.Element('color', Color, False), xmlr.Element('texture', Texture, False) ]) class LinkMaterial(Material): def check_valid(self): pass class Visual(xmlr.Object): def __init__(self, geometry = None, material = None, origin = None): self.geometry = geometry self.material = material self.origin = origin xmlr.reflect(Visual, params = [ origin_element, xmlr.Element('geometry', 'geometric'), xmlr.Element('material', LinkMaterial, False) ]) class Inertia(xmlr.Object): KEYS = ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz'] def __init__(self, ixx=0.0, ixy=0.0, ixz=0.0, iyy=0.0, iyz=0.0, izz=0.0): self.ixx = ixx self.ixy = ixy self.ixz = ixz self.iyy = iyy self.iyz = iyz self.izz = izz def to_matrix(self): return [ [self.ixx, self.ixy, self.ixz], [self.ixy, self.iyy, self.iyz], [self.ixz, self.iyz, self.izz]] xmlr.reflect(Inertia, params = [xmlr.Attribute(key, float) for key in Inertia.KEYS]) class Inertial(xmlr.Object): def __init__(self, mass = 0.0, inertia = None, origin=None): self.mass = mass self.inertia = inertia self.origin = origin xmlr.reflect(Inertial, params = [ origin_element, xmlr.Element('mass', 'element_value'), xmlr.Element('inertia', Inertia, False) ]) #FIXME: we are missing the reference position here. class JointCalibration(xmlr.Object): def __init__(self, rising=None, falling=None): self.rising = rising self.falling = falling xmlr.reflect(JointCalibration, params = [ xmlr.Attribute('rising', float, False, 0), xmlr.Attribute('falling', float, False, 0) ]) class JointLimit(xmlr.Object): def __init__(self, effort=None, velocity=None, lower=None, upper=None): self.effort = effort self.velocity = velocity self.lower = lower self.upper = upper xmlr.reflect(JointLimit, params = [ xmlr.Attribute('effort', float), xmlr.Attribute('lower', float, False, 0), xmlr.Attribute('upper', float, False, 0), xmlr.Attribute('velocity', float) ]) #FIXME: we are missing __str__ here. class JointMimic(xmlr.Object): def __init__(self, joint_name=None, multiplier=None, offset=None): self.joint = joint_name self.multiplier = multiplier self.offset = offset xmlr.reflect(JointMimic, params = [ xmlr.Attribute('joint', str), xmlr.Attribute('multiplier', float, False), xmlr.Attribute('offset', float, False) ]) class SafetyController(xmlr.Object): def __init__(self, velocity=None, position=None, lower=None, upper=None): self.k_velocity = velocity self.k_position = position self.soft_lower_limit = lower self.soft_upper_limit = upper xmlr.reflect(SafetyController, params = [ xmlr.Attribute('k_velocity', float), xmlr.Attribute('k_position', float, False, 0), xmlr.Attribute('soft_lower_limit', float, False, 0), xmlr.Attribute('soft_upper_limit', float, False, 0) ]) class Joint(xmlr.Object): TYPES = ['unknown', 'revolute', 'continuous', 'prismatic', 'floating', 'planar', 'fixed'] def __init__(self, name=None, parent=None, child=None, joint_type=None, axis=None, origin=None, limit=None, dynamics=None, safety_controller=None, calibration=None, mimic=None): self.name = name self.parent = parent self.child = child self.type = joint_type self.axis = axis self.origin = origin self.limit = limit self.dynamics = dynamics self.safety_controller = safety_controller self.calibration = calibration self.mimic = mimic def check_valid(self): assert self.type in self.TYPES, "Invalid joint type: {}".format(self.type) # Aliases @property def joint_type(self): return self.type @joint_type.setter def joint_type(self, value): self.type = value xmlr.reflect(Joint, params = [ name_attribute, xmlr.Attribute('type', str), origin_element, xmlr.Element('axis', 'element_xyz', False), xmlr.Element('parent', 'element_link'), xmlr.Element('child', 'element_link'), xmlr.Element('limit', JointLimit, False), xmlr.Element('dynamics', JointDynamics, False), xmlr.Element('safety_controller', SafetyController, False), xmlr.Element('calibration', JointCalibration, False), xmlr.Element('mimic', JointMimic, False), ]) class Link(xmlr.Object): def __init__(self, name=None, visual=None, inertial=None, collision=None, origin = None): self.name = name self.visual = visual self.inertial = inertial self.collision = collision self.origin = origin xmlr.reflect(Link, params = [ name_attribute, origin_element, xmlr.Element('inertial', Inertial, False), xmlr.Element('visual', Visual, False), xmlr.Element('collision', Collision, False) ]) class PR2Transmission(xmlr.Object): def __init__(self, name = None, joint = None, actuator = None, type = None, mechanicalReduction = 1): self.name = name self.type = type self.joint = joint self.actuator = actuator self.mechanicalReduction = mechanicalReduction xmlr.reflect(PR2Transmission, tag = 'pr2_transmission', params = [ name_attribute, xmlr.Attribute('type', str), xmlr.Element('joint', 'element_name'), xmlr.Element('actuator', 'element_name'), xmlr.Element('mechanicalReduction', float) ]) class Actuator(xmlr.Object): def __init__(self, name = None, mechanicalReduction = 1): self.name = name self.mechanicalReduction = None xmlr.reflect(Actuator, tag = 'actuator', params = [ name_attribute, xmlr.Element('mechanicalReduction', float, required = False) ]) class TransmissionJoint(xmlr.Object): def __init__(self, name = None): self.aggregate_init() self.name = name self.hardwareInterfaces = [] def check_valid(self): assert len(self.hardwareInterfaces) > 0, "no hardwareInterface defined" xmlr.reflect(TransmissionJoint, tag = 'joint', params = [ name_attribute, xmlr.AggregateElement('hardwareInterface', str), ]) class Transmission(xmlr.Object): """ New format: http://wiki.ros.org/urdf/XML/Transmission """ def __init__(self, name = None): self.aggregate_init() self.name = name self.joints = [] self.actuators = [] def check_valid(self): assert len(self.joints) > 0, "no joint defined" assert len(self.actuators) > 0, "no actuator defined" xmlr.reflect(Transmission, tag = 'new_transmission', params = [ name_attribute, xmlr.Element('type', str), xmlr.AggregateElement('joint', TransmissionJoint), xmlr.AggregateElement('actuator', Actuator) ]) xmlr.add_type('transmission', xmlr.DuckTypedFactory('transmission', [Transmission, PR2Transmission])) class Robot(xmlr.Object): def __init__(self, name = None): self.aggregate_init() self.name = name self.joints = [] self.links = [] self.materials = [] self.gazebos = [] self.transmissions = [] self.joint_map = {} self.link_map = {} self.parent_map = {} self.child_map = {} def add_aggregate(self, typeName, elem): xmlr.Object.add_aggregate(self, typeName, elem) if typeName == 'joint': joint = elem self.joint_map[joint.name] = joint self.parent_map[ joint.child ] = (joint.name, joint.parent) if joint.parent in self.child_map: self.child_map[joint.parent].append( (joint.name, joint.child) ) else: self.child_map[joint.parent] = [ (joint.name, joint.child) ] elif typeName == 'link': link = elem self.link_map[link.name] = link def add_link(self, link): self.add_aggregate('link', link) def add_joint(self, joint): self.add_aggregate('joint', joint) def get_chain(self, root, tip, joints=True, links=True, fixed=True): chain = [] if links: chain.append(tip) link = tip while link != root: (joint, parent) = self.parent_map[link] if joints: if fixed or self.joint_map[joint].joint_type != 'fixed': chain.append(joint) if links: chain.append(parent) link = parent chain.reverse() return chain def get_root(self): root = None for link in self.link_map: if link not in self.parent_map: assert root is None, "Multiple roots detected, invalid URDF." root = link assert root is not None, "No roots detected, invalid URDF." return root @classmethod def from_parameter_server(cls, key = 'robot_description'): """ Retrieve the robot model on the parameter server and parse it to create a URDF robot structure. Warning: this requires roscore to be running. """ # Could move this into xml_reflection import rospy return cls.from_xml_string(rospy.get_param(key)) xmlr.reflect(Robot, tag = 'robot', params = [ xmlr.Attribute('name', str, False), # Is 'name' a required attribute? xmlr.AggregateElement('link', Link), xmlr.AggregateElement('joint', Joint), xmlr.AggregateElement('gazebo', xmlr.RawType()), xmlr.AggregateElement('transmission', 'transmission'), xmlr.AggregateElement('material', Material) ]) # Make an alias URDF = Robot xmlr.end_namespace() urdfdom-0.4.1/urdf_parser_py/src/urdf_parser_py/xml_reflection/000077500000000000000000000000001265476635200250455ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser_py/src/urdf_parser_py/xml_reflection/__init__.py000066400000000000000000000000611265476635200271530ustar00rootroot00000000000000from urdf_parser_py.xml_reflection.core import * urdfdom-0.4.1/urdf_parser_py/src/urdf_parser_py/xml_reflection/basics.py000066400000000000000000000045401265476635200266660ustar00rootroot00000000000000import string import yaml import collections from lxml import etree # Different implementations mix well it seems # @todo Do not use this? from xml.etree.ElementTree import ElementTree def xml_string(rootXml, addHeader = True): # Meh xmlString = etree.tostring(rootXml, pretty_print = True) if addHeader: xmlString = '\n' + xmlString return xmlString def dict_sub(obj, keys): return dict((key, obj[key]) for key in keys) def node_add(doc, sub): if sub is None: return None if type(sub) == str: return etree.SubElement(doc, sub) elif isinstance(sub, etree._Element): doc.append(sub) # This screws up the rest of the tree for prettyprint... return sub else: raise Exception('Invalid sub value') def pfloat(x): return str(x).rstrip('.') def xml_children(node): children = node.getchildren() def predicate(node): return not isinstance(node, etree._Comment) return list(filter(predicate, children)) def isstring(obj): try: return isinstance(obj, basestring) except NameError: return isinstance(obj, str) def to_yaml(obj): """ Simplify yaml representation for pretty printing """ # Is there a better way to do this by adding a representation with yaml.Dumper? # Ordered dict: http://pyyaml.org/ticket/29#comment:11 if obj is None or isstring(obj): out = str(obj) elif type(obj) in [int, float, bool]: return obj elif hasattr(obj, 'to_yaml'): out = obj.to_yaml() elif isinstance(obj, etree._Element): out = etree.tostring(obj, pretty_print = True) elif type(obj) == dict: out = {} for (var, value) in obj.items(): out[str(var)] = to_yaml(value) elif hasattr(obj, 'tolist'): # For numpy objects out = to_yaml(obj.tolist()) elif isinstance(obj, collections.Iterable): out = [to_yaml(item) for item in obj] else: out = str(obj) return out class SelectiveReflection(object): def get_refl_vars(self): return list(vars(self).keys()) class YamlReflection(SelectiveReflection): def to_yaml(self): raw = dict((var, getattr(self, var)) for var in self.get_refl_vars()) return to_yaml(raw) def __str__(self): return yaml.dump(self.to_yaml()).rstrip() # Good idea? Will it remove other important things? urdfdom-0.4.1/urdf_parser_py/src/urdf_parser_py/xml_reflection/core.py000066400000000000000000000374431265476635200263620ustar00rootroot00000000000000from urdf_parser_py.xml_reflection.basics import * import sys import copy # @todo Get rid of "import *" # @todo Make this work with decorators # Is this reflection or serialization? I think it's serialization... # Rename? # Do parent operations after, to allow child to 'override' parameters? # Need to make sure that duplicate entires do not get into the 'unset*' lists def reflect(cls, *args, **kwargs): """ Simple wrapper to add XML reflection to an xml_reflection.Object class """ cls.XML_REFL = Reflection(*args, **kwargs) # Rename 'write_xml' to 'write_xml' to have paired 'load/dump', and make 'pre_dump' and 'post_load'? # When dumping to yaml, include tag name? # How to incorporate line number and all that jazz? def on_error(message): """ What to do on an error. This can be changed to raise an exception. """ sys.stderr.write(message) skip_default = True #defaultIfMatching = True # Not implemeneted yet # Registering Types value_types = {} value_type_prefix = '' def start_namespace(namespace): """ Basic mechanism to prevent conflicts for string types for URDF and SDF @note Does not handle nesting! """ global value_type_prefix value_type_prefix = namespace + '.' def end_namespace(): global value_type_prefix value_type_prefix = '' def add_type(key, value): if isinstance(key, str): key = value_type_prefix + key assert key not in value_types value_types[key] = value def get_type(cur_type): """ Can wrap value types if needed """ if value_type_prefix and isinstance(cur_type, str): # See if it exists in current 'namespace' curKey = value_type_prefix + cur_type value_type = value_types.get(curKey) else: value_type = None if value_type is None: # Try again, in 'global' scope value_type = value_types.get(cur_type) if value_type is None: value_type = make_type(cur_type) add_type(cur_type, value_type) return value_type def make_type(cur_type): if isinstance(cur_type, ValueType): return cur_type elif isinstance(cur_type, str): if cur_type.startswith('vector'): extra = cur_type[6:] if extra: count = float(extra) else: count = None return VectorType(count) else: raise Exception("Invalid value type: {}".format(cur_type)) elif cur_type == list: return ListType() elif issubclass(cur_type, Object): return ObjectType(cur_type) elif cur_type in [str, float]: return BasicType(cur_type) else: raise Exception("Invalid type: {}".format(cur_type)) class ValueType(object): """ Primitive value type """ def from_xml(self, node): return self.from_string(node.text) def write_xml(self, node, value): """ If type has 'write_xml', this function should expect to have it's own XML already created i.e., In Axis.to_sdf(self, node), 'node' would be the 'axis' element. @todo Add function that makes an XML node completely independently?""" node.text = self.to_string(value) def equals(self, a, b): return a == b class BasicType(ValueType): def __init__(self, cur_type): self.type = cur_type def to_string(self, value): return str(value) def from_string(self, value): return self.type(value) class ListType(ValueType): def to_string(self, values): return ' '.join(values) def from_string(self, text): return text.split() def equals(self, aValues, bValues): return len(aValues) == len(bValues) and all(a == b for (a, b) in zip(aValues, bValues)) class VectorType(ListType): def __init__(self, count = None): self.count = count def check(self, values): if self.count is not None: assert len(values) == self.count, "Invalid vector length" def to_string(self, values): self.check(values) raw = list(map(str, values)) return ListType.to_string(self, raw) def from_string(self, text): raw = ListType.from_string(self, text) self.check(raw) return list(map(float, raw)) class RawType(ValueType): """ Simple, raw XML value. Need to bugfix putting this back into a document """ def from_xml(self, node): return node def write_xml(self, node, value): # @todo rying to insert an element at root level seems to screw up pretty printing children = xml_children(value) list(map(node.append, children)) # Copy attributes for (attrib_key, attrib_value) in value.attrib.iteritems(): node.set(attrib_key, attrib_value) class SimpleElementType(ValueType): """ Extractor that retrieves data from an element, given a specified attribute, casted to value_type. """ def __init__(self, attribute, value_type): self.attribute = attribute self.value_type = get_type(value_type) def from_xml(self, node): text = node.get(self.attribute) return self.value_type.from_string(text) def write_xml(self, node, value): text = self.value_type.to_string(value) node.set(self.attribute, text) class ObjectType(ValueType): def __init__(self, cur_type): self.type = cur_type def from_xml(self, node): obj = self.type() obj.read_xml(node) return obj def write_xml(self, node, obj): obj.write_xml(node) class FactoryType(ValueType): def __init__(self, name, typeMap): self.name = name self.typeMap = typeMap self.nameMap = {} for (key, value) in typeMap.items(): # Reverse lookup self.nameMap[value] = key def from_xml(self, node): cur_type = self.typeMap.get(node.tag) if cur_type is None: raise Exception("Invalid {} tag: {}".format(self.name, node.tag)) value_type = get_type(cur_type) return value_type.from_xml(node) def get_name(self, obj): cur_type = type(obj) name = self.nameMap.get(cur_type) if name is None: raise Exception("Invalid {} type: {}".format(self.name, cur_type)) return name def write_xml(self, node, obj): obj.write_xml(node) class DuckTypedFactory(ValueType): def __init__(self, name, typeOrder): self.name = name assert len(typeOrder) > 0 self.type_order = typeOrder def from_xml(self, node): error_set = [] for value_type in self.type_order: try: return value_type.from_xml(node) except Exception as e: error_set.append((value_type, e)) # Should have returned, we encountered errors out = "Could not perform duck-typed parsing." for (value_type, e) in error_set: out += "\nValue Type: {}\nException: {}\n".format(value_type, e) raise Exception(out) def write_xml(self, node, obj): obj.write_xml(node) class Param(object): """ Mirroring Gazebo's SDF api @param xml_var: Xml name @todo If the value_type is an object with a tag defined in it's reflection, allow it to act as the default tag name? @param var: Python class variable name. By default it's the same as the XML name """ def __init__(self, xml_var, value_type, required = True, default = None, var = None): self.xml_var = xml_var if var is None: self.var = xml_var else: self.var = var self.type = None self.value_type = get_type(value_type) self.default = default if required: assert default is None, "Default does not make sense for a required field" self.required = required self.is_aggregate = False def set_default(self): if self.required: raise Exception("Required {} not set in XML: {}".format(self.type, self.xml_var)) elif not skip_default: setattr(obj, self.var, self.default) class Attribute(Param): def __init__(self, xml_var, value_type, required = True, default = None, var = None): Param.__init__(self, xml_var, value_type, required, default, var) self.type = 'attribute' def set_from_string(self, obj, value): """ Node is the parent node in this case """ # Duplicate attributes cannot occur at this point setattr(obj, self.var, self.value_type.from_string(value)) def add_to_xml(self, obj, node): value = getattr(obj, self.var) # Do not set with default value if value is None if value is None: if self.required: raise Exception("Required attribute not set in object: {}".format(self.var)) elif not skip_default: value = self.default # Allow value type to handle None? if value is not None: node.set(self.xml_var, self.value_type.to_string(value)) # Add option if this requires a header? Like .... ??? Not really... This would be a specific list type, not really aggregate class Element(Param): def __init__(self, xml_var, value_type, required = True, default = None, var = None, is_raw = False): Param.__init__(self, xml_var, value_type, required, default, var) self.type = 'element' self.is_raw = is_raw def set_from_xml(self, obj, node): value = self.value_type.from_xml(node) setattr(obj, self.var, value) def add_to_xml(self, obj, parent): value = getattr(obj, self.xml_var) if value is None: if self.required: raise Exception("Required element not defined in object: {}".format(self.var)) elif not skip_default: value = self.default if value is not None: self.add_scalar_to_xml(parent, value) def add_scalar_to_xml(self, parent, value): if self.is_raw: node = parent else: node = node_add(parent, self.xml_var) self.value_type.write_xml(node, value) class AggregateElement(Element): def __init__(self, xml_var, value_type, var = None, is_raw = False): if var is None: var = xml_var + 's' Element.__init__(self, xml_var, value_type, required = False, var = var, is_raw = is_raw) self.is_aggregate = True def add_from_xml(self, obj, node): value = self.value_type.from_xml(node) obj.add_aggregate(self.xml_var, value) def set_default(self): pass class Info: """ Small container for keeping track of what's been consumed """ def __init__(self, node): self.attributes = list(node.attrib.keys()) self.children = xml_children(node) class Reflection(object): def __init__(self, params = [], parent_cls = None, tag = None): """ Construct a XML reflection thing @param parent_cls: Parent class, to use it's reflection as well. @param tag: Only necessary if you intend to use Object.write_xml_doc() This does not override the name supplied in the reflection definition thing. """ if parent_cls is not None: self.parent = parent_cls.XML_REFL else: self.parent = None self.tag = tag # Laziness for now attributes = [] elements = [] for param in params: if isinstance(param, Element): elements.append(param) else: attributes.append(param) self.vars = [] self.paramMap = {} self.attributes = attributes self.attribute_map = {} self.required_attribute_names = [] for attribute in attributes: self.attribute_map[attribute.xml_var] = attribute self.paramMap[attribute.xml_var] = attribute self.vars.append(attribute.var) if attribute.required: self.required_attribute_names.append(attribute.xml_var) self.elements = [] self.element_map = {} self.required_element_names = [] self.aggregates = [] self.scalars = [] self.scalarNames = [] for element in elements: self.element_map[element.xml_var] = element self.paramMap[element.xml_var] = element self.vars.append(element.var) if element.required: self.required_element_names.append(element.xml_var) if element.is_aggregate: self.aggregates.append(element) else: self.scalars.append(element) self.scalarNames.append(element.xml_var) def set_from_xml(self, obj, node, info = None): is_final = False if info is None: is_final = True info = Info(node) if self.parent: self.parent.set_from_xml(obj, node, info) # Make this a map instead? Faster access? {name: isSet} ? unset_attributes = list(self.attribute_map.keys()) unset_scalars = copy.copy(self.scalarNames) # Better method? Queues? for xml_var in copy.copy(info.attributes): attribute = self.attribute_map.get(xml_var) if attribute is not None: value = node.attrib[xml_var] attribute.set_from_string(obj, value) unset_attributes.remove(xml_var) info.attributes.remove(xml_var) # Parse unconsumed nodes for child in copy.copy(info.children): tag = child.tag element = self.element_map.get(tag) if element is not None: if element.is_aggregate: element.add_from_xml(obj, child) else: if tag in unset_scalars: element.set_from_xml(obj, child) unset_scalars.remove(tag) else: on_error("Scalar element defined multiple times: {}".format(tag)) info.children.remove(child) for attribute in map(self.attribute_map.get, unset_attributes): attribute.set_default() for element in map(self.element_map.get, unset_scalars): element.set_default() if is_final: for xml_var in info.attributes: on_error('Unknown attribute: {}'.format(xml_var)) for node in info.children: on_error('Unknown tag: {}'.format(node.tag)) def add_to_xml(self, obj, node): if self.parent: self.parent.add_to_xml(obj, node) for attribute in self.attributes: attribute.add_to_xml(obj, node) for element in self.scalars: element.add_to_xml(obj, node) # Now add in aggregates if self.aggregates: obj.add_aggregates_to_xml(node) class Object(YamlReflection): """ Raw python object for yaml / xml representation """ XML_REFL = None def get_refl_vars(self): return self.XML_REFL.vars def check_valid(self): pass def pre_write_xml(self): """ If anything needs to be converted prior to dumping to xml i.e., getting the names of objects and such """ pass def write_xml(self, node): """ Adds contents directly to XML node """ self.check_valid() self.pre_write_xml() self.XML_REFL.add_to_xml(self, node) def to_xml(self): """ Creates an overarching tag and adds its contents to the node """ tag = self.XML_REFL.tag assert tag is not None, "Must define 'tag' in reflection to use this function" doc = etree.Element(tag) self.write_xml(doc) return doc def to_xml_string(self): return xml_string(self.to_xml()) def post_read_xml(self): pass def read_xml(self, node): self.XML_REFL.set_from_xml(self, node) self.post_read_xml() self.check_valid() @classmethod def from_xml(cls, node): cur_type = get_type(cls) return cur_type.from_xml(node) @classmethod def from_xml_string(cls, xml_string): node = etree.fromstring(xml_string) return cls.from_xml(node) @classmethod def from_xml_file(cls, file_path): xml_string= open(file_path, 'r').read() return cls.from_xml_string(xml_string) # Confusing distinction between loading code in object and reflection registry thing... def get_aggregate_list(self, xml_var): var = self.XML_REFL.paramMap[xml_var].var values = getattr(self, var) assert isinstance(values, list) return values def aggregate_init(self): """ Must be called in constructor! """ self.aggregate_order = [] # Store this info in the loaded object??? Nah self.aggregate_type = {} def add_aggregate(self, xml_var, obj): """ NOTE: One must keep careful track of aggregate types for this system. Can use 'lump_aggregates()' before writing if you don't care. """ self.get_aggregate_list(xml_var).append(obj) self.aggregate_order.append(obj) self.aggregate_type[obj] = xml_var def add_aggregates_to_xml(self, node): for value in self.aggregate_order: typeName = self.aggregate_type[value] element = self.XML_REFL.element_map[typeName] element.add_scalar_to_xml(node, value) def remove_aggregate(self, obj): self.aggregate_order.remove(obj) xml_var = self.aggregate_type[obj] del self.aggregate_type[obj] self.get_aggregate_list(xml_var).remove(obj) def lump_aggregates(self): """ Put all aggregate types together, just because """ self.aggregate_init() for param in self.XML_REFL.aggregates: for obj in self.get_aggregate_list(param.xml_var): self.add_aggregate(param.var, obj) """ Compatibility """ def parse(self, xml_string): node = etree.fromstring(xml_string) self.read_xml(node) return self # Really common types # Better name: element_with_name? Attributed element? add_type('element_name', SimpleElementType('name', str)) add_type('element_value', SimpleElementType('value', float)) # Add in common vector types so they aren't absorbed into the namespaces get_type('vector3') get_type('vector4') get_type('vector6') urdfdom-0.4.1/urdf_parser_py/test/000077500000000000000000000000001265476635200171775ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser_py/test/calvin/000077500000000000000000000000001265476635200204535ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser_py/test/calvin/calvin.urdf000066400000000000000000001373571265476635200226310ustar00rootroot00000000000000 true cmd_vel odom joint_states left_front_wheel_joint left_middle_wheel_joint left_rear_wheel_joint right_front_wheel_joint right_middle_wheel_joint right_rear_wheel_joint 0.41 0.197 0.95 4.0 0.9 40.0 true 30.0 base_footprint base_footprint_pose_ground_truth 0.0 map true false scan 75.0 laser scan 0.005 75.0 181 1 -1.571 1.571 0.08 10.0 0.01 true 20.0 1.57 640 480 R8G8B8 0.1 100 webcam image_raw camera_info webcam_optical_link transmission_interface/SimpleTransmission EffortJointInterface 1 transmission_interface/SimpleTransmission EffortJointInterface 1 transmission_interface/SimpleTransmission EffortJointInterface 1 transmission_interface/SimpleTransmission EffortJointInterface 1 transmission_interface/SimpleTransmission EffortJointInterface 1 transmission_interface/SimpleTransmission EffortJointInterface 1 transmission_interface/SimpleTransmission EffortJointInterface 1 gazebo_ros_control/DefaultRobotHWSim true katana katana_r_finger_joint katana_l_finger_joint 1000.0 0.5 20 40 2 katana_r_finger_link katana_l_finger_link katana_gripper_link true true true true true true true true -3.022 2.163 -2.207 -2.026 -2.990 0.3 0.3 true 1.0 0.994837673637 L8 640 480 0.01 5 true 1.0 kinect_ir /kinect/depth/image_raw /kinect/depth/camera_info /kinect/depth/image_raw /kinect/depth/camera_info /kinect/depth/points kinect_depth_optical_frame 0.5 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true 1.0 0.994837673637 R8G8B8 640 480 0.01 5 true 1.0 kinect_rgb /kinect/rgb/image_raw /kinect/rgb/camera_info /kinect/depth/image_raw /kinect/depth/camera_info /kinect/depth_registered/points kinect_rgb_optical_frame 0.5 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 transmission_interface/SimpleTransmission EffortJointInterface 1 transmission_interface/SimpleTransmission EffortJointInterface 1 true 100.0 imu imu /default_imu 2.89e-08 0 0 0 0 0 0 true 15.0 0 0 0 0 0 0 false 271 1 -2.357 2.357 0.05 4.0 0.01 0.005 true 15.0 scan laser urdfdom-0.4.1/urdf_parser_py/test/calvin/gen.patch000066400000000000000000001623561265476635200222620ustar00rootroot00000000000000--- ./calvin.urdf 2014-01-18 18:57:26.178433701 -0600 +++ /tmp/calvin.urdf 2014-01-18 19:38:06.758197834 -0600 @@ -1,282 +1,238 @@ - - - - - - + + + + + + + - + - - + - + - - - - - - + + + + + - + - - + - - + - - - - - - - - - - + - + + - + + + + + - + - - + - - + - - - - - - - - - - + - + + - + + + + + - + - - + - - + - - - - - - - - - - + - + + - + + + + + - + - - + - - + - - - - - - - - - - + - + + - + + + + + - + - - + - - + - - - - - - - - - - + - + + - + + + + + - + - - + - - + - - - - - - - - - - + - + + - - - + - - + true cmd_vel odom @@ -295,8 +251,7 @@ 40.0 - - + true 30.0 base_footprint @@ -307,121 +262,107 @@ + + + + + - - + - - - - - - - + - + - + + + + + - - + - - - - - - - + - + + + + + - - + - - - - - - - + - - - - + + + + + - + - - - - - - - - + true false @@ -451,15 +392,11 @@ + - - - - - - + true 20.0 @@ -502,46 +439,51 @@ - + + - + + + + + - + - + - + - - - - - - + + + + + + - + @@ -549,57 +491,48 @@ - - - - - - + - - - + + + + + - + - + - + - - - - - + - - - + transmission_interface/SimpleTransmission EffortJointInterface - 1 + 1.0 @@ -607,7 +540,7 @@ EffortJointInterface - 1 + 1.0 @@ -615,7 +548,7 @@ EffortJointInterface - 1 + 1.0 @@ -623,7 +556,7 @@ EffortJointInterface - 1 + 1.0 @@ -631,7 +564,7 @@ EffortJointInterface - 1 + 1.0 @@ -639,7 +572,7 @@ EffortJointInterface - 1 + 1.0 @@ -647,14 +580,12 @@ EffortJointInterface - 1 + 1.0 - - + gazebo_ros_control/DefaultRobotHWSim - true katana @@ -663,7 +594,6 @@ 1000.0 0.5 - 20 @@ -675,417 +605,372 @@ katana_gripper_link - - + - - + true - - + true - - + true - - + true - - + true - - + true - - + true - - + true - - -3.022 + -3.022 - - 2.163 + 2.163 - - -2.207 + -2.207 - - -2.026 + -2.026 - - -2.990 + -2.990 - - 0.3 + 0.3 - - 0.3 + 0.3 + + + + + - + - + - + - - - - - - + + - - + + + + + - + - + - - - - - - + + - - + + + + + - + - - + - - - - - - + + - - - + + + + + - + - - + - - - - - - + + - - + + + + + - + - - + - - - - - - + + - - - + + + + + + - + - - + - - - - - + - + + + + + - + - + - - - - - + + - - - + + + + + + - + - + - - - - - + + - - - + + + + + + - + - + - - - - - + - - - - + + - + - - - - + + + + + - + - + - + - - - - - + + - - + + + + + - + - + - + - - - - - - + - - + + - + - - + + - - + true 1.0 @@ -1121,31 +1006,30 @@ - + - - + + - + - - + + - - + true 1.0 @@ -1181,16 +1065,16 @@ + - transmission_interface/SimpleTransmission EffortJointInterface - 1 + 1.0 @@ -1198,12 +1082,10 @@ EffortJointInterface - 1 + 1.0 - - - + true 100.0 imu @@ -1217,23 +1099,28 @@ + - - + + - + + + + + - + @@ -1241,14 +1128,8 @@ - - - - - - - + true 15.0 @@ -1279,7 +1160,7 @@ - + urdfdom-0.4.1/urdf_parser_py/test/calvin/test.sh000077500000000000000000000003441265476635200217720ustar00rootroot00000000000000#!/bin/bash dir=$(dirname $BASH_SOURCE) scripts="$dir/../../scripts" name=calvin orig="$dir/$name.urdf" gen="/tmp/$name.urdf" patch="/tmp/$name.patch" $scripts/display_urdf "$orig" -o "$gen" diff -u "$orig" "$gen" > "$patch" urdfdom-0.4.1/urdf_parser_py/test/romeo/000077500000000000000000000000001265476635200203205ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser_py/test/romeo/gen.patch000066400000000000000000002522501265476635200221200ustar00rootroot00000000000000diff --git a/romeo_description/urdf/romeo.urdf b/tmp/romeo.urdf index c78c618..0138356 100644 --- a/romeo_description/urdf/romeo.urdf +++ b/tmp/romeo.urdf @@ -1,1585 +1,1450 @@ - - + - - + - + - + - + - + - + - - + - + - + - + - + - + - - + - + - - + - + - + - + - + - + - - + - + - - + - + - + - + - + - + - - + - + - - + - + - + - + - + - + - - + - + - - + - + - + - + - + - + - - + - + - + - + - + - + - - + - + - + - + - + - + - - + - + - + - + - + - + - - + - + - - + - + - + - + - + - + - - + - + - - + - + - + - + - + - + - - + - + - + - + - + - + - - + - + - + - + - + - + - - + - + - + - + - + - + - - + - + - - + - + - - + - + - + - + - + - + - - + - + - + - + - + - + - - + - + - - + - + - + - + - + - + - - + - + - + - + - + - + - - + - + - - + - + - - + - + - + - + - + - + - - + - + - + - + - + - + - - + - + - - + - + - + - + - + - + - - + - + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - + + + - + - + - + - + - - - - - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - + - - - + + - - + + - - - + + - - + + - - - + + - - + + - - - + + - - + + - - - + + - - + + - - - + + - - - + + - - + + - - - + + - - + + - - - + + - - - + + - - + + - - - + + - - + + - - + - - - + + - - + + - - - + + - - + + - - - + + - - + + - - - + + - - + + - - - + + - - + + - - - + + - - - + + - - + + - - - + + - - + + - - - + + - - - + + - - + + - - - + + - - + + - + - + - - - + - - - - - + + - + - - - + + - + - - - + + - + - - - + + - + - - + - - + - urdfdom-0.4.1/urdf_parser_py/test/romeo/romeo.urdf000066400000000000000000001436751265476635200223430ustar00rootroot00000000000000 urdfdom-0.4.1/urdf_parser_py/test/romeo/test.sh000077500000000000000000000004051265476635200216350ustar00rootroot00000000000000#!/bin/bash dir=$(dirname $BASH_SOURCE) scripts="$dir/../../scripts" name=romeo orig="$(rospack find romeo_description)/urdf/$name.urdf" gen="/tmp/$name.urdf" patch="/tmp/$name.patch" $scripts/display_urdf "$orig" -o "$gen" diff -u "$orig" "$gen" > "$patch" urdfdom-0.4.1/urdf_parser_py/test/test_urdf.py000066400000000000000000000101161265476635200215470ustar00rootroot00000000000000from __future__ import print_function import unittest import mock from xml.dom import minidom from xml_matching import xml_matches from urdf_parser_py import urdf class ParseException(Exception): pass class TestURDFParser(unittest.TestCase): @mock.patch('urdf_parser_py.xml_reflection.on_error', mock.Mock(side_effect=ParseException)) def parse(self, xml): return urdf.Robot.from_xml_string(xml) def parse_and_compare(self, orig): xml = minidom.parseString(orig) robot = urdf.Robot.from_xml_string(orig) rewritten = minidom.parseString(robot.to_xml_string()) self.assertTrue(xml_matches(xml, rewritten)) def test_new_transmission(self): xml = ''' transmission_interface/SimpleTransmission EffortJointInterface 50.0 ''' self.parse_and_compare(xml) def test_new_transmission_multiple_joints(self): xml = ''' transmission_interface/SimpleTransmission EffortJointInterface EffortJointInterface EffortJointInterface 50.0 ''' self.parse_and_compare(xml) def test_new_transmission_multiple_actuators(self): xml = ''' transmission_interface/SimpleTransmission EffortJointInterface 50.0 ''' self.parse_and_compare(xml) def test_new_transmission_missing_joint(self): xml = ''' transmission_interface/SimpleTransmission ''' self.assertRaises(Exception, self.parse, xml) def test_new_transmission_missing_actuator(self): xml = ''' transmission_interface/SimpleTransmission EffortJointInterface ''' self.assertRaises(Exception, self.parse, xml) def test_old_transmission(self): xml = ''' 1.0 ''' self.parse_and_compare(xml) def test_link_material_missing_color_and_texture(self): xml = ''' ''' self.parse_and_compare(xml) def test_robot_material(self): xml = ''' ''' self.parse_and_compare(xml) def test_robot_material_missing_color_and_texture(self): xml = ''' ''' self.assertRaises(ParseException, self.parse, xml) if __name__ == '__main__': unittest.main() urdfdom-0.4.1/urdf_parser_py/test/transmission/000077500000000000000000000000001265476635200217305ustar00rootroot00000000000000urdfdom-0.4.1/urdf_parser_py/test/transmission/gen.patch000066400000000000000000000014211265476635200235200ustar00rootroot00000000000000--- test.urdf 2014-01-18 20:01:44.102247274 -0600 +++ /tmp/test.urdf 2014-01-18 20:01:50.822247507 -0600 @@ -1,8 +1,5 @@ - - - transmission_interface/SimpleTransmission @@ -11,12 +8,9 @@ 50.0 - - 1.0 - urdfdom-0.4.1/urdf_parser_py/test/transmission/test.py000077500000000000000000000006701265476635200232670ustar00rootroot00000000000000#!/usr/bin/python import os from urdf_parser_py import urdf import subprocess as sp orig = 'test.urdf' gen = '/tmp/test.urdf' patch = '/tmp/test.patch' os.chdir(os.path.dirname(__file__)) robot = urdf.Robot.from_xml_file('test.urdf') for transmission in robot.transmissions: print type(transmission) with open(gen, 'w') as f: f.write(robot.to_xml_string()) sp.call('diff -u "{}" "{}" > "{}"'.format(orig, gen, patch), shell=True) urdfdom-0.4.1/urdf_parser_py/test/transmission/test.urdf000066400000000000000000000013241265476635200235710ustar00rootroot00000000000000 transmission_interface/SimpleTransmission EffortJointInterface 50.0 1.0 urdfdom-0.4.1/urdf_parser_py/test/xml_matching.py000066400000000000000000000070511265476635200222260ustar00rootroot00000000000000import xml.dom import re import sys # regex to match whitespace whitespace = re.compile(r'\s+') def all_attributes_match(a, b): if len(a.attributes) != len(b.attributes): print("Different number of attributes") return False a_atts = [(a.attributes.item(i).name, a.attributes.item(i).value) for i in range(len(a.attributes))] b_atts = [(b.attributes.item(i).name, b.attributes.item(i).value) for i in range(len(b.attributes))] a_atts.sort() b_atts.sort() for i in range(len(a_atts)): if a_atts[i][0] != b_atts[i][0]: print("Different attribute names: %s and %s" % (a_atts[i][0], b_atts[i][0])) return False try: if abs(float(a_atts[i][1]) - float(b_atts[i][1])) > 1.0e-9: print("Different attribute values: %s and %s" % (a_atts[i][1], b_atts[i][1])) return False except ValueError: # Attribute values aren't numeric if a_atts[i][1] != b_atts[i][1]: print("Different attribute values: %s and %s" % (a_atts[i][1], b_atts[i][1])) return False return True def text_matches(a, b): a_norm = whitespace.sub(' ', a) b_norm = whitespace.sub(' ', b) if a_norm.strip() == b_norm.strip(): return True print("Different text values: '%s' and '%s'" % (a, b)) return False def nodes_match(a, b, ignore_nodes): if not a and not b: return True if not a or not b: return False if a.nodeType != b.nodeType: print("Different node types: %s and %s" % (a, b)) return False # compare text-valued nodes if a.nodeType in [xml.dom.Node.TEXT_NODE, xml.dom.Node.CDATA_SECTION_NODE, xml.dom.Node.COMMENT_NODE]: return text_matches(a.data, b.data) # ignore all other nodes except ELEMENTs if a.nodeType != xml.dom.Node.ELEMENT_NODE: return True # compare ELEMENT nodes if a.nodeName != b.nodeName: print("Different element names: %s and %s" % (a.nodeName, b.nodeName)) return False if not all_attributes_match(a, b): return False a = a.firstChild b = b.firstChild while a or b: # ignore whitespace-only text nodes # we could have several text nodes in a row, due to replacements while (a and ((a.nodeType in ignore_nodes) or (a.nodeType == xml.dom.Node.TEXT_NODE and whitespace.sub('', a.data) == ""))): a = a.nextSibling while (b and ((b.nodeType in ignore_nodes) or (b.nodeType == xml.dom.Node.TEXT_NODE and whitespace.sub('', b.data) == ""))): b = b.nextSibling if not nodes_match(a, b, ignore_nodes): return False if a: a = a.nextSibling if b: b = b.nextSibling return True def xml_matches(a, b, ignore_nodes=[]): if isinstance(a, str): return xml_matches(xml.dom.minidom.parseString(a).documentElement, b, ignore_nodes) if isinstance(b, str): return xml_matches(a, xml.dom.minidom.parseString(b).documentElement, ignore_nodes) if a.nodeType == xml.dom.Node.DOCUMENT_NODE: return xml_matches(a.documentElement, b, ignore_nodes) if b.nodeType == xml.dom.Node.DOCUMENT_NODE: return xml_matches(a, b.documentElement, ignore_nodes) if not nodes_match(a, b, ignore_nodes): print("Match failed:") a.writexml(sys.stdout) print() print('=' * 78) b.writexml(sys.stdout) print() return False return True urdfdom-0.4.1/xsd/000077500000000000000000000000001265476635200137725ustar00rootroot00000000000000urdfdom-0.4.1/xsd/autogenerated.urdf000066400000000000000000001202111265476635200175000ustar00rootroot00000000000000 1 -1.7976931348623157E+308 1.7976931348623157E+308 -0.10000002384185791 2.1000000238418579 -1.7976931348623157E+308 1.7976931348623157E+308 -1.2000000476837158 3.2000000476837158 -1.7976931348623157E+308 1.7976931348623157E+308 -2.3000000715255737 4.3000000715255737 -1.7976931348623157E+308 1.7976931348623157E+308 -3.4000000953674316 5.4000000953674316 -1.7976931348623157E+308 1.7976931348623157E+308 -4.5000001192092896 6.5000001192092896 -1.7976931348623157E+308 1.7976931348623157E+308 -5.6000001430511475 7.6000001430511475 -1.7976931348623157E+308 1.7976931348623157E+308 urdfdom-0.4.1/xsd/urdf.xsd000066400000000000000000000267471265476635200154720ustar00rootroot00000000000000