├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── pkgconfig │ └── urdfdom_headers.pc.in ├── uninstall.cmake.in └── urdfdom_headers-config.cmake.in └── include ├── urdf_exception └── exception.h ├── urdf_model ├── color.h ├── joint.h ├── link.h ├── model.h ├── pose.h ├── twist.h ├── types.h └── utils.h ├── urdf_model_state ├── model_state.h ├── twist.h └── types.h ├── urdf_sensor ├── sensor.h └── types.h └── urdf_world ├── types.h └── world.h /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required( VERSION 3.8 FATAL_ERROR ) 2 | project (urdfdom_headers) 3 | 4 | include(GNUInstallDirs) 5 | 6 | option(APPEND_PROJECT_NAME_TO_INCLUDEDIR 7 | "When ON headers are installed to a folder ending with an extra ${PROJECT_NAME}. \ 8 | This avoids include directory search order issues when overriding this package 9 | from a merged catkin, ament, or colcon workspace." ON) 10 | 11 | if(APPEND_PROJECT_NAME_TO_INCLUDEDIR) 12 | set(CMAKE_INSTALL_INCLUDEDIR "${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME}") 13 | endif() 14 | 15 | set (URDF_MAJOR_VERSION 1) 16 | set (URDF_MINOR_VERSION 1) 17 | set (URDF_PATCH_VERSION 2) 18 | 19 | set (URDF_VERSION ${URDF_MAJOR_VERSION}.${URDF_MINOR_VERSION}.${URDF_PATCH_VERSION}) 20 | 21 | message (STATUS "${PROJECT_NAME} version ${URDF_VERSION}") 22 | 23 | # This shouldn't be necessary, but there has been trouble 24 | # with MSVC being set off, but MSVCXX ON. 25 | if(MSVC OR MSVC90 OR MSVC10) 26 | set(MSVC ON) 27 | endif (MSVC OR MSVC90 OR MSVC10) 28 | 29 | install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 30 | 31 | if(WIN32 AND NOT CYGWIN) 32 | set(CMAKE_CONFIG_INSTALL_DIR CMake) 33 | else() 34 | set(CMAKE_CONFIG_INSTALL_DIR ${CMAKE_INSTALL_LIBDIR}/${PROJECT_NAME}/cmake) 35 | endif() 36 | string(REGEX REPLACE "[^/]+" ".." RELATIVE_PATH_CMAKE_DIR_TO_PREFIX "${CMAKE_CONFIG_INSTALL_DIR}") 37 | string(REGEX REPLACE "[^/]+" ".." RELATIVE_PATH_LIBDIR_TO_PREFIX "${CMAKE_INSTALL_LIBDIR}") 38 | 39 | set(PACKAGE_NAME ${PROJECT_NAME}) 40 | set(cmake_conf_file "${PROJECT_NAME}-config.cmake") 41 | configure_file("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${cmake_conf_file}.in" "${CMAKE_BINARY_DIR}/${cmake_conf_file}" @ONLY) 42 | set(cmake_conf_version_file "${PROJECT_NAME}-config-version.cmake") 43 | # Use write_basic_package_version_file to generate a ConfigVersion file that 44 | # allow users of gazebo to specify the API or version to depend on 45 | # TODO: keep this instruction until deprecate Ubuntu/Precise and update with 46 | # https://github.com/Kitware/CMake/blob/v2.8.8/Modules/CMakePackageConfigHelpers.cmake 47 | include(WriteBasicConfigVersionFile) 48 | write_basic_config_version_file( 49 | ${CMAKE_CURRENT_BINARY_DIR}/${cmake_conf_version_file} 50 | VERSION "${URDF_VERSION}" 51 | COMPATIBILITY SameMajorVersion) 52 | install(FILES 53 | "${CMAKE_BINARY_DIR}/${cmake_conf_file}" 54 | "${CMAKE_BINARY_DIR}/${cmake_conf_version_file}" 55 | DESTINATION ${CMAKE_CONFIG_INSTALL_DIR} COMPONENT cmake) 56 | 57 | # Make the package config file 58 | set(PACKAGE_DESC "Unified Robot Description Format") 59 | set(pkg_conf_file "urdfdom_headers.pc") 60 | configure_file("${CMAKE_CURRENT_SOURCE_DIR}/cmake/pkgconfig/${pkg_conf_file}.in" "${CMAKE_BINARY_DIR}/${pkg_conf_file}" @ONLY) 61 | install(FILES "${CMAKE_BINARY_DIR}/${pkg_conf_file}" DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig/ COMPONENT pkgconfig) 62 | 63 | add_library(${PROJECT_NAME} INTERFACE) 64 | target_include_directories(${PROJECT_NAME} INTERFACE 65 | "$") 66 | install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}) 67 | install( 68 | EXPORT ${PROJECT_NAME} 69 | DESTINATION ${CMAKE_CONFIG_INSTALL_DIR} 70 | NAMESPACE "${PROJECT_NAME}::" 71 | FILE "${PROJECT_NAME}Export.cmake" 72 | ) 73 | 74 | # Add uninstall target 75 | # Ref: http://www.cmake.org/Wiki/CMake_FAQ#Can_I_do_.22make_uninstall.22_with_CMake.3F 76 | configure_file("${PROJECT_SOURCE_DIR}/cmake/uninstall.cmake.in" "${PROJECT_BINARY_DIR}/uninstall.cmake" IMMEDIATE @ONLY) 77 | add_custom_target(uninstall "${CMAKE_COMMAND}" -P "${PROJECT_BINARY_DIR}/uninstall.cmake") 78 | 79 | message(STATUS "Configuration successful. Type make install to install urdfdom_headers.") 80 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | urdfdom_headers 2 | =========== 3 | 4 | The URDF (U-Robot Description Format) headers provides core data structure headers for URDF. 5 | 6 | For now, the details of the URDF specifications reside on http://ros.org/wiki/urdf 7 | 8 | ### Build Status 9 | [![Build Status](https://travis-ci.org/ros/urdfdom_headers.png)](https://travis-ci.org/ros/urdfdom_headers) 10 | 11 | ### Using with ROS 12 | 13 | If you choose to check this repository out for use with ROS, be aware that the necessary ``package.xml`` is not 14 | included in this repo but instead is added in during the ROS release process. To emulate this, pull the appropriate 15 | file into this repository using the following format. Be sure to replace the ALLCAPS words with the appropriate terms: 16 | 17 | ``` 18 | wget https://raw.github.com/ros-gbp/urdfdom_headers-release/debian/ROS_DISTRO/UBUNTU_DISTRO/urdfdom_headers/package.xml 19 | ``` 20 | 21 | For example: 22 | ``` 23 | wget https://raw.github.com/ros-gbp/urdfdom_headers-release/debian/hydro/precise/urdfdom_headers/package.xml 24 | ``` 25 | 26 | -------------------------------------------------------------------------------- /cmake/pkgconfig/urdfdom_headers.pc.in: -------------------------------------------------------------------------------- 1 | # This file was generated by CMake for @PROJECT_NAME@ 2 | prefix=${pcfiledir}/../@RELATIVE_PATH_LIBDIR_TO_PREFIX@ 3 | exec_prefix=${prefix} 4 | includedir=${prefix}/@CMAKE_INSTALL_INCLUDEDIR@ 5 | 6 | Name: @PACKAGE_NAME@ 7 | Description: @PACKAGE_DESC@ 8 | Version: @URDF_VERSION@ 9 | Requires: 10 | Cflags: -I${includedir} 11 | -------------------------------------------------------------------------------- /cmake/uninstall.cmake.in: -------------------------------------------------------------------------------- 1 | # Ref: http://www.cmake.org/Wiki/CMake_FAQ#Can_I_do_.22make_uninstall.22_with_CMake.3F 2 | if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 3 | message(FATAL_ERROR "Cannot find install manifest: @CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 4 | endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 5 | 6 | file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) 7 | string(REGEX REPLACE "\n" ";" files "${files}") 8 | foreach(file ${files}) 9 | message(STATUS "Uninstalling $ENV{DESTDIR}${file}") 10 | if(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") 11 | exec_program("@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\"" 12 | OUTPUT_VARIABLE rm_out 13 | RETURN_VALUE rm_retval) 14 | if(NOT "${rm_retval}" STREQUAL 0) 15 | message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}") 16 | endif(NOT "${rm_retval}" STREQUAL 0) 17 | else(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") 18 | message(STATUS "File $ENV{DESTDIR}${file} does not exist.") 19 | endif(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") 20 | endforeach(file) 21 | -------------------------------------------------------------------------------- /cmake/urdfdom_headers-config.cmake.in: -------------------------------------------------------------------------------- 1 | if (@PACKAGE_NAME@_CONFIG_INCLUDED) 2 | return() 3 | endif() 4 | set(@PACKAGE_NAME@_CONFIG_INCLUDED TRUE) 5 | 6 | set(@PACKAGE_NAME@_INCLUDE_DIRS "${@PROJECT_NAME@_DIR}/@RELATIVE_PATH_CMAKE_DIR_TO_PREFIX@/@CMAKE_INSTALL_INCLUDEDIR@") 7 | 8 | include("${@PACKAGE_NAME@_DIR}/@PACKAGE_NAME@Export.cmake") 9 | 10 | list(APPEND @PACKAGE_NAME@_TARGETS @PACKAGE_NAME@::@PACKAGE_NAME@) 11 | -------------------------------------------------------------------------------- /include/urdf_exception/exception.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | // URDF exceptions 36 | #ifndef URDF_INTERFACE_EXCEPTION_H_ 37 | #define URDF_INTERFACE_EXCEPTION_H_ 38 | 39 | #include 40 | #include 41 | 42 | namespace urdf 43 | { 44 | 45 | class ParseError: public std::runtime_error 46 | { 47 | public: 48 | ParseError(const std::string &error_msg) : std::runtime_error(error_msg) {}; 49 | }; 50 | 51 | } 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /include/urdf_model/color.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Josh Faust */ 36 | 37 | #ifndef URDF_INTERFACE_COLOR_H 38 | #define URDF_INTERFACE_COLOR_H 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | 48 | namespace urdf 49 | { 50 | 51 | class Color 52 | { 53 | public: 54 | Color() {this->clear();}; 55 | float r; 56 | float g; 57 | float b; 58 | float a; 59 | 60 | void clear() 61 | { 62 | r = g = b = 0.0f; 63 | a = 1.0f; 64 | } 65 | bool init(const std::string &vector_str) 66 | { 67 | this->clear(); 68 | std::vector pieces; 69 | std::vector rgba; 70 | urdf::split_string( pieces, vector_str, " "); 71 | for (unsigned int i = 0; i < pieces.size(); ++i) 72 | { 73 | if (!pieces[i].empty()) 74 | { 75 | try 76 | { 77 | double piece = strToDouble(pieces[i].c_str()); 78 | if ((piece < 0) || (piece > 1)) 79 | throw ParseError("Component [" + pieces[i] + "] is outside the valid range for colors [0, 1]"); 80 | rgba.push_back(static_cast(piece)); 81 | } 82 | catch (std::runtime_error &/*e*/) { 83 | throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a color value)"); 84 | } 85 | } 86 | } 87 | 88 | if (rgba.size() != 4) 89 | { 90 | return false; 91 | } 92 | 93 | this->r = rgba[0]; 94 | this->g = rgba[1]; 95 | this->b = rgba[2]; 96 | this->a = rgba[3]; 97 | 98 | return true; 99 | }; 100 | }; 101 | 102 | } 103 | 104 | #endif 105 | 106 | -------------------------------------------------------------------------------- /include/urdf_model/joint.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef URDF_INTERFACE_JOINT_H 38 | #define URDF_INTERFACE_JOINT_H 39 | 40 | #include 41 | #include 42 | 43 | #include "urdf_model/pose.h" 44 | #include "urdf_model/types.h" 45 | 46 | 47 | namespace urdf{ 48 | 49 | class Link; 50 | 51 | class JointDynamics 52 | { 53 | public: 54 | JointDynamics() { this->clear(); }; 55 | double damping; 56 | double friction; 57 | 58 | void clear() 59 | { 60 | damping = 0; 61 | friction = 0; 62 | }; 63 | }; 64 | 65 | class JointLimits 66 | { 67 | public: 68 | JointLimits() { this->clear(); }; 69 | double lower; 70 | double upper; 71 | double effort; 72 | double velocity; 73 | 74 | void clear() 75 | { 76 | lower = 0; 77 | upper = 0; 78 | effort = 0; 79 | velocity = 0; 80 | }; 81 | }; 82 | 83 | /// \brief Parameters for Joint Safety Controllers 84 | class JointSafety 85 | { 86 | public: 87 | /// clear variables on construction 88 | JointSafety() { this->clear(); }; 89 | 90 | /// 91 | /// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage. 92 | /// 93 | /// Basic safety controller operation is as follows 94 | /// 95 | /// current safety controllers will take effect on joints outside the position range below: 96 | /// 97 | /// position range: [JointSafety::soft_lower_limit + JointLimits::velocity / JointSafety::k_position, 98 | /// JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position] 99 | /// 100 | /// if (joint_position is outside of the position range above) 101 | /// velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit) 102 | /// velocity_limit_max = JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit) 103 | /// else 104 | /// velocity_limit_min = -JointLimits::velocity 105 | /// velocity_limit_max = JointLimits::velocity 106 | /// 107 | /// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity, 108 | /// velocity_limit_max - JointLimits::effort / JointSafety::k_velocity] 109 | /// 110 | /// if (joint_velocity is outside of the velocity range above) 111 | /// effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min) 112 | /// effort_limit_max = JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max) 113 | /// else 114 | /// effort_limit_min = -JointLimits::effort 115 | /// effort_limit_max = JointLimits::effort 116 | /// 117 | /// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max] 118 | /// 119 | /// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits 120 | /// 121 | double soft_upper_limit; 122 | double soft_lower_limit; 123 | double k_position; 124 | double k_velocity; 125 | 126 | void clear() 127 | { 128 | soft_upper_limit = 0; 129 | soft_lower_limit = 0; 130 | k_position = 0; 131 | k_velocity = 0; 132 | }; 133 | }; 134 | 135 | 136 | class JointCalibration 137 | { 138 | public: 139 | JointCalibration() { this->clear(); }; 140 | double reference_position; 141 | DoubleSharedPtr rising, falling; 142 | 143 | void clear() 144 | { 145 | reference_position = 0; 146 | }; 147 | }; 148 | 149 | class JointMimic 150 | { 151 | public: 152 | JointMimic() { this->clear(); }; 153 | double offset; 154 | double multiplier; 155 | std::string joint_name; 156 | 157 | void clear() 158 | { 159 | offset = 0.0; 160 | multiplier = 0.0; 161 | joint_name.clear(); 162 | }; 163 | }; 164 | 165 | 166 | class Joint 167 | { 168 | public: 169 | 170 | Joint() { this->clear(); }; 171 | 172 | std::string name; 173 | enum 174 | { 175 | UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED 176 | } type; 177 | 178 | /// \brief type_ meaning of axis_ 179 | /// ------------------------------------------------------ 180 | /// UNKNOWN unknown type 181 | /// REVOLUTE rotation axis 182 | /// PRISMATIC translation axis 183 | /// FLOATING N/A 184 | /// PLANAR plane normal axis 185 | /// FIXED N/A 186 | Vector3 axis; 187 | 188 | /// child Link element 189 | /// child link frame is the same as the Joint frame 190 | std::string child_link_name; 191 | 192 | /// parent Link element 193 | /// origin specifies the transform from Parent Link to Joint Frame 194 | std::string parent_link_name; 195 | /// transform from Parent Link frame to Joint frame 196 | Pose parent_to_joint_origin_transform; 197 | 198 | /// Joint Dynamics 199 | JointDynamicsSharedPtr dynamics; 200 | 201 | /// Joint Limits 202 | JointLimitsSharedPtr limits; 203 | 204 | /// Unsupported Hidden Feature 205 | JointSafetySharedPtr safety; 206 | 207 | /// Unsupported Hidden Feature 208 | JointCalibrationSharedPtr calibration; 209 | 210 | /// Option to Mimic another Joint 211 | JointMimicSharedPtr mimic; 212 | 213 | void clear() 214 | { 215 | this->axis.clear(); 216 | this->child_link_name.clear(); 217 | this->parent_link_name.clear(); 218 | this->parent_to_joint_origin_transform.clear(); 219 | this->dynamics.reset(); 220 | this->limits.reset(); 221 | this->safety.reset(); 222 | this->calibration.reset(); 223 | this->mimic.reset(); 224 | this->type = UNKNOWN; 225 | }; 226 | }; 227 | 228 | } 229 | 230 | #endif 231 | -------------------------------------------------------------------------------- /include/urdf_model/link.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef URDF_INTERFACE_LINK_H 38 | #define URDF_INTERFACE_LINK_H 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | #include "joint.h" 45 | #include "color.h" 46 | #include "types.h" 47 | 48 | namespace urdf{ 49 | 50 | class Geometry 51 | { 52 | public: 53 | enum {SPHERE, BOX, CYLINDER, MESH} type; 54 | 55 | virtual ~Geometry(void) 56 | { 57 | } 58 | }; 59 | 60 | class Sphere : public Geometry 61 | { 62 | public: 63 | Sphere() { this->clear(); type = SPHERE; }; 64 | double radius; 65 | 66 | void clear() 67 | { 68 | radius = 0; 69 | }; 70 | }; 71 | 72 | class Box : public Geometry 73 | { 74 | public: 75 | Box() { this->clear(); type = BOX; }; 76 | Vector3 dim; 77 | 78 | void clear() 79 | { 80 | this->dim.clear(); 81 | }; 82 | }; 83 | 84 | class Cylinder : public Geometry 85 | { 86 | public: 87 | Cylinder() { this->clear(); type = CYLINDER; }; 88 | double length; 89 | double radius; 90 | 91 | void clear() 92 | { 93 | length = 0; 94 | radius = 0; 95 | }; 96 | }; 97 | 98 | class Mesh : public Geometry 99 | { 100 | public: 101 | Mesh() { this->clear(); type = MESH; }; 102 | std::string filename; 103 | Vector3 scale; 104 | 105 | void clear() 106 | { 107 | filename.clear(); 108 | // default scale 109 | scale.x = 1; 110 | scale.y = 1; 111 | scale.z = 1; 112 | }; 113 | }; 114 | 115 | class Material 116 | { 117 | public: 118 | Material() { this->clear(); }; 119 | std::string name; 120 | std::string texture_filename; 121 | Color color; 122 | 123 | void clear() 124 | { 125 | color.clear(); 126 | texture_filename.clear(); 127 | name.clear(); 128 | }; 129 | }; 130 | 131 | class Inertial 132 | { 133 | public: 134 | Inertial() { this->clear(); }; 135 | Pose origin; 136 | double mass; 137 | double ixx,ixy,ixz,iyy,iyz,izz; 138 | 139 | void clear() 140 | { 141 | origin.clear(); 142 | mass = 0; 143 | ixx = ixy = ixz = iyy = iyz = izz = 0; 144 | }; 145 | }; 146 | 147 | class Visual 148 | { 149 | public: 150 | Visual() { this->clear(); }; 151 | Pose origin; 152 | GeometrySharedPtr geometry; 153 | 154 | std::string material_name; 155 | MaterialSharedPtr material; 156 | 157 | void clear() 158 | { 159 | origin.clear(); 160 | material_name.clear(); 161 | material.reset(); 162 | geometry.reset(); 163 | name.clear(); 164 | }; 165 | 166 | std::string name; 167 | }; 168 | 169 | class Collision 170 | { 171 | public: 172 | Collision() { this->clear(); }; 173 | Pose origin; 174 | GeometrySharedPtr geometry; 175 | 176 | void clear() 177 | { 178 | origin.clear(); 179 | geometry.reset(); 180 | name.clear(); 181 | }; 182 | 183 | std::string name; 184 | 185 | }; 186 | 187 | 188 | class Link 189 | { 190 | public: 191 | Link() { this->clear(); }; 192 | 193 | std::string name; 194 | 195 | /// inertial element 196 | InertialSharedPtr inertial; 197 | 198 | /// visual element 199 | VisualSharedPtr visual; 200 | 201 | /// collision element 202 | CollisionSharedPtr collision; 203 | 204 | /// if more than one collision element is specified, all collision elements are placed in this array (the collision member points to the first element of the array) 205 | std::vector collision_array; 206 | 207 | /// if more than one visual element is specified, all visual elements are placed in this array (the visual member points to the first element of the array) 208 | std::vector visual_array; 209 | 210 | /// Parent Joint element 211 | /// explicitly stating "parent" because we want directional-ness for tree structure 212 | /// every link can have one parent 213 | JointSharedPtr parent_joint; 214 | 215 | std::vector child_joints; 216 | std::vector child_links; 217 | 218 | LinkSharedPtr getParent() const 219 | {return parent_link_.lock();}; 220 | 221 | void setParent(const LinkSharedPtr &parent) 222 | { parent_link_ = parent; } 223 | 224 | void clear() 225 | { 226 | this->name.clear(); 227 | this->inertial.reset(); 228 | this->visual.reset(); 229 | this->collision.reset(); 230 | this->parent_joint.reset(); 231 | this->child_joints.clear(); 232 | this->child_links.clear(); 233 | this->collision_array.clear(); 234 | this->visual_array.clear(); 235 | }; 236 | 237 | private: 238 | LinkWeakPtr parent_link_; 239 | 240 | }; 241 | 242 | 243 | 244 | 245 | } 246 | 247 | #endif 248 | -------------------------------------------------------------------------------- /include/urdf_model/model.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef URDF_INTERFACE_MODEL_H 38 | #define URDF_INTERFACE_MODEL_H 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace urdf { 47 | 48 | class ModelInterface 49 | { 50 | public: 51 | LinkConstSharedPtr getRoot(void) const{return this->root_link_;}; 52 | LinkConstSharedPtr getLink(const std::string& name) const 53 | { 54 | LinkConstSharedPtr ptr; 55 | if (this->links_.find(name) == this->links_.end()) 56 | ptr.reset(); 57 | else 58 | ptr = this->links_.find(name)->second; 59 | return ptr; 60 | }; 61 | 62 | JointConstSharedPtr getJoint(const std::string& name) const 63 | { 64 | JointConstSharedPtr ptr; 65 | if (this->joints_.find(name) == this->joints_.end()) 66 | ptr.reset(); 67 | else 68 | ptr = this->joints_.find(name)->second; 69 | return ptr; 70 | }; 71 | 72 | 73 | const std::string& getName() const {return name_;}; 74 | void getLinks(std::vector& links) const 75 | { 76 | for (std::map::const_iterator link = this->links_.begin();link != this->links_.end(); link++) 77 | { 78 | links.push_back(link->second); 79 | } 80 | }; 81 | 82 | void clear() 83 | { 84 | name_.clear(); 85 | this->links_.clear(); 86 | this->joints_.clear(); 87 | this->materials_.clear(); 88 | this->root_link_.reset(); 89 | }; 90 | 91 | /// non-const getLink() 92 | void getLink(const std::string& name, LinkSharedPtr &link) const 93 | { 94 | LinkSharedPtr ptr; 95 | if (this->links_.find(name) == this->links_.end()) 96 | ptr.reset(); 97 | else 98 | ptr = this->links_.find(name)->second; 99 | link = ptr; 100 | }; 101 | 102 | /// non-const getMaterial() 103 | MaterialSharedPtr getMaterial(const std::string& name) const 104 | { 105 | MaterialSharedPtr ptr; 106 | if (this->materials_.find(name) == this->materials_.end()) 107 | ptr.reset(); 108 | else 109 | ptr = this->materials_.find(name)->second; 110 | return ptr; 111 | }; 112 | 113 | void initTree(std::map &parent_link_tree) 114 | { 115 | // loop through all joints, for every link, assign children links and children joints 116 | for (std::map::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) 117 | { 118 | std::string parent_link_name = joint->second->parent_link_name; 119 | std::string child_link_name = joint->second->child_link_name; 120 | 121 | if (parent_link_name.empty() || child_link_name.empty()) 122 | { 123 | throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification."); 124 | } 125 | else 126 | { 127 | // find child and parent links 128 | LinkSharedPtr child_link, parent_link; 129 | this->getLink(child_link_name, child_link); 130 | if (!child_link) 131 | { 132 | throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found"); 133 | } 134 | this->getLink(parent_link_name, parent_link); 135 | if (!parent_link) 136 | { 137 | throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found. This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [" + joint->first + "] from your urdf file, or add \"\" to your urdf file."); 138 | } 139 | 140 | //set parent link for child link 141 | child_link->setParent(parent_link); 142 | 143 | //set parent joint for child link 144 | child_link->parent_joint = joint->second; 145 | 146 | //set child joint for parent link 147 | parent_link->child_joints.push_back(joint->second); 148 | 149 | //set child link for parent link 150 | parent_link->child_links.push_back(child_link); 151 | 152 | // fill in child/parent string map 153 | parent_link_tree[child_link->name] = parent_link_name; 154 | } 155 | } 156 | } 157 | 158 | void initRoot(const std::map &parent_link_tree) 159 | { 160 | this->root_link_.reset(); 161 | 162 | // find the links that have no parent in the tree 163 | for (std::map::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++) 164 | { 165 | std::map::const_iterator parent = parent_link_tree.find(l->first); 166 | if (parent == parent_link_tree.end()) 167 | { 168 | // store root link 169 | if (!this->root_link_) 170 | { 171 | getLink(l->first, this->root_link_); 172 | } 173 | // we already found a root link 174 | else 175 | { 176 | throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]"); 177 | } 178 | } 179 | } 180 | if (!this->root_link_) 181 | { 182 | throw ParseError("No root link found. The robot xml is not a valid tree."); 183 | } 184 | } 185 | 186 | 187 | /// \brief complete list of Links 188 | std::map links_; 189 | /// \brief complete list of Joints 190 | std::map joints_; 191 | /// \brief complete list of Materials 192 | std::map materials_; 193 | 194 | /// \brief The name of the robot model 195 | std::string name_; 196 | 197 | /// \brief The root is always a link (the parent of the tree describing the robot) 198 | LinkSharedPtr root_link_; 199 | 200 | 201 | 202 | }; 203 | 204 | } 205 | 206 | #endif 207 | -------------------------------------------------------------------------------- /include/urdf_model/pose.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #ifndef URDF_INTERFACE_POSE_H 38 | #define URDF_INTERFACE_POSE_H 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | 49 | namespace urdf{ 50 | 51 | class Vector3 52 | { 53 | public: 54 | Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;}; 55 | Vector3() {this->clear();}; 56 | double x; 57 | double y; 58 | double z; 59 | 60 | void clear() {this->x=this->y=this->z=0.0;}; 61 | void init(const std::string &vector_str) 62 | { 63 | this->clear(); 64 | std::vector pieces; 65 | std::vector xyz; 66 | urdf::split_string( pieces, vector_str, " "); 67 | for (unsigned int i = 0; i < pieces.size(); ++i){ 68 | if (pieces[i] != ""){ 69 | try { 70 | xyz.push_back(strToDouble(pieces[i].c_str())); 71 | } catch(std::runtime_error &) { 72 | throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)"); 73 | } 74 | } 75 | } 76 | 77 | if (xyz.size() != 3) 78 | throw ParseError("Parser found " + std::to_string(xyz.size()) + " elements but 3 expected while parsing vector [" + vector_str + "]"); 79 | 80 | this->x = xyz[0]; 81 | this->y = xyz[1]; 82 | this->z = xyz[2]; 83 | } 84 | 85 | Vector3 operator+(Vector3 vec) 86 | { 87 | return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z); 88 | }; 89 | }; 90 | 91 | class Rotation 92 | { 93 | public: 94 | Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;}; 95 | Rotation() {this->clear();}; 96 | void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const 97 | { 98 | quat_x = this->x; 99 | quat_y = this->y; 100 | quat_z = this->z; 101 | quat_w = this->w; 102 | }; 103 | void getRPY(double &roll,double &pitch,double &yaw) const 104 | { 105 | double sqw; 106 | double sqx; 107 | double sqy; 108 | double sqz; 109 | 110 | sqx = this->x * this->x; 111 | sqy = this->y * this->y; 112 | sqz = this->z * this->z; 113 | sqw = this->w * this->w; 114 | 115 | // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ 116 | double sarg = -2 * (this->x*this->z - this->w*this->y); 117 | const double pi_2 = 1.57079632679489661923; 118 | if (sarg <= -0.99999) { 119 | pitch = -pi_2; 120 | roll = 0; 121 | yaw = 2 * atan2(this->x, -this->y); 122 | } else if (sarg >= 0.99999) { 123 | pitch = pi_2; 124 | roll = 0; 125 | yaw = 2 * atan2(-this->x, this->y); 126 | } else { 127 | pitch = asin(sarg); 128 | roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz); 129 | yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz); 130 | } 131 | 132 | }; 133 | void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w) 134 | { 135 | this->x = quat_x; 136 | this->y = quat_y; 137 | this->z = quat_z; 138 | this->w = quat_w; 139 | this->normalize(); 140 | }; 141 | void setFromRPY(double roll, double pitch, double yaw) 142 | { 143 | double phi, the, psi; 144 | 145 | phi = roll / 2.0; 146 | the = pitch / 2.0; 147 | psi = yaw / 2.0; 148 | 149 | this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi); 150 | this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi); 151 | this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi); 152 | this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi); 153 | 154 | this->normalize(); 155 | }; 156 | 157 | double x,y,z,w; 158 | 159 | void init(const std::string &rotation_str) 160 | { 161 | this->clear(); 162 | Vector3 rpy; 163 | rpy.init(rotation_str); 164 | setFromRPY(rpy.x, rpy.y, rpy.z); 165 | } 166 | 167 | void clear() { this->x=this->y=this->z=0.0;this->w=1.0; } 168 | 169 | void normalize() 170 | { 171 | const double squared_norm = 172 | this->x * this->x + 173 | this->y * this->y + 174 | this->z * this->z + 175 | this->w * this->w; 176 | 177 | // Note: This function historically checked if the norm of the elements 178 | // equaled 0 using a strict floating point equals (s == 0.0) to prevent a 179 | // divide by zero error. This comparison will cause the compiler to issue a 180 | // warning with `-Wfloat-equal`. Comparing against a small epsilon would 181 | // likely be more ideal, but its choice may be application specific and 182 | // could change behavior of legacy code if chosen poorly. Using `<=` 183 | // silences the warning without changing the behavior of this function, even 184 | // though there are no situations squared_norm can be strictly less than 0. 185 | if (squared_norm <= 0.0) 186 | { 187 | this->x = 0.0; 188 | this->y = 0.0; 189 | this->z = 0.0; 190 | this->w = 1.0; 191 | } 192 | else 193 | { 194 | const double s = sqrt(squared_norm); 195 | this->x /= s; 196 | this->y /= s; 197 | this->z /= s; 198 | this->w /= s; 199 | } 200 | }; 201 | 202 | // Multiplication operator (copied from gazebo) 203 | Rotation operator*( const Rotation &qt ) const 204 | { 205 | Rotation c; 206 | 207 | c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y; 208 | c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x; 209 | c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w; 210 | c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z; 211 | 212 | return c; 213 | }; 214 | /// Rotate a vector using the quaternion 215 | Vector3 operator*(Vector3 vec) const 216 | { 217 | Rotation tmp; 218 | Vector3 result; 219 | 220 | tmp.w = 0.0; 221 | tmp.x = vec.x; 222 | tmp.y = vec.y; 223 | tmp.z = vec.z; 224 | 225 | tmp = (*this) * (tmp * this->GetInverse()); 226 | 227 | result.x = tmp.x; 228 | result.y = tmp.y; 229 | result.z = tmp.z; 230 | 231 | return result; 232 | }; 233 | // Get the inverse of this quaternion 234 | Rotation GetInverse() const 235 | { 236 | Rotation q; 237 | 238 | double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z; 239 | 240 | if (norm > 0.0) 241 | { 242 | q.w = this->w / norm; 243 | q.x = -this->x / norm; 244 | q.y = -this->y / norm; 245 | q.z = -this->z / norm; 246 | } 247 | 248 | return q; 249 | }; 250 | 251 | 252 | }; 253 | 254 | class Pose 255 | { 256 | public: 257 | Pose() { this->clear(); }; 258 | 259 | Vector3 position; 260 | Rotation rotation; 261 | 262 | void clear() 263 | { 264 | this->position.clear(); 265 | this->rotation.clear(); 266 | }; 267 | }; 268 | 269 | } 270 | 271 | #endif 272 | -------------------------------------------------------------------------------- /include/urdf_model/twist.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: John Hsu */ 36 | 37 | #ifndef URDF_TWIST_H 38 | #define URDF_TWIST_H 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace urdf{ 47 | 48 | 49 | class Twist 50 | { 51 | public: 52 | Twist() { this->clear(); }; 53 | 54 | Vector3 linear; 55 | // Angular velocity represented by Euler angles 56 | Vector3 angular; 57 | 58 | void clear() 59 | { 60 | this->linear.clear(); 61 | this->angular.clear(); 62 | }; 63 | }; 64 | 65 | } 66 | 67 | #endif 68 | 69 | -------------------------------------------------------------------------------- /include/urdf_model/types.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Steve Peters */ 36 | 37 | #ifndef URDF_MODEL_TYPES_H 38 | #define URDF_MODEL_TYPES_H 39 | 40 | #include 41 | 42 | #define URDF_TYPEDEF_CLASS_POINTER(Class) \ 43 | class Class; \ 44 | typedef std::shared_ptr Class##SharedPtr; \ 45 | typedef std::shared_ptr Class##ConstSharedPtr; \ 46 | typedef std::weak_ptr Class##WeakPtr 47 | 48 | namespace urdf{ 49 | 50 | // shared pointer used in joint.h 51 | typedef std::shared_ptr DoubleSharedPtr; 52 | 53 | URDF_TYPEDEF_CLASS_POINTER(Box); 54 | URDF_TYPEDEF_CLASS_POINTER(Collision); 55 | URDF_TYPEDEF_CLASS_POINTER(Cylinder); 56 | URDF_TYPEDEF_CLASS_POINTER(Geometry); 57 | URDF_TYPEDEF_CLASS_POINTER(Inertial); 58 | URDF_TYPEDEF_CLASS_POINTER(Joint); 59 | URDF_TYPEDEF_CLASS_POINTER(JointCalibration); 60 | URDF_TYPEDEF_CLASS_POINTER(JointDynamics); 61 | URDF_TYPEDEF_CLASS_POINTER(JointLimits); 62 | URDF_TYPEDEF_CLASS_POINTER(JointMimic); 63 | URDF_TYPEDEF_CLASS_POINTER(JointSafety); 64 | URDF_TYPEDEF_CLASS_POINTER(Link); 65 | URDF_TYPEDEF_CLASS_POINTER(Material); 66 | URDF_TYPEDEF_CLASS_POINTER(Mesh); 67 | URDF_TYPEDEF_CLASS_POINTER(Sphere); 68 | URDF_TYPEDEF_CLASS_POINTER(Visual); 69 | 70 | // create *_pointer_cast functions in urdf namespace 71 | template 72 | std::shared_ptr const_pointer_cast(std::shared_ptr const & r) 73 | { 74 | return std::const_pointer_cast(r); 75 | } 76 | 77 | template 78 | std::shared_ptr dynamic_pointer_cast(std::shared_ptr const & r) 79 | { 80 | return std::dynamic_pointer_cast(r); 81 | } 82 | 83 | template 84 | std::shared_ptr static_pointer_cast(std::shared_ptr const & r) 85 | { 86 | return std::static_pointer_cast(r); 87 | } 88 | 89 | } 90 | 91 | #endif 92 | -------------------------------------------------------------------------------- /include/urdf_model/utils.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Open Source Robotics Foundation (OSRF) 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the OSRF nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Steve Peters */ 36 | 37 | #ifndef URDF_INTERFACE_UTILS_H 38 | #define URDF_INTERFACE_UTILS_H 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace urdf { 47 | 48 | // Replacement for boost::split( ... , ... , boost::is_any_of(" ")) 49 | inline 50 | void split_string(std::vector &result, 51 | const std::string &input, 52 | const std::string &isAnyOf) 53 | { 54 | std::string::size_type start = 0; 55 | std::string::size_type end = input.find_first_of(isAnyOf, start); 56 | while (end != std::string::npos) 57 | { 58 | result.push_back(input.substr(start, end-start)); 59 | start = end + 1; 60 | end = input.find_first_of(isAnyOf, start); 61 | } 62 | if (start < input.length()) 63 | { 64 | result.push_back(input.substr(start)); 65 | } 66 | } 67 | 68 | // This is a locale-safe version of string-to-double, which is suprisingly 69 | // difficult to do correctly. This function ensures that the C locale is used 70 | // for parsing, as that matches up with what the XSD for double specifies. 71 | // On success, the double is returned; on failure, a std::runtime_error is 72 | // thrown. 73 | static inline double strToDouble(const char *in) 74 | { 75 | std::stringstream ss; 76 | ss.imbue(std::locale::classic()); 77 | 78 | ss << in; 79 | 80 | double out; 81 | ss >> out; 82 | 83 | if (ss.fail() || !ss.eof()) { 84 | throw std::runtime_error("Failed converting string to double"); 85 | } 86 | 87 | return out; 88 | } 89 | 90 | } 91 | 92 | #endif 93 | -------------------------------------------------------------------------------- /include/urdf_model_state/model_state.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: John Hsu */ 36 | 37 | #ifndef URDF_MODEL_STATE_H 38 | #define URDF_MODEL_STATE_H 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | #include "urdf_model/pose.h" 45 | #include 46 | #include "urdf_model_state/types.h" 47 | 48 | 49 | namespace urdf{ 50 | 51 | //round is not defined in C++98 52 | //So in Visual Studio <= 2012 is necessary to define it 53 | #ifdef _MSC_VER 54 | #if (_MSC_VER <= 1700) 55 | double round(double value) 56 | { 57 | return (value >= 0.0f)?(floor(value + 0.5f)):(ceil(value - 0.5f)); 58 | } 59 | #endif 60 | #endif 61 | 62 | class Time 63 | { 64 | public: 65 | Time() { this->clear(); }; 66 | 67 | void set(double _seconds) 68 | { 69 | this->sec = (int32_t)(floor(_seconds)); 70 | this->nsec = (int32_t)(round((_seconds - this->sec) * 1e9)); 71 | this->Correct(); 72 | }; 73 | 74 | operator double () 75 | { 76 | return (static_cast(this->sec) + 77 | static_cast(this->nsec)*1e-9); 78 | }; 79 | 80 | int32_t sec; 81 | int32_t nsec; 82 | 83 | void clear() 84 | { 85 | this->sec = 0; 86 | this->nsec = 0; 87 | }; 88 | private: 89 | void Correct() 90 | { 91 | // Make any corrections 92 | if (this->nsec >= 1e9) 93 | { 94 | this->sec++; 95 | this->nsec = (int32_t)(this->nsec - 1e9); 96 | } 97 | else if (this->nsec < 0) 98 | { 99 | this->sec--; 100 | this->nsec = (int32_t)(this->nsec + 1e9); 101 | } 102 | }; 103 | }; 104 | 105 | 106 | class JointState 107 | { 108 | public: 109 | JointState() { this->clear(); }; 110 | 111 | /// joint name 112 | std::string joint; 113 | 114 | std::vector position; 115 | std::vector velocity; 116 | std::vector effort; 117 | 118 | void clear() 119 | { 120 | this->joint.clear(); 121 | this->position.clear(); 122 | this->velocity.clear(); 123 | this->effort.clear(); 124 | } 125 | }; 126 | 127 | class ModelState 128 | { 129 | public: 130 | ModelState() { this->clear(); }; 131 | 132 | /// state name must be unique 133 | std::string name; 134 | 135 | Time time_stamp; 136 | 137 | void clear() 138 | { 139 | this->name.clear(); 140 | this->time_stamp.set(0); 141 | this->joint_states.clear(); 142 | }; 143 | 144 | std::vector joint_states; 145 | 146 | }; 147 | 148 | } 149 | 150 | #endif 151 | 152 | -------------------------------------------------------------------------------- /include/urdf_model_state/twist.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef URDF_MODEL_STATE_TWIST_ 36 | #define URDF_MODEL_STATE_TWIST_ 37 | 38 | #warning "Please Use #include " 39 | 40 | #include 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /include/urdf_model_state/types.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Steve Peters */ 36 | 37 | #ifndef URDF_MODEL_STATE_TYPES_H 38 | #define URDF_MODEL_STATE_TYPES_H 39 | 40 | #include 41 | 42 | 43 | namespace urdf{ 44 | 45 | class JointState; 46 | 47 | // typedef shared pointers 48 | typedef std::shared_ptr JointStateSharedPtr; 49 | 50 | } 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /include/urdf_sensor/sensor.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: John Hsu */ 36 | 37 | /* example 38 | 39 | 40 | 41 | 42 | 1.5708 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | */ 58 | 59 | 60 | 61 | #ifndef URDF_SENSOR_H 62 | #define URDF_SENSOR_H 63 | 64 | #include 65 | #include 66 | #include 67 | #include "urdf_model/pose.h" 68 | #include "urdf_model/joint.h" 69 | #include "urdf_model/link.h" 70 | #include "urdf_model/types.h" 71 | #include "urdf_sensor/types.h" 72 | 73 | namespace urdf{ 74 | 75 | class VisualSensor 76 | { 77 | public: 78 | enum {CAMERA, RAY} type; 79 | virtual ~VisualSensor(void) 80 | { 81 | } 82 | }; 83 | 84 | class Camera : public VisualSensor 85 | { 86 | public: 87 | Camera() { this->clear(); }; 88 | unsigned int width, height; 89 | /// format is optional: defaults to R8G8B8), but can be 90 | /// (L8|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8) 91 | std::string format; 92 | double hfov; 93 | double near; 94 | double far; 95 | 96 | void clear() 97 | { 98 | hfov = 0; 99 | width = 0; 100 | height = 0; 101 | format.clear(); 102 | near = 0; 103 | far = 0; 104 | }; 105 | }; 106 | 107 | class Ray : public VisualSensor 108 | { 109 | public: 110 | Ray() { this->clear(); }; 111 | unsigned int horizontal_samples; 112 | double horizontal_resolution; 113 | double horizontal_min_angle; 114 | double horizontal_max_angle; 115 | unsigned int vertical_samples; 116 | double vertical_resolution; 117 | double vertical_min_angle; 118 | double vertical_max_angle; 119 | 120 | void clear() 121 | { 122 | // set defaults 123 | horizontal_samples = 1; 124 | horizontal_resolution = 1; 125 | horizontal_min_angle = 0; 126 | horizontal_max_angle = 0; 127 | vertical_samples = 1; 128 | vertical_resolution = 1; 129 | vertical_min_angle = 0; 130 | vertical_max_angle = 0; 131 | }; 132 | }; 133 | 134 | 135 | class Sensor 136 | { 137 | public: 138 | Sensor() { this->clear(); }; 139 | 140 | /// sensor name must be unique 141 | std::string name; 142 | 143 | /// update rate in Hz 144 | double update_rate; 145 | 146 | /// transform from parent frame to optical center 147 | /// with z-forward and x-right, y-down 148 | Pose origin; 149 | 150 | /// sensor 151 | VisualSensorSharedPtr sensor; 152 | 153 | 154 | /// Parent link element name. A pointer is stored in parent_link_. 155 | std::string parent_link_name; 156 | 157 | LinkSharedPtr getParent() const 158 | {return parent_link_.lock();}; 159 | 160 | void setParent(LinkSharedPtr parent) 161 | { this->parent_link_ = parent; } 162 | 163 | void clear() 164 | { 165 | this->name.clear(); 166 | this->sensor.reset(); 167 | this->parent_link_name.clear(); 168 | this->parent_link_.reset(); 169 | }; 170 | 171 | private: 172 | LinkWeakPtr parent_link_; 173 | 174 | }; 175 | } 176 | #endif 177 | -------------------------------------------------------------------------------- /include/urdf_sensor/types.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Steve Peters */ 36 | 37 | #ifndef URDF_SENSOR_TYPES_H 38 | #define URDF_SENSOR_TYPES_H 39 | 40 | #include 41 | 42 | 43 | namespace urdf{ 44 | 45 | class VisualSensor; 46 | 47 | // typedef shared pointers 48 | typedef std::shared_ptr VisualSensorSharedPtr; 49 | 50 | } 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /include/urdf_world/types.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Steve Peters */ 36 | 37 | #ifndef URDF_WORLD_TYPES_H 38 | #define URDF_WORLD_TYPES_H 39 | 40 | #include 41 | 42 | 43 | namespace urdf{ 44 | 45 | class ModelInterface; 46 | 47 | // typedef shared pointers 48 | typedef std::shared_ptr ModelInterfaceSharedPtr; 49 | 50 | } 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /include/urdf_world/world.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: John Hsu */ 36 | 37 | /* encapsulates components in a world 38 | see http://ros.org/wiki/usdf/XML/urdf_world 39 | for details 40 | */ 41 | /* example world XML 42 | 43 | 44 | 47 | 48 | ... 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | */ 69 | 70 | #ifndef URDF_WORLD_H 71 | #define URDF_WORLD_H 72 | 73 | #include 74 | #include 75 | #include 76 | 77 | #include "urdf_model/model.h" 78 | #include "urdf_model/pose.h" 79 | #include "urdf_model/twist.h" 80 | #include "urdf_world/types.h" 81 | 82 | namespace urdf{ 83 | 84 | class Entity 85 | { 86 | public: 87 | ModelInterfaceSharedPtr model; 88 | Pose origin; 89 | Twist twist; 90 | }; 91 | 92 | class World 93 | { 94 | public: 95 | World() { this->clear(); }; 96 | 97 | /// world name must be unique 98 | std::string name; 99 | 100 | std::vector models; 101 | 102 | void clear() 103 | { 104 | this->name.clear(); 105 | }; 106 | }; 107 | } 108 | 109 | #endif 110 | 111 | --------------------------------------------------------------------------------