├── .gitignore ├── Makefile ├── test_data ├── README.md └── robots │ └── bh280.urdf ├── cmake └── FindTinyXML2.cmake ├── bin └── fix_stls ├── catkin-env-hooks └── 20.or_urdf.sh.in ├── scripts ├── startup_herb.py ├── startup_tim.py └── load.py ├── .travis.yml ├── package.xml ├── empty.iv ├── src ├── catkin_finder.h ├── boostfs_helpers.h ├── urdf_loader.h ├── catkin_finder.cpp ├── picojson.h └── urdf_loader.cpp ├── LICENSE ├── CMakeLists.txt ├── CHANGELOG.rst ├── tests └── test_urdf_loader.py └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk 2 | -------------------------------------------------------------------------------- /test_data/README.md: -------------------------------------------------------------------------------- 1 | Many of the robot files here have certain tags removed so as to not depend on herbpy. They should not be treated as complete URDF files and should only be used for testing OR_URDF. 2 | -------------------------------------------------------------------------------- /cmake/FindTinyXML2.cmake: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2014 Andrew Kelley 2 | # This file is MIT licensed. 3 | # See http://opensource.org/licenses/MIT 4 | 5 | # TinyXML2_FOUND 6 | # TinyXML2_INCLUDE_DIRS 7 | # TinyXML2_LIBRARIES 8 | 9 | find_path(TinyXML2_INCLUDE_DIRS NAMES tinyxml2.h) 10 | find_library(TinyXML2_LIBRARIES NAMES tinyxml2) 11 | 12 | include(FindPackageHandleStandardArgs) 13 | find_package_handle_standard_args(TinyXML2 DEFAULT_MSG TinyXML2_LIBRARIES TinyXML2_INCLUDE_DIRS) 14 | 15 | mark_as_advanced(TinyXML2_INCLUDE_DIRS TinyXML2_LIBRARIES) 16 | -------------------------------------------------------------------------------- /bin/fix_stls: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # 3 | # Repairs binary STL files to make them smaller and easier to import 4 | # 5 | # author: Pras Velagapudi 6 | # 7 | # Usage: ./cleanup.sh [model directory] 8 | # 9 | 10 | # Test for a provided directory, otherwise use current directory 11 | if test "$1" == "" 12 | then 13 | STL_DIR=`pwd` 14 | else 15 | STL_DIR="$1" 16 | fi 17 | 18 | # Replace the 'solid' keyword that sometimes shows up at the start of 19 | # binary STL files, causing them to be imported incorrectly as ASCII. 20 | sed -i 's/^solid/MODEL/g' `find $1 -iname \*.stl` 21 | -------------------------------------------------------------------------------- /catkin-env-hooks/20.or_urdf.sh.in: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # determine if we're in the devel or install space 4 | if [ "@DEVELSPACE@" = "True" -o "@DEVELSPACE@" = "true" ] 5 | then 6 | PLUGINS=@CATKIN_DEVEL_PREFIX@/lib/openrave-@OpenRAVE_LIBRARY_SUFFIX@ 7 | else 8 | PLUGINS=@CMAKE_INSTALL_PREFIX@/lib/openrave-@OpenRAVE_LIBRARY_SUFFIX@ 9 | fi 10 | 11 | # prepend to paths (if not already there) 12 | # from http://unix.stackexchange.com/a/124447 13 | case ":${OPENRAVE_PLUGINS:=$PLUGINS}:" in 14 | *:$PLUGINS:*) ;; 15 | *) OPENRAVE_PLUGINS="$PLUGINS:$OPENRAVE_PLUGINS" ;; 16 | esac 17 | 18 | export OPENRAVE_PLUGINS 19 | -------------------------------------------------------------------------------- /scripts/startup_herb.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Loads a URDF to test the loader. 4 | # 5 | import os; 6 | 7 | # Get this script path (in or_urdf) and add it to the openrave path 8 | or_urdf_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'lib') 9 | or_plugin_path = os.getenv('OPENRAVE_PLUGINS', '') 10 | os.environ['OPENRAVE_PLUGINS'] = os.pathsep.join([or_urdf_path, or_plugin_path]) 11 | 12 | import openravepy 13 | 14 | env = openravepy.Environment() 15 | env.SetViewer('qtcoin') 16 | plugin = openravepy.RaveCreateModule(env, "urdf") 17 | 18 | plugin.SendCommand("load /homes/pkv/ros/local/systemconf/herb2.urdf") 19 | #plugin.SendCommand("load /homes/pkv/ros/local/apps/librarian/tema_tim_description/robots/tim/tim.urdf") 20 | #plugin.SendCommand("load /homes/pkv/ros/local/herb_urdf/robots/herb_urdf.URDF") 21 | -------------------------------------------------------------------------------- /scripts/startup_tim.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Loads a URDF to test the loader. 4 | # 5 | import os; 6 | 7 | # Get this script path (in or_urdf) and add it to the openrave path 8 | or_urdf_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'lib') 9 | or_plugin_path = os.getenv('OPENRAVE_PLUGINS', '') 10 | os.environ['OPENRAVE_PLUGINS'] = os.pathsep.join([or_urdf_path, or_plugin_path]) 11 | 12 | import openravepy 13 | 14 | env = openravepy.Environment() 15 | env.SetViewer('qtcoin') 16 | plugin = openravepy.RaveCreateModule(env, "urdf") 17 | 18 | #plugin.SendCommand("load /homes/pkv/ros/local/systemconf/herb2.urdf") 19 | plugin.SendCommand("load /homes/pkv/ros/local/apps/librarian/tema_tim_description/robots/tim/tim.urdf") 20 | #plugin.SendCommand("load /homes/pkv/ros/local/herb_urdf/robots/herb_urdf.URDF") 21 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | dist: trusty 2 | sudo: required 3 | language: generic 4 | cache: 5 | - apt 6 | env: 7 | global: 8 | - REPOSITORY=or_urdf 9 | - DISTRIBUTION=https://raw.githubusercontent.com/personalrobotics/pr-rosinstalls/feature/rosdistro/repositories.yml 10 | # Install test fixture dependencies. 11 | before_install: 12 | - mkdir -p "${HOME}/workspace/src" 13 | - cd "${HOME}/workspace" 14 | - curl -sSo distribution.yml "${DISTRIBUTION}" 15 | - git clone https://github.com/personalrobotics/pr-cleanroom.git scripts 16 | - ./scripts/internal-setup.sh 17 | - export PACKAGE_NAMES="$(./scripts/internal-get-packages.py distribution.yml ${REPOSITORY})" 18 | install: 19 | - mv "${TRAVIS_BUILD_DIR}" src 20 | - ./scripts/internal-distro.py --workspace=src distribution.yml --repository "${REPOSITORY}" 21 | script: 22 | - ./scripts/internal-build.sh ${PACKAGE_NAMES} 23 | - ./scripts/internal-test.sh ${PACKAGE_NAMES} 24 | after_script: 25 | - ./scripts/view-all-results.sh test_results 26 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | or_urdf 4 | 0.4.0 5 | 6 | OpenRAVE plugin for loading URDF and SRDF files. 7 | 8 | https://github.com/personalrobotics/or_urdf 9 | Michael Koval 10 | Michael Koval 11 | Pras Velagapudi 12 | BSD 13 | catkin 14 | boost 15 | tinyxml2 16 | openrave 17 | roslib 18 | srdfdom 19 | urdf 20 | boost 21 | tinyxml2 22 | openrave 23 | roslib 24 | srdfdom 25 | urdf 26 | python-nose 27 | python-numpy 28 | python 29 | 30 | -------------------------------------------------------------------------------- /empty.iv: -------------------------------------------------------------------------------- 1 | #Inventor V2.0 ascii 2 | 3 | Separator { 4 | Info { 5 | string "empty.iv generated by IVCON." 6 | string "Original data in file NO_IN_NAME." 7 | } 8 | Separator { 9 | LightModel { 10 | model PHONG 11 | } 12 | MatrixTransform { matrix 13 | 0.000000 0.000000 0.000000 0.000000 14 | 0.000000 0.000000 0.000000 0.000000 15 | 0.000000 0.000000 0.000000 0.000000 16 | 0.000000 0.000000 0.000000 0.000000 17 | } 18 | Material { 19 | ambientColor 0.2 0.2 0.2 20 | diffuseColor 0.8 0.8 0.8 21 | emissiveColor 0.0 0.0 0.0 22 | specularColor 0.0 0.0 0.0 23 | shininess 0.2 24 | transparency 0.0 25 | } 26 | MaterialBinding { 27 | value PER_VERTEX_INDEXED 28 | } 29 | NormalBinding { 30 | value PER_VERTEX_INDEXED 31 | } 32 | TextureCoordinateBinding { 33 | value PER_VERTEX_INDEXED 34 | } 35 | ShapeHints { 36 | vertexOrdering COUNTERCLOCKWISE 37 | shapeType UNKNOWN_SHAPE_TYPE 38 | faceType CONVEX 39 | creaseAngle 6.28319 40 | } 41 | Coordinate3 { 42 | point [ 43 | ] 44 | } 45 | TextureCoordinate2 { 46 | point [ 47 | ] 48 | } 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /src/catkin_finder.h: -------------------------------------------------------------------------------- 1 | /** \file catkin_finder.h 2 | * \brief Utility to resolve package to file URIs in catkin workspaces 3 | * \author Michael Koval 4 | * \author Chris Dellin 5 | * \date 2015 6 | */ 7 | 8 | /* (C) Copyright 2015 Carnegie Mellon University */ 9 | 10 | #ifndef OR_URDF_CATKIN_FINDER_H_ 11 | #define OR_URDF_CATKIN_FINDER_H_ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace or_urdf { 18 | 19 | /// Utility to resolve "package://" paths against catkin workspaces. 20 | /// Should replicate the behaviour of the tool 21 | /// catkin_find package relative/path 22 | /// 23 | /// Resolution code borrowed heavily from 24 | /// aikido::util::CatkinResourceRetriever, 25 | /// simplified to remove DART dependencies 26 | /// and to do local path resolution only 27 | class CatkinFinder 28 | { 29 | public: 30 | CatkinFinder(); 31 | ~CatkinFinder(); 32 | 33 | // input: package:// URI 34 | // output: native local filesystem path if found ("" otherwise) 35 | std::string find(const std::string & uri) const; 36 | 37 | private: 38 | struct Workspace { 39 | std::string mPath; 40 | std::unordered_map mSourceMap; 41 | }; 42 | std::vector mWorkspaces; 43 | static std::vector getWorkspaces(); 44 | }; 45 | 46 | } // namespace or_urdf 47 | 48 | #endif // ifndef OR_URDF_CATKIN_FINDER_H_ 49 | -------------------------------------------------------------------------------- /scripts/load.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = 'or_urdf' 3 | import argparse, openravepy, os, sys 4 | 5 | parser = argparse.ArgumentParser(description='loads a URDF model into OpenRAVE') 6 | parser.add_argument('urdf_path', type=str) 7 | parser.add_argument('srdf_path', type=str, default='', nargs='?') 8 | parser.add_argument('-i', '--interactive', action='store_true', help='display the model in an OpenRAVE viewer') 9 | parser.add_argument('-c', '--config', type=str, action='store', help='config file for urdf generation') 10 | args = parser.parse_args() 11 | 12 | # Load the plugin. 13 | env = openravepy.Environment() 14 | plugin = openravepy.RaveCreateModule(env, 'urdf') 15 | if plugin is None: 16 | parser.error('Unable to load urdf plugin.') 17 | sys.exit(1) 18 | 19 | # Generate the KinBody XML. 20 | try: 21 | kinbody_xml = plugin.SendCommand('load {0:s} {1:s}'.format(args.urdf_path, args.srdf_path)) 22 | if kinbody_xml is None: 23 | raise openravepy.openrave_exception('An unknown error has occurred.') 24 | except openravepy.openrave_exception, e: 25 | parser.error('Failed generating KinBody: {0:s}'.format(e.message)) 26 | sys.exit(1) 27 | 28 | #env.SetViewer('qtcoin') 29 | 30 | body = env.GetBodies()[0] 31 | handles = list() 32 | 33 | """ 34 | for link in body.GetLinks(): 35 | pose = link.GetTransform() 36 | handle = openravepy.misc.DrawAxes(env, pose, 0.2, 2) 37 | handles.append(handle) 38 | """ 39 | 40 | if args.interactive: 41 | import IPython; IPython.embed() 42 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, Carnegie Mellon University 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | - Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | - Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | - Neither the name of Carnegie Mellon University nor the names of its 13 | contributors may be used to endorse or promote products derived from this 14 | software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | POSSIBILITY OF SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(or_urdf) 3 | 4 | list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roslib 8 | srdfdom 9 | urdf 10 | ) 11 | 12 | find_package(Boost REQUIRED COMPONENTS 13 | filesystem 14 | system 15 | ) 16 | find_package(OpenRAVE REQUIRED) 17 | find_package(TinyXML2 REQUIRED) 18 | 19 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 20 | 21 | catkin_package() 22 | catkin_add_env_hooks("20.${PROJECT_NAME}" 23 | SHELLS sh 24 | DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/catkin-env-hooks" 25 | ) 26 | 27 | include_directories( 28 | ${Boost_INCLUDE_DIRS} 29 | ${catkin_INCLUDE_DIRS} 30 | ${TinyXML2_INCLUDE_DIRS} 31 | ${OpenRAVE_INCLUDE_DIRS} 32 | ) 33 | link_directories( 34 | ${Boost_LIBRARY_DIRS} 35 | ${catkin_LIBRARY_DIRS} 36 | ${OpenRAVE_LIBRARY_DIRS} 37 | ) 38 | 39 | add_library("${PROJECT_NAME}_plugin" MODULE 40 | src/urdf_loader.cpp 41 | src/catkin_finder.cpp 42 | ) 43 | set_target_properties(${PROJECT_NAME}_plugin PROPERTIES 44 | PREFIX "" 45 | COMPILE_FLAGS "${OpenRAVE_CXX_FLAGS}" 46 | LINK_FLAGS "${OpenRAVE_LINK_FLAGS}" 47 | LIBRARY_OUTPUT_DIRECTORY 48 | "${CATKIN_DEVEL_PREFIX}/lib/openrave-${OpenRAVE_LIBRARY_SUFFIX}") 49 | target_link_libraries("${PROJECT_NAME}_plugin" 50 | ${catkin_LIBRARIES} 51 | ${TinyXML2_LIBRARIES} 52 | ${OpenRAVE_LIBRARIES} 53 | ) 54 | 55 | if(CATKIN_ENABLE_TESTING) 56 | catkin_add_nosetests(tests) 57 | endif(CATKIN_ENABLE_TESTING) 58 | 59 | install(TARGETS "${PROJECT_NAME}_plugin" 60 | LIBRARY DESTINATION "lib/openrave-${OpenRAVE_LIBRARY_SUFFIX}" 61 | ) 62 | 63 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package or_urdf 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.4.0 (2015-10-12) 6 | ------------------ 7 | * Added support for multiple collision objects within one link (`#13 `_) 8 | * Added missing dependency on OpenRAVE 9 | * Set the source URI of loaded bodies (`#14 `_) 10 | * Contributors: Chris Dellin, Dawid Seredyński, Michael Koval 11 | 12 | 0.3.0 (2015-04-15) 13 | ------------------ 14 | * Update README.md 15 | * Added some more unsupported features to the README 16 | * Fixed a typo (thanks @cdellin!) 17 | * Merge pull request `#12 `_ from personalrobotics/bugfix/8 18 | Support package:// URDF and SRDF URIs 19 | * Support package:// URDF and SRDF URIs (`#8 `_) 20 | * Merge pull request `#11 `_ from personalrobotics/bugfix/ContinuousJointLimits 21 | Fixed support for continuous joints. 22 | * Added a fix for limits on continuous joints. 23 | * Don't convert revolute joints to continuous joints. 24 | This was previously happening if a continuous joint defined any limits, 25 | including velocity and effort limits. 26 | * Force continuous joints to have +/-INF limits. 27 | * Contributors: Michael Koval 28 | 29 | 0.2.0 (2015-03-30) 30 | ------------------ 31 | * Replaced RAVELOG_INFO with RAVELOG_DEBUG. 32 | * Manually apply the URDF collision scale to meshes. 33 | * Added support for scaling visual meshes. 34 | * Added LICENSE. 35 | * Added a README. 36 | * Stripped out YAML dependencies (unnecessary if an SRDF is being used). 37 | * Contributors: Michael Koval, Prasanna Velagapudi 38 | -------------------------------------------------------------------------------- /src/boostfs_helpers.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file boostfs_helpers.h 3 | * \brief Helper functions for BoostFS. 4 | * \author Pras Velagapudi 5 | * \date 2013 6 | */ 7 | /* (C) Copyright 2013 Carnegie Mellon University */ 8 | #include 9 | #include 10 | #include "boost/version.hpp" 11 | 12 | namespace boost 13 | { 14 | #if BOOST_VERSION > 104900 15 | namespace filesystem 16 | #else 17 | namespace filesystem3 18 | #endif 19 | { 20 | template < > 21 | path& path::append< typename path::iterator >( typename path::iterator begin, typename path::iterator end, const codecvt_type& cvt) 22 | { 23 | for( ; begin != end ; ++begin ) 24 | *this /= *begin; 25 | return *this; 26 | } 27 | 28 | // Return path when appended to a_From will resolve to same as a_To 29 | boost::filesystem::path make_relative( boost::filesystem::path a_From, boost::filesystem::path a_To ) 30 | { 31 | a_From = boost::filesystem::absolute( a_From ); a_To = boost::filesystem::absolute( a_To ); 32 | boost::filesystem::path ret; 33 | boost::filesystem::path::const_iterator itrFrom( a_From.begin() ), itrTo( a_To.begin() ); 34 | 35 | // Find common base 36 | for( boost::filesystem::path::const_iterator toEnd( a_To.end() ), fromEnd( a_From.end() ) ; 37 | itrFrom != fromEnd && itrTo != toEnd && *itrFrom == *itrTo; ++itrFrom, ++itrTo ); 38 | 39 | // Navigate backwards in directory to reach previously found base 40 | for( boost::filesystem::path::const_iterator fromEnd( a_From.end() ); itrFrom != fromEnd; ++itrFrom ) { 41 | if( (*itrFrom) != "." ) 42 | ret /= ".."; 43 | } 44 | 45 | // Now navigate down the directory branch 46 | ret.append( itrTo, a_To.end() ); 47 | return ret; 48 | } 49 | } 50 | } 51 | 52 | 53 | -------------------------------------------------------------------------------- /tests/test_urdf_loader.py: -------------------------------------------------------------------------------- 1 | #!//bin/env python 2 | import numpy 3 | import unittest 4 | import openravepy 5 | import subprocess 6 | import os 7 | import sys 8 | 9 | # Find the bh_280 URDF file 10 | from catkin.find_in_workspaces import find_in_workspaces 11 | 12 | share_directories = find_in_workspaces(search_dirs=['share'], 13 | project='or_urdf', 14 | path='test_data/' 15 | ) 16 | 17 | if not share_directories: 18 | raise ValueError('Unable to find BH280 model.') 19 | 20 | found_models = False 21 | for share_directory in share_directories: 22 | urdf_path = os.path.join(share_directories[0], 'robots', 'bh280.urdf') 23 | if os.path.exists(urdf_path): 24 | found_models=True 25 | break 26 | 27 | if not found_models: 28 | raise ValueError('Unable to find BH280 URDF file.') 29 | 30 | 31 | class TestORUrdf(unittest.TestCase): 32 | def setUp(self): 33 | self.env = openravepy.Environment() 34 | urdf_module = openravepy.RaveCreateModule(self.env, 'urdf') 35 | 36 | with self.env: 37 | arg = 'load {:s}'.format(urdf_path) 38 | bh_name = urdf_module.SendCommand(arg) 39 | self.bh_body = self.env.GetKinBody(bh_name) 40 | 41 | def tearDown(self): 42 | self.env.Destroy() 43 | 44 | def TestLinkLocalInertia(self): 45 | #Test inertia for a random link 46 | hand_base_link = self.bh_body.GetLink('/hand_base') 47 | numpy.testing.assert_array_almost_equal( 48 | hand_base_link.GetLocalInertia(), 49 | numpy.diag([0.0006986,0.00050354,0.00062253])) 50 | 51 | def TestLinkMass(self): 52 | #Test mass for a random link 53 | finger0_0 = self.bh_body.GetLink('/finger0_0') 54 | numpy.testing.assert_almost_equal(finger0_0.GetMass(),0.14109) 55 | 56 | def TestLinkLocalCOM(self): 57 | finger0_0 = self.bh_body.GetLink('/finger0_0') 58 | numpy.testing.assert_array_almost_equal( 59 | finger0_0.GetLocalCOM(), 60 | numpy.array([0.030616,-7.3219e-05,-0.011201])) 61 | 62 | def TestJointLimits(self): 63 | #Test extent and velocity limits of a join 64 | joint = self.bh_body.GetJoint('/j00') 65 | numpy.testing.assert_array_almost_equal( 66 | joint.GetLimits()[0],numpy.array([0])) 67 | numpy.testing.assert_array_almost_equal( 68 | joint.GetLimits()[1],numpy.array([numpy.pi])) 69 | numpy.testing.assert_array_almost_equal( 70 | joint.GetMaxVel(),2.0) 71 | 72 | def TestParentChild(self): 73 | #Test parent and child links of joint 74 | joint = self.bh_body.GetJoint('/j01') 75 | numpy.testing.assert_equal( 76 | joint.GetHierarchyChildLink().GetName(),'/finger0_1') 77 | numpy.testing.assert_equal( 78 | joint.GetHierarchyParentLink().GetName(),'/finger0_0') 79 | 80 | def TestMimicEquation(self): 81 | #Test mimic equation for a join 82 | joint = self.bh_body.GetJoint('/j22') 83 | numpy.testing.assert_equal( 84 | joint.GetMimicEquation(),'/j21*0.321429+0.000000') 85 | -------------------------------------------------------------------------------- /src/urdf_loader.h: -------------------------------------------------------------------------------- 1 | /** \file urdf_loader.h 2 | * \brief Interface to a URDF loading plugin for OpenRAVE 3 | * \author Pras Velagapudi 4 | * \date 2013 5 | */ 6 | 7 | /* (C) Copyright 2013 Carnegie Mellon University */ 8 | #ifndef URDF_LOADER_H 9 | #define URDF_LOADER_H 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include "catkin_finder.h" 23 | 24 | namespace or_urdf 25 | { 26 | class URDFLoader : public OpenRAVE::ModuleBase 27 | { 28 | public: 29 | /** Opens a URDF file and returns a robot in OpenRAVE */ 30 | bool loadURI(std::ostream& sout, std::istream& sin); 31 | bool loadJsonString(std::ostream& sout, std::istream& sin); 32 | bool deprecatedLoad(std::ostream&, std::istream& sin); 33 | 34 | /** Constructs plugin and registers functions */ 35 | URDFLoader(OpenRAVE::EnvironmentBasePtr env) : OpenRAVE::ModuleBase(env) 36 | { 37 | __description = "URDFLoader: Loader that imports URDF files."; 38 | _env = env; 39 | 40 | RegisterCommand("LoadURI", boost::bind(&URDFLoader::loadURI, this, _1, _2), 41 | "load URDF and SRDF from file/URI"); 42 | RegisterCommand("LoadString", boost::bind(&URDFLoader::loadJsonString, this, _1, _2), 43 | "load URDF and SRDF from json string wrapping XML"); 44 | RegisterCommand("load", boost::bind(&URDFLoader::deprecatedLoad, this, _1, _2), 45 | "Deprecated method name to load URDF and SRDF from file/URI"); 46 | } 47 | 48 | void Destroy() { RAVELOG_INFO("URDF loader unloaded from environment\n"); } 49 | 50 | virtual ~URDFLoader() {} 51 | 52 | void ParseURDF(urdf::Model &model, std::vector &link_infos, 53 | std::vector &joint_infos); 54 | 55 | void ParseSRDF(urdf::Model const &urdf, 56 | srdf::Model const &srdf, 57 | std::vector &link_infos, 58 | std::vector &joint_infos, 59 | std::vector &manip_infos); 60 | 61 | void ProcessGeometryGroupTagsFromURDF( 62 | TiXmlDocument &xml_doc, 63 | std::vector &link_infos); 64 | 65 | /* This is called on env.LoadProblem(m, 'command') */ 66 | int main(const std::string& cmd) { RAVELOG_INFO("URDF loader initialized with command: %s\n", cmd.c_str()); return 0; } 67 | 68 | private: 69 | /** Reference to OpenRAVE environment, filled in on construction */ 70 | OpenRAVE::EnvironmentBasePtr _env; 71 | or_urdf::CatkinFinder _catkin_finder; 72 | 73 | std::string loadModel(urdf::Model &urdf_model, TiXmlDocument &xml_doc, 74 | std::shared_ptr srdf_model = nullptr, 75 | std::string uri = std::string()); 76 | std::string resolveURI(const std::string &path) const; 77 | std::string resolveURIorPath(const std::string &path) const; 78 | }; 79 | 80 | } /* namespace or_urdf */ 81 | 82 | #endif // URDF_LOADER_H 83 | -------------------------------------------------------------------------------- /test_data/robots/bh280.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # or_urdf 2 | 3 | [![Build Status](https://travis-ci.org/personalrobotics/or_urdf.svg?branch=master)](https://travis-ci.org/personalrobotics/or_urdf) 4 | 5 | or_urdf is an OpenRAVE plugin for loading a URDF and, optionally, SRDF file as 6 | an OpenRAVE `KinBody` or `Robot`. This package provides the OpenRAVE `URDF` 7 | module through a plugin. This plugin can be instantiated using the following 8 | Python code: 9 | 10 | ```python 11 | module = RaveCreateModule(env, 'urdf') 12 | ``` 13 | 14 | Once the module has been instantiated, you can use the module to load a 15 | `KinBody` by calling the custom `load` command with a path to a URDF file. 16 | Similarly, you can create an OpenRAVE `Robot` calling the `load` command with 17 | paths to both `URDF` and `SRDF` files. 18 | 19 | 20 | ## Why URDF and SRDF? 21 | 22 | [URDF](http://wiki.ros.org/urdf) and [SRDF](http://wiki.ros.org/srdf) are the 23 | standard XML file formats used to describe robots in [ROS](http://www.ros.org). 24 | These files are [available for many robots](http://wiki.ros.org/urdf/Examples) 25 | and are a key requirement of using 26 | [MoveIt!](http://moveit.ros.org) and/or [RViz](http://wiki.ros.org/rviz). This 27 | module uses the standard [urdfdom](https://github.com/ros/urdfdom) and 28 | [srdfdom](https://github.com/ros-planning/srdfdom) parsers and, thus, should be 29 | compatible with any robot specification that obeys the URDF and SRDF standards. 30 | 31 | 32 | ## Why programmatic construction? 33 | 34 | or_urdf takes the unique approach of *progammatically constructing OpenRAVE 35 | objects* from the URDF and SRDF files. This is in contrast to other conversion 36 | functions, like the [`urdf_to_collada` 37 | script](http://wiki.ros.org/collada_urdf), which converts the input URDF into 38 | an intermediate file format. Programmatically constructing the OpenRAVE objects 39 | has several key advantages over this alternative approach: 40 | 41 | 1. Relative `file://` and `package://` URI's are resolved at run-time. 42 | 2. There is no need to re-generate any auto-generated files when the URDF or 43 | SRDF files change. 44 | 3. There is no loss in precision due to the serialization and deserialization 45 | of floating point numbers. 46 | 4. The URDF and SRDF specifications can be loaded directly from the 47 | `robot_description` and `semantic_robot_description` ROS parameters. 48 | 49 | 50 | ## Loading a KinBody from URDF 51 | 52 | The following code will load the URDF model `/path/to/my/model.urdf` as an 53 | OpenRAVE `KinBody`: 54 | 55 | ```python 56 | with env: 57 | name = module.SendCommand('load /path/to/my/model.urdf') 58 | body = env.GetKinBody(name) 59 | ``` 60 | 61 | The following OpenRAVE properties have no equivalent in URDF and, thus, must be 62 | manually configured on a `KinBody` created by or_urdf: 63 | 64 | - Acceleration limits (`robot.SetDOFAccelerationLimits`) 65 | - DOF resolutions (`robot.SetDOFResolutions`) 66 | - Mass and inertia properties are **currently untested** 67 | 68 | The `load` command programmatically creates a `KinBody` from the URDF by 69 | building a list of `LinkInfo` and `JointInfo` structures. This `KinBody` is 70 | then added to the environment with the `anonymous` flag set. The command 71 | returns the name of the object that was added to the environment. This is 72 | necessary to avoid introducing name conflicts or ambiguity when loading 73 | multiple instances of a URDF file into the same environment. 74 | 75 | You can easily change the name of the resultant `KinBody` if the name generated 76 | by this automatic procedure are undesirable. To do so, you must: (1) remove the 77 | `KinBody` from the environment, (2) change the `KinBody`'s name, and (3) add 78 | the `KinBody` back to the environment. For example: 79 | 80 | ```python 81 | with env: 82 | name = module.SendCommand('load /path/to/my/model.urdf') 83 | body = env.GetKinBody(name) 84 | 85 | env.Remove(body) 86 | body.SetName('my_custom_name') 87 | env.Add(body) 88 | ``` 89 | 90 | 91 | ## Loading a Robot from URDF and SRDF 92 | 93 | or_urdf also supports loading an OpenRAVE `Robot` from the combination of a 94 | URDF and SRDF file. In this case, the URDF file fills the role of an OpenRAVE 95 | `.kinbody.xml` file and the SRDF file fills the role of an OpenRAVE 96 | `.robot.xml` file. Unfortunately, there is not a direct mapping between the 97 | SRDF file format and the features supported by OpenRAVE. or_urdf performs the 98 | conversion as follows: 99 | 100 | - ``: flags the pair of links ad adjacent 101 | - ``: is used to specify one or more `` tags, which: 102 | - ``: creates a sphere geometry in the `spheres` geometry group 103 | - ``: defines a manipulator 104 | - `name`: sets the name of the OpenRAVE manipulator 105 | - `parent_group`: contains the manipulator DOFs (this is **required**) 106 | - `group`: contains the gripper DOFs (this is **required**, but the group be empty) 107 | - ``: not used 108 | - ``: not used 109 | - ``: not used 110 | - ``: not used, except if referenced in `` 111 | 112 | The following OpenRAVE properties have no equivalent in SRDF and, thus, must be 113 | manually configured on a `Robot` create by `or_urdf`: 114 | 115 | - Robot DOF weights (`robot.SetDOFWeights`) 116 | - Manipulator IK solver (`manipulaor.SetIkSolver`) 117 | - Manipulator closing/chucking direction (`manipulaor.SetClosingDirection` or 118 | `manipulator.SetChuckingDirection`, depending upon the version of OpenRAVE) 119 | - Manipulator tool transform and direction (`manipulator.SetLocalToolTransform` 120 | and `manipulator.SetLocalToolDirection`) 121 | - All methods on `KinBody` listed above 122 | 123 | Just as with creating a `KinBody`, or_urdf programmatically creates the `Robot` 124 | by constructing `LinkInfo`, `JointInfo`, and `ManipulatorInfo` structs. The 125 | following code creates an OpenRAVE `Robot` from the paired 126 | `/path/to/my/model.urdf` and `/path/to/my/model.srdf` files: 127 | 128 | ```python 129 | with env: 130 | name = module.SendCommand('load /path/to/my/model.urdf /path/to/my/model.srdf') 131 | body = env.GetRobot(name) 132 | ``` 133 | 134 | See above for more information about how to rename the robot created by the 135 | module. 136 | 137 | 138 | ## License 139 | 140 | or_urdf is licensed under a BSD license. See `LICENSE` for more information. 141 | 142 | 143 | # Contributors 144 | 145 | or_urdf was developed by the 146 | [Personal Robotics Lab](https://personalrobotics.ri.cmu.edu) in the 147 | [Robotics Institute](https://www.ri.cmu.edu) at 148 | [Carnegie Mellon University](http://www.cmu.edu). This library is developed and 149 | maintained by 150 | [Michael Koval](https://github.com/mkoval) and 151 | [Pras Velagapudi](https://github.com/psigen). 152 | 153 | -------------------------------------------------------------------------------- /src/catkin_finder.cpp: -------------------------------------------------------------------------------- 1 | /** \file catkin_finder.h 2 | * \brief Utility to resolve package to file URIs in catkin workspaces 3 | * \author Michael Koval 4 | * \author Chris Dellin 5 | * \date 2015 6 | */ 7 | 8 | /* (C) Copyright 2015 Carnegie Mellon University */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "catkin_finder.h" 17 | 18 | static const std::string CATKIN_MARKER(".catkin"); 19 | 20 | namespace { 21 | 22 | std::string getPackageNameFromXML(const std::string& _path) 23 | { 24 | using tinyxml2::XMLHandle; 25 | using tinyxml2::XMLElement; 26 | 27 | tinyxml2::XMLDocument document; 28 | if (document.LoadFile(_path.c_str())) 29 | { 30 | RAVELOG_WARN("Failed loading package.xml file '%s': %s\n", 31 | _path.c_str(), document.GetErrorStr1()); 32 | return ""; 33 | } 34 | 35 | XMLHandle root_handle(document.RootElement()); 36 | XMLHandle name_handle = root_handle.FirstChildElement("name"); 37 | XMLElement* name_element = name_handle.ToElement(); 38 | 39 | if (!name_element) 40 | { 41 | RAVELOG_WARN("Failed loading package.xml file '%s': " 42 | "File does not contain a element.\n", _path.c_str()); 43 | return ""; 44 | } 45 | 46 | if (!name_element->GetText()) 47 | { 48 | RAVELOG_WARN("Failed loading package.xml file '%s': " 49 | " element is empty.\n", _path.c_str()); 50 | return ""; 51 | } 52 | 53 | std::string package_name = name_element->GetText(); 54 | boost::algorithm::trim(package_name); 55 | 56 | if (package_name.empty()) 57 | { 58 | RAVELOG_WARN("Failed loading package.xml file '%s': " 59 | " element is empty.\n", _path.c_str()); 60 | return ""; 61 | } 62 | 63 | return package_name; 64 | } 65 | 66 | void searchForPackages(const boost::filesystem::path& _packagePath, 67 | std::unordered_map& _packageMap) 68 | { 69 | using boost::filesystem::directory_iterator; 70 | using boost::filesystem::path; 71 | using boost::filesystem::file_status; 72 | using boost::filesystem::exists; 73 | 74 | // Ignore this directory if it contains a CATKIN_IGNORE file. 75 | const path catkin_ignore_path = _packagePath / "CATKIN_IGNORE"; 76 | if (exists(catkin_ignore_path)) 77 | return; 78 | 79 | // Try loading the package.xml file. 80 | const path package_xml_path = _packagePath / "package.xml"; 81 | if (exists(package_xml_path)) 82 | { 83 | const std::string package_name = getPackageNameFromXML( 84 | package_xml_path.string()); 85 | if (!package_name.empty()) 86 | { 87 | const auto result = _packageMap.insert( 88 | std::make_pair(package_name, _packagePath.string())); 89 | if (!result.second) 90 | { 91 | RAVELOG_WARN("Found two package.xml files for package '%s': '%s' and '%s'.\n", 92 | package_name.c_str(), 93 | result.first->second.c_str(), 94 | _packagePath.c_str()); 95 | } 96 | return; // Don't search for packages inside packages. 97 | } 98 | } 99 | 100 | // Recurse on subdirectories. 101 | directory_iterator it(_packagePath); 102 | directory_iterator end; 103 | 104 | while (it != end) 105 | { 106 | boost::system::error_code status_error; 107 | const file_status status = it->status(status_error); 108 | if (status_error) 109 | { 110 | RAVELOG_WARN("Failed recursing into directory '%s'.\n", it->path().c_str()); 111 | continue; 112 | } 113 | 114 | if (status.type() == boost::filesystem::directory_file) 115 | searchForPackages(it->path().string(), _packageMap); 116 | 117 | ++it; 118 | } 119 | } 120 | 121 | } // anonymous namespace 122 | 123 | or_urdf::CatkinFinder::CatkinFinder(): 124 | mWorkspaces(getWorkspaces()) 125 | { 126 | } 127 | 128 | std::vector or_urdf::CatkinFinder::getWorkspaces() 129 | { 130 | std::vector workspaces; 131 | 132 | // initialize workspaces 133 | // (from CatkinResourceRetriever::getWorkspaces()) 134 | using boost::filesystem::path; 135 | 136 | const char *cmake_prefix_path = std::getenv("CMAKE_PREFIX_PATH"); 137 | if (!cmake_prefix_path) 138 | { 139 | RAVELOG_WARN("The CMAKE_PREFIX_PATH" 140 | " environmental variable is not defined. Did you source" 141 | " 'setup.bash' in this shell?\n"); 142 | return workspaces; 143 | } 144 | 145 | // Split CMAKE_PREFIX_PATH by the ':' path separator delimiter. 146 | std::vector workspace_candidates; 147 | boost::split(workspace_candidates, cmake_prefix_path, boost::is_any_of(":")); 148 | 149 | for (const std::string& workspace_path : workspace_candidates) 150 | { 151 | // Skip empty or non-existant workspace directories 152 | if (workspace_path.empty()) 153 | continue; 154 | boost::filesystem::path workspace_bpath(workspace_path); 155 | if (!boost::filesystem::is_directory(workspace_bpath)) 156 | continue; 157 | 158 | // Construct workspace 159 | or_urdf::CatkinFinder::Workspace workspace; 160 | workspace.mPath = workspace_path; 161 | 162 | // Filter out directories that are missing the Catkin marker file. If the 163 | // marker file exists, then we also read its contents to build a list of all 164 | // source directories. 165 | boost::filesystem::path marker_bpath = workspace_bpath / CATKIN_MARKER; 166 | if (boost::filesystem::exists(marker_bpath)) 167 | { 168 | std::ifstream ifs(marker_bpath.c_str()); 169 | std::string contents( (std::istreambuf_iterator(ifs) ), 170 | (std::istreambuf_iterator() ) ); 171 | 172 | // Split the string into a list of paths by the ';' delimiter. I'm not 173 | // sure why this doesn't use the standard path separator delimitor ':'. 174 | std::vector source_paths; 175 | if (!contents.empty()) 176 | boost::split(source_paths, contents, boost::is_any_of(";")); 177 | 178 | for (const std::string& source_path : source_paths) 179 | searchForPackages(source_path, workspace.mSourceMap); 180 | } 181 | else 182 | { 183 | RAVELOG_WARN("Failed reading package" 184 | " source directories from the marker file '%s" 185 | "'. Resources in the source" 186 | " space of this workspace will not resolve.\n", 187 | marker_bpath.c_str()); 188 | } 189 | 190 | workspaces.push_back(workspace); 191 | } 192 | 193 | return workspaces; 194 | } 195 | 196 | 197 | or_urdf::CatkinFinder::~CatkinFinder() 198 | { 199 | } 200 | 201 | std::string or_urdf::CatkinFinder::find(const std::string & uri) const 202 | { 203 | using boost::filesystem::path; 204 | 205 | // ensure uri starts with package:// 206 | if (uri.substr(0,10) != "package://") 207 | { 208 | RAVELOG_ERROR("CatkinFinder passed a non package:// path!\n"); 209 | return ""; 210 | } 211 | std::string nonprefix = uri.substr(10); 212 | 213 | // split into package name and relative path 214 | std::string packageName; 215 | std::string relativePath; 216 | size_t islash = nonprefix.find('/'); 217 | if (islash == nonprefix.npos) 218 | { 219 | packageName = uri; 220 | } 221 | else 222 | { 223 | packageName = nonprefix.substr(0,islash); 224 | relativePath = nonprefix.substr(islash+1); 225 | } 226 | 227 | // Sequentially check each chained workspace. 228 | for (const Workspace& workspace : mWorkspaces) 229 | { 230 | // First check the 'devel' or 'install' space. 231 | const path develPath = path(workspace.mPath) / "share" / packageName / relativePath; 232 | if (boost::filesystem::exists(develPath)) 233 | return develPath.native(); 234 | 235 | // Next, check the source space. 236 | const auto it = workspace.mSourceMap.find(packageName); 237 | if (it != std::end(workspace.mSourceMap)) 238 | { 239 | const path sourcePath = path(it->second) / relativePath; 240 | if (boost::filesystem::exists(sourcePath)) 241 | return sourcePath.native(); 242 | } 243 | } 244 | 245 | RAVELOG_WARN("Could not find file '%s' under catkin package '%s'!\n", 246 | packageName.c_str(), relativePath.c_str()); 247 | return ""; 248 | } 249 | -------------------------------------------------------------------------------- /src/picojson.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2009-2010 Cybozu Labs, Inc. 3 | * Copyright 2011-2014 Kazuho Oku 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | #ifndef picojson_h 29 | #define picojson_h 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | // for isnan/isinf 46 | #if __cplusplus>=201103L 47 | # include 48 | #else 49 | extern "C" { 50 | # ifdef _MSC_VER 51 | # include 52 | # elif defined(__INTEL_COMPILER) 53 | # include 54 | # else 55 | # include 56 | # endif 57 | } 58 | #endif 59 | 60 | #ifndef PICOJSON_USE_RVALUE_REFERENCE 61 | # if (defined(__cpp_rvalue_references) && __cpp_rvalue_references >= 200610) || (defined(_MSC_VER) && _MSC_VER >= 1600) 62 | # define PICOJSON_USE_RVALUE_REFERENCE 1 63 | # else 64 | # define PICOJSON_USE_RVALUE_REFERENCE 0 65 | # endif 66 | #endif//PICOJSON_USE_RVALUE_REFERENCE 67 | 68 | 69 | // experimental support for int64_t (see README.mkdn for detail) 70 | #ifdef PICOJSON_USE_INT64 71 | # define __STDC_FORMAT_MACROS 72 | # include 73 | # include 74 | #endif 75 | 76 | // to disable the use of localeconv(3), set PICOJSON_USE_LOCALE to 0 77 | #ifndef PICOJSON_USE_LOCALE 78 | # define PICOJSON_USE_LOCALE 1 79 | #endif 80 | #if PICOJSON_USE_LOCALE 81 | extern "C" { 82 | # include 83 | } 84 | #endif 85 | 86 | #ifndef PICOJSON_ASSERT 87 | # define PICOJSON_ASSERT(e) do { if (! (e)) throw std::runtime_error(#e); } while (0) 88 | #endif 89 | 90 | #ifdef _MSC_VER 91 | #define SNPRINTF _snprintf_s 92 | #pragma warning(push) 93 | #pragma warning(disable : 4244) // conversion from int to char 94 | #pragma warning(disable : 4127) // conditional expression is constant 95 | #pragma warning(disable : 4702) // unreachable code 96 | #else 97 | #define SNPRINTF snprintf 98 | #endif 99 | 100 | namespace picojson { 101 | 102 | enum { 103 | null_type, 104 | boolean_type, 105 | number_type, 106 | string_type, 107 | array_type, 108 | object_type 109 | #ifdef PICOJSON_USE_INT64 110 | , int64_type 111 | #endif 112 | }; 113 | 114 | enum { 115 | INDENT_WIDTH = 2 116 | }; 117 | 118 | struct null {}; 119 | 120 | class value { 121 | public: 122 | typedef std::vector array; 123 | typedef std::map object; 124 | union _storage { 125 | bool boolean_; 126 | double number_; 127 | #ifdef PICOJSON_USE_INT64 128 | int64_t int64_; 129 | #endif 130 | std::string* string_; 131 | array* array_; 132 | object* object_; 133 | }; 134 | protected: 135 | int type_; 136 | _storage u_; 137 | public: 138 | value(); 139 | value(int type, bool); 140 | explicit value(bool b); 141 | #ifdef PICOJSON_USE_INT64 142 | explicit value(int64_t i); 143 | #endif 144 | explicit value(double n); 145 | explicit value(const std::string& s); 146 | explicit value(const array& a); 147 | explicit value(const object& o); 148 | explicit value(const char* s); 149 | value(const char* s, size_t len); 150 | ~value(); 151 | value(const value& x); 152 | value& operator=(const value& x); 153 | #if PICOJSON_USE_RVALUE_REFERENCE 154 | value(value&& x)throw(); 155 | value& operator=(value&& x)throw(); 156 | #endif 157 | void swap(value& x)throw(); 158 | template bool is() const; 159 | template const T& get() const; 160 | template T& get(); 161 | template void set(const T &); 162 | #if PICOJSON_USE_RVALUE_REFERENCE 163 | template void set(T &&); 164 | #endif 165 | bool evaluate_as_boolean() const; 166 | const value& get(size_t idx) const; 167 | const value& get(const std::string& key) const; 168 | value& get(size_t idx); 169 | value& get(const std::string& key); 170 | 171 | bool contains(size_t idx) const; 172 | bool contains(const std::string& key) const; 173 | std::string to_str() const; 174 | template void serialize(Iter os, bool prettify = false) const; 175 | std::string serialize(bool prettify = false) const; 176 | private: 177 | template value(const T*); // intentionally defined to block implicit conversion of pointer to bool 178 | template static void _indent(Iter os, int indent); 179 | template void _serialize(Iter os, int indent) const; 180 | std::string _serialize(int indent) const; 181 | void clear(); 182 | }; 183 | 184 | typedef value::array array; 185 | typedef value::object object; 186 | 187 | inline value::value() : type_(null_type) {} 188 | 189 | inline value::value(int type, bool) : type_(type) { 190 | switch (type) { 191 | #define INIT(p, v) case p##type: u_.p = v; break 192 | INIT(boolean_, false); 193 | INIT(number_, 0.0); 194 | #ifdef PICOJSON_USE_INT64 195 | INIT(int64_, 0); 196 | #endif 197 | INIT(string_, new std::string()); 198 | INIT(array_, new array()); 199 | INIT(object_, new object()); 200 | #undef INIT 201 | default: break; 202 | } 203 | } 204 | 205 | inline value::value(bool b) : type_(boolean_type) { 206 | u_.boolean_ = b; 207 | } 208 | 209 | #ifdef PICOJSON_USE_INT64 210 | inline value::value(int64_t i) : type_(int64_type) { 211 | u_.int64_ = i; 212 | } 213 | #endif 214 | 215 | inline value::value(double n) : type_(number_type) { 216 | if ( 217 | #ifdef _MSC_VER 218 | ! _finite(n) 219 | #elif __cplusplus>=201103L || !(defined(isnan) && defined(isinf)) 220 | std::isnan(n) || std::isinf(n) 221 | #else 222 | isnan(n) || isinf(n) 223 | #endif 224 | ) { 225 | throw std::overflow_error(""); 226 | } 227 | u_.number_ = n; 228 | } 229 | 230 | inline value::value(const std::string& s) : type_(string_type) { 231 | u_.string_ = new std::string(s); 232 | } 233 | 234 | inline value::value(const array& a) : type_(array_type) { 235 | u_.array_ = new array(a); 236 | } 237 | 238 | inline value::value(const object& o) : type_(object_type) { 239 | u_.object_ = new object(o); 240 | } 241 | 242 | inline value::value(const char* s) : type_(string_type) { 243 | u_.string_ = new std::string(s); 244 | } 245 | 246 | inline value::value(const char* s, size_t len) : type_(string_type) { 247 | u_.string_ = new std::string(s, len); 248 | } 249 | 250 | inline void value::clear() { 251 | switch (type_) { 252 | #define DEINIT(p) case p##type: delete u_.p; break 253 | DEINIT(string_); 254 | DEINIT(array_); 255 | DEINIT(object_); 256 | #undef DEINIT 257 | default: break; 258 | } 259 | } 260 | 261 | inline value::~value() { 262 | clear(); 263 | } 264 | 265 | inline value::value(const value& x) : type_(x.type_) { 266 | switch (type_) { 267 | #define INIT(p, v) case p##type: u_.p = v; break 268 | INIT(string_, new std::string(*x.u_.string_)); 269 | INIT(array_, new array(*x.u_.array_)); 270 | INIT(object_, new object(*x.u_.object_)); 271 | #undef INIT 272 | default: 273 | u_ = x.u_; 274 | break; 275 | } 276 | } 277 | 278 | inline value& value::operator=(const value& x) { 279 | if (this != &x) { 280 | value t(x); 281 | swap(t); 282 | } 283 | return *this; 284 | } 285 | 286 | #if PICOJSON_USE_RVALUE_REFERENCE 287 | inline value::value(value&& x)throw() : type_(null_type) { 288 | swap(x); 289 | } 290 | inline value& value::operator=(value&& x)throw() { 291 | swap(x); 292 | return *this; 293 | } 294 | #endif 295 | inline void value::swap(value& x)throw() { 296 | std::swap(type_, x.type_); 297 | std::swap(u_, x.u_); 298 | } 299 | 300 | #define IS(ctype, jtype) \ 301 | template <> inline bool value::is() const { \ 302 | return type_ == jtype##_type; \ 303 | } 304 | IS(null, null) 305 | IS(bool, boolean) 306 | #ifdef PICOJSON_USE_INT64 307 | IS(int64_t, int64) 308 | #endif 309 | IS(std::string, string) 310 | IS(array, array) 311 | IS(object, object) 312 | #undef IS 313 | template <> inline bool value::is() const { 314 | return type_ == number_type 315 | #ifdef PICOJSON_USE_INT64 316 | || type_ == int64_type 317 | #endif 318 | ; 319 | } 320 | 321 | #define GET(ctype, var) \ 322 | template <> inline const ctype& value::get() const { \ 323 | PICOJSON_ASSERT("type mismatch! call is() before get()" \ 324 | && is()); \ 325 | return var; \ 326 | } \ 327 | template <> inline ctype& value::get() { \ 328 | PICOJSON_ASSERT("type mismatch! call is() before get()" \ 329 | && is()); \ 330 | return var; \ 331 | } 332 | GET(bool, u_.boolean_) 333 | GET(std::string, *u_.string_) 334 | GET(array, *u_.array_) 335 | GET(object, *u_.object_) 336 | #ifdef PICOJSON_USE_INT64 337 | GET(double, (type_ == int64_type && (const_cast(this)->type_ = number_type, const_cast(this)->u_.number_ = u_.int64_), u_.number_)) 338 | GET(int64_t, u_.int64_) 339 | #else 340 | GET(double, u_.number_) 341 | #endif 342 | #undef GET 343 | 344 | #define SET(ctype, jtype, setter) \ 345 | template <> inline void value::set(const ctype &_val) { \ 346 | clear(); \ 347 | type_ = jtype##_type; \ 348 | setter \ 349 | } 350 | SET(bool, boolean, u_.boolean_ = _val;) 351 | SET(std::string, string, u_.string_ = new std::string(_val);) 352 | SET(array, array, u_.array_ = new array(_val);) 353 | SET(object, object, u_.object_ = new object(_val);) 354 | SET(double, number, u_.number_ = _val;) 355 | #ifdef PICOJSON_USE_INT64 356 | SET(int64_t, int64, u_.int64_ = _val;) 357 | #endif 358 | #undef SET 359 | 360 | #if PICOJSON_USE_RVALUE_REFERENCE 361 | #define MOVESET(ctype, jtype, setter) \ 362 | template <> inline void value::set(ctype &&_val) { \ 363 | clear(); \ 364 | type_ = jtype##_type; \ 365 | setter \ 366 | } 367 | MOVESET(std::string, string, u_.string_ = new std::string(std::move(_val));) 368 | MOVESET(array, array, u_.array_ = new array(std::move(_val));) 369 | MOVESET(object, object, u_.object_ = new object(std::move(_val));) 370 | #undef MOVESET 371 | #endif 372 | 373 | inline bool value::evaluate_as_boolean() const { 374 | switch (type_) { 375 | case null_type: 376 | return false; 377 | case boolean_type: 378 | return u_.boolean_; 379 | case number_type: 380 | return u_.number_ != 0; 381 | #ifdef PICOJSON_USE_INT64 382 | case int64_type: 383 | return u_.int64_ != 0; 384 | #endif 385 | case string_type: 386 | return ! u_.string_->empty(); 387 | default: 388 | return true; 389 | } 390 | } 391 | 392 | inline const value& value::get(size_t idx) const { 393 | static value s_null; 394 | PICOJSON_ASSERT(is()); 395 | return idx < u_.array_->size() ? (*u_.array_)[idx] : s_null; 396 | } 397 | 398 | inline value& value::get(size_t idx) { 399 | static value s_null; 400 | PICOJSON_ASSERT(is()); 401 | return idx < u_.array_->size() ? (*u_.array_)[idx] : s_null; 402 | } 403 | 404 | inline const value& value::get(const std::string& key) const { 405 | static value s_null; 406 | PICOJSON_ASSERT(is()); 407 | object::const_iterator i = u_.object_->find(key); 408 | return i != u_.object_->end() ? i->second : s_null; 409 | } 410 | 411 | inline value& value::get(const std::string& key) { 412 | static value s_null; 413 | PICOJSON_ASSERT(is()); 414 | object::iterator i = u_.object_->find(key); 415 | return i != u_.object_->end() ? i->second : s_null; 416 | } 417 | 418 | inline bool value::contains(size_t idx) const { 419 | PICOJSON_ASSERT(is()); 420 | return idx < u_.array_->size(); 421 | } 422 | 423 | inline bool value::contains(const std::string& key) const { 424 | PICOJSON_ASSERT(is()); 425 | object::const_iterator i = u_.object_->find(key); 426 | return i != u_.object_->end(); 427 | } 428 | 429 | inline std::string value::to_str() const { 430 | switch (type_) { 431 | case null_type: return "null"; 432 | case boolean_type: return u_.boolean_ ? "true" : "false"; 433 | #ifdef PICOJSON_USE_INT64 434 | case int64_type: { 435 | char buf[sizeof("-9223372036854775808")]; 436 | SNPRINTF(buf, sizeof(buf), "%" PRId64, u_.int64_); 437 | return buf; 438 | } 439 | #endif 440 | case number_type: { 441 | char buf[256]; 442 | double tmp; 443 | SNPRINTF(buf, sizeof(buf), fabs(u_.number_) < (1ULL << 53) && modf(u_.number_, &tmp) == 0 ? "%.f" : "%.17g", u_.number_); 444 | #if PICOJSON_USE_LOCALE 445 | char *decimal_point = localeconv()->decimal_point; 446 | if (strcmp(decimal_point, ".") != 0) { 447 | size_t decimal_point_len = strlen(decimal_point); 448 | for (char *p = buf; *p != '\0'; ++p) { 449 | if (strncmp(p, decimal_point, decimal_point_len) == 0) { 450 | return std::string(buf, p) + "." + (p + decimal_point_len); 451 | } 452 | } 453 | } 454 | #endif 455 | return buf; 456 | } 457 | case string_type: return *u_.string_; 458 | case array_type: return "array"; 459 | case object_type: return "object"; 460 | default: PICOJSON_ASSERT(0); 461 | #ifdef _MSC_VER 462 | __assume(0); 463 | #endif 464 | } 465 | return std::string(); 466 | } 467 | 468 | template void copy(const std::string& s, Iter oi) { 469 | std::copy(s.begin(), s.end(), oi); 470 | } 471 | 472 | template 473 | struct serialize_str_char { 474 | Iter oi; 475 | void operator()(char c) { 476 | switch (c) { 477 | #define MAP(val, sym) case val: copy(sym, oi); break 478 | MAP('"', "\\\""); 479 | MAP('\\', "\\\\"); 480 | MAP('/', "\\/"); 481 | MAP('\b', "\\b"); 482 | MAP('\f', "\\f"); 483 | MAP('\n', "\\n"); 484 | MAP('\r', "\\r"); 485 | MAP('\t', "\\t"); 486 | #undef MAP 487 | default: 488 | if (static_cast(c) < 0x20 || c == 0x7f) { 489 | char buf[7]; 490 | SNPRINTF(buf, sizeof(buf), "\\u%04x", c & 0xff); 491 | copy(buf, buf + 6, oi); 492 | } else { 493 | *oi++ = c; 494 | } 495 | break; 496 | } 497 | } 498 | }; 499 | 500 | template void serialize_str(const std::string& s, Iter oi) { 501 | *oi++ = '"'; 502 | serialize_str_char process_char = { oi }; 503 | std::for_each(s.begin(), s.end(), process_char); 504 | *oi++ = '"'; 505 | } 506 | 507 | template void value::serialize(Iter oi, bool prettify) const { 508 | return _serialize(oi, prettify ? 0 : -1); 509 | } 510 | 511 | inline std::string value::serialize(bool prettify) const { 512 | return _serialize(prettify ? 0 : -1); 513 | } 514 | 515 | template void value::_indent(Iter oi, int indent) { 516 | *oi++ = '\n'; 517 | for (int i = 0; i < indent * INDENT_WIDTH; ++i) { 518 | *oi++ = ' '; 519 | } 520 | } 521 | 522 | template void value::_serialize(Iter oi, int indent) const { 523 | switch (type_) { 524 | case string_type: 525 | serialize_str(*u_.string_, oi); 526 | break; 527 | case array_type: { 528 | *oi++ = '['; 529 | if (indent != -1) { 530 | ++indent; 531 | } 532 | for (array::const_iterator i = u_.array_->begin(); 533 | i != u_.array_->end(); 534 | ++i) { 535 | if (i != u_.array_->begin()) { 536 | *oi++ = ','; 537 | } 538 | if (indent != -1) { 539 | _indent(oi, indent); 540 | } 541 | i->_serialize(oi, indent); 542 | } 543 | if (indent != -1) { 544 | --indent; 545 | if (! u_.array_->empty()) { 546 | _indent(oi, indent); 547 | } 548 | } 549 | *oi++ = ']'; 550 | break; 551 | } 552 | case object_type: { 553 | *oi++ = '{'; 554 | if (indent != -1) { 555 | ++indent; 556 | } 557 | for (object::const_iterator i = u_.object_->begin(); 558 | i != u_.object_->end(); 559 | ++i) { 560 | if (i != u_.object_->begin()) { 561 | *oi++ = ','; 562 | } 563 | if (indent != -1) { 564 | _indent(oi, indent); 565 | } 566 | serialize_str(i->first, oi); 567 | *oi++ = ':'; 568 | if (indent != -1) { 569 | *oi++ = ' '; 570 | } 571 | i->second._serialize(oi, indent); 572 | } 573 | if (indent != -1) { 574 | --indent; 575 | if (! u_.object_->empty()) { 576 | _indent(oi, indent); 577 | } 578 | } 579 | *oi++ = '}'; 580 | break; 581 | } 582 | default: 583 | copy(to_str(), oi); 584 | break; 585 | } 586 | if (indent == 0) { 587 | *oi++ = '\n'; 588 | } 589 | } 590 | 591 | inline std::string value::_serialize(int indent) const { 592 | std::string s; 593 | _serialize(std::back_inserter(s), indent); 594 | return s; 595 | } 596 | 597 | template class input { 598 | protected: 599 | Iter cur_, end_; 600 | bool consumed_; 601 | int line_; 602 | public: 603 | input(const Iter& first, const Iter& last) : cur_(first), end_(last), consumed_(false), line_(1) {} 604 | int getc() { 605 | if (consumed_) { 606 | if (*cur_ == '\n') { 607 | ++line_; 608 | } 609 | ++cur_; 610 | } 611 | if (cur_ == end_) { 612 | consumed_ = false; 613 | return -1; 614 | } 615 | consumed_ = true; 616 | return *cur_ & 0xff; 617 | } 618 | void ungetc() { 619 | consumed_ = false; 620 | } 621 | Iter cur() const { 622 | if (consumed_) { 623 | input *self = const_cast*>(this); 624 | self->consumed_ = false; 625 | ++self->cur_; 626 | } 627 | return cur_; 628 | } 629 | int line() const { return line_; } 630 | void skip_ws() { 631 | while (1) { 632 | int ch = getc(); 633 | if (! (ch == ' ' || ch == '\t' || ch == '\n' || ch == '\r')) { 634 | ungetc(); 635 | break; 636 | } 637 | } 638 | } 639 | bool expect(int expect) { 640 | skip_ws(); 641 | if (getc() != expect) { 642 | ungetc(); 643 | return false; 644 | } 645 | return true; 646 | } 647 | bool match(const std::string& pattern) { 648 | for (std::string::const_iterator pi(pattern.begin()); 649 | pi != pattern.end(); 650 | ++pi) { 651 | if (getc() != *pi) { 652 | ungetc(); 653 | return false; 654 | } 655 | } 656 | return true; 657 | } 658 | }; 659 | 660 | template inline int _parse_quadhex(input &in) { 661 | int uni_ch = 0, hex; 662 | for (int i = 0; i < 4; i++) { 663 | if ((hex = in.getc()) == -1) { 664 | return -1; 665 | } 666 | if ('0' <= hex && hex <= '9') { 667 | hex -= '0'; 668 | } else if ('A' <= hex && hex <= 'F') { 669 | hex -= 'A' - 0xa; 670 | } else if ('a' <= hex && hex <= 'f') { 671 | hex -= 'a' - 0xa; 672 | } else { 673 | in.ungetc(); 674 | return -1; 675 | } 676 | uni_ch = uni_ch * 16 + hex; 677 | } 678 | return uni_ch; 679 | } 680 | 681 | template inline bool _parse_codepoint(String& out, input& in) { 682 | int uni_ch; 683 | if ((uni_ch = _parse_quadhex(in)) == -1) { 684 | return false; 685 | } 686 | if (0xd800 <= uni_ch && uni_ch <= 0xdfff) { 687 | if (0xdc00 <= uni_ch) { 688 | // a second 16-bit of a surrogate pair appeared 689 | return false; 690 | } 691 | // first 16-bit of surrogate pair, get the next one 692 | if (in.getc() != '\\' || in.getc() != 'u') { 693 | in.ungetc(); 694 | return false; 695 | } 696 | int second = _parse_quadhex(in); 697 | if (! (0xdc00 <= second && second <= 0xdfff)) { 698 | return false; 699 | } 700 | uni_ch = ((uni_ch - 0xd800) << 10) | ((second - 0xdc00) & 0x3ff); 701 | uni_ch += 0x10000; 702 | } 703 | if (uni_ch < 0x80) { 704 | out.push_back(uni_ch); 705 | } else { 706 | if (uni_ch < 0x800) { 707 | out.push_back(0xc0 | (uni_ch >> 6)); 708 | } else { 709 | if (uni_ch < 0x10000) { 710 | out.push_back(0xe0 | (uni_ch >> 12)); 711 | } else { 712 | out.push_back(0xf0 | (uni_ch >> 18)); 713 | out.push_back(0x80 | ((uni_ch >> 12) & 0x3f)); 714 | } 715 | out.push_back(0x80 | ((uni_ch >> 6) & 0x3f)); 716 | } 717 | out.push_back(0x80 | (uni_ch & 0x3f)); 718 | } 719 | return true; 720 | } 721 | 722 | template inline bool _parse_string(String& out, input& in) { 723 | while (1) { 724 | int ch = in.getc(); 725 | if (ch < ' ') { 726 | in.ungetc(); 727 | return false; 728 | } else if (ch == '"') { 729 | return true; 730 | } else if (ch == '\\') { 731 | if ((ch = in.getc()) == -1) { 732 | return false; 733 | } 734 | switch (ch) { 735 | #define MAP(sym, val) case sym: out.push_back(val); break 736 | MAP('"', '\"'); 737 | MAP('\\', '\\'); 738 | MAP('/', '/'); 739 | MAP('b', '\b'); 740 | MAP('f', '\f'); 741 | MAP('n', '\n'); 742 | MAP('r', '\r'); 743 | MAP('t', '\t'); 744 | #undef MAP 745 | case 'u': 746 | if (! _parse_codepoint(out, in)) { 747 | return false; 748 | } 749 | break; 750 | default: 751 | return false; 752 | } 753 | } else { 754 | out.push_back(ch); 755 | } 756 | } 757 | return false; 758 | } 759 | 760 | template inline bool _parse_array(Context& ctx, input& in) { 761 | if (! ctx.parse_array_start()) { 762 | return false; 763 | } 764 | size_t idx = 0; 765 | if (in.expect(']')) { 766 | return ctx.parse_array_stop(idx); 767 | } 768 | do { 769 | if (! ctx.parse_array_item(in, idx)) { 770 | return false; 771 | } 772 | idx++; 773 | } while (in.expect(',')); 774 | return in.expect(']') && ctx.parse_array_stop(idx); 775 | } 776 | 777 | template inline bool _parse_object(Context& ctx, input& in) { 778 | if (! ctx.parse_object_start()) { 779 | return false; 780 | } 781 | if (in.expect('}')) { 782 | return true; 783 | } 784 | do { 785 | std::string key; 786 | if (! in.expect('"') 787 | || ! _parse_string(key, in) 788 | || ! in.expect(':')) { 789 | return false; 790 | } 791 | if (! ctx.parse_object_item(in, key)) { 792 | return false; 793 | } 794 | } while (in.expect(',')); 795 | return in.expect('}'); 796 | } 797 | 798 | template inline std::string _parse_number(input& in) { 799 | std::string num_str; 800 | while (1) { 801 | int ch = in.getc(); 802 | if (('0' <= ch && ch <= '9') || ch == '+' || ch == '-' 803 | || ch == 'e' || ch == 'E') { 804 | num_str.push_back(ch); 805 | } else if (ch == '.') { 806 | #if PICOJSON_USE_LOCALE 807 | num_str += localeconv()->decimal_point; 808 | #else 809 | num_str.push_back('.'); 810 | #endif 811 | } else { 812 | in.ungetc(); 813 | break; 814 | } 815 | } 816 | return num_str; 817 | } 818 | 819 | template inline bool _parse(Context& ctx, input& in) { 820 | in.skip_ws(); 821 | int ch = in.getc(); 822 | switch (ch) { 823 | #define IS(ch, text, op) case ch: \ 824 | if (in.match(text) && op) { \ 825 | return true; \ 826 | } else { \ 827 | return false; \ 828 | } 829 | IS('n', "ull", ctx.set_null()); 830 | IS('f', "alse", ctx.set_bool(false)); 831 | IS('t', "rue", ctx.set_bool(true)); 832 | #undef IS 833 | case '"': 834 | return ctx.parse_string(in); 835 | case '[': 836 | return _parse_array(ctx, in); 837 | case '{': 838 | return _parse_object(ctx, in); 839 | default: 840 | if (('0' <= ch && ch <= '9') || ch == '-') { 841 | double f; 842 | char *endp; 843 | in.ungetc(); 844 | std::string num_str = _parse_number(in); 845 | if (num_str.empty()) { 846 | return false; 847 | } 848 | #ifdef PICOJSON_USE_INT64 849 | { 850 | errno = 0; 851 | intmax_t ival = strtoimax(num_str.c_str(), &endp, 10); 852 | if (errno == 0 853 | && std::numeric_limits::min() <= ival 854 | && ival <= std::numeric_limits::max() 855 | && endp == num_str.c_str() + num_str.size()) { 856 | ctx.set_int64(ival); 857 | return true; 858 | } 859 | } 860 | #endif 861 | f = strtod(num_str.c_str(), &endp); 862 | if (endp == num_str.c_str() + num_str.size()) { 863 | ctx.set_number(f); 864 | return true; 865 | } 866 | return false; 867 | } 868 | break; 869 | } 870 | in.ungetc(); 871 | return false; 872 | } 873 | 874 | class deny_parse_context { 875 | public: 876 | bool set_null() { return false; } 877 | bool set_bool(bool) { return false; } 878 | #ifdef PICOJSON_USE_INT64 879 | bool set_int64(int64_t) { return false; } 880 | #endif 881 | bool set_number(double) { return false; } 882 | template bool parse_string(input&) { return false; } 883 | bool parse_array_start() { return false; } 884 | template bool parse_array_item(input&, size_t) { 885 | return false; 886 | } 887 | bool parse_array_stop(size_t) { return false; } 888 | bool parse_object_start() { return false; } 889 | template bool parse_object_item(input&, const std::string&) { 890 | return false; 891 | } 892 | }; 893 | 894 | class default_parse_context { 895 | protected: 896 | value* out_; 897 | public: 898 | default_parse_context(value* out) : out_(out) {} 899 | bool set_null() { 900 | *out_ = value(); 901 | return true; 902 | } 903 | bool set_bool(bool b) { 904 | *out_ = value(b); 905 | return true; 906 | } 907 | #ifdef PICOJSON_USE_INT64 908 | bool set_int64(int64_t i) { 909 | *out_ = value(i); 910 | return true; 911 | } 912 | #endif 913 | bool set_number(double f) { 914 | *out_ = value(f); 915 | return true; 916 | } 917 | template bool parse_string(input& in) { 918 | *out_ = value(string_type, false); 919 | return _parse_string(out_->get(), in); 920 | } 921 | bool parse_array_start() { 922 | *out_ = value(array_type, false); 923 | return true; 924 | } 925 | template bool parse_array_item(input& in, size_t) { 926 | array& a = out_->get(); 927 | a.push_back(value()); 928 | default_parse_context ctx(&a.back()); 929 | return _parse(ctx, in); 930 | } 931 | bool parse_array_stop(size_t) { return true; } 932 | bool parse_object_start() { 933 | *out_ = value(object_type, false); 934 | return true; 935 | } 936 | template bool parse_object_item(input& in, const std::string& key) { 937 | object& o = out_->get(); 938 | default_parse_context ctx(&o[key]); 939 | return _parse(ctx, in); 940 | } 941 | private: 942 | default_parse_context(const default_parse_context&); 943 | default_parse_context& operator=(const default_parse_context&); 944 | }; 945 | 946 | class null_parse_context { 947 | public: 948 | struct dummy_str { 949 | void push_back(int) {} 950 | }; 951 | public: 952 | null_parse_context() {} 953 | bool set_null() { return true; } 954 | bool set_bool(bool) { return true; } 955 | #ifdef PICOJSON_USE_INT64 956 | bool set_int64(int64_t) { return true; } 957 | #endif 958 | bool set_number(double) { return true; } 959 | template bool parse_string(input& in) { 960 | dummy_str s; 961 | return _parse_string(s, in); 962 | } 963 | bool parse_array_start() { return true; } 964 | template bool parse_array_item(input& in, size_t) { 965 | return _parse(*this, in); 966 | } 967 | bool parse_array_stop(size_t) { return true; } 968 | bool parse_object_start() { return true; } 969 | template bool parse_object_item(input& in, const std::string&) { 970 | return _parse(*this, in); 971 | } 972 | private: 973 | null_parse_context(const null_parse_context&); 974 | null_parse_context& operator=(const null_parse_context&); 975 | }; 976 | 977 | // obsolete, use the version below 978 | template inline std::string parse(value& out, Iter& pos, const Iter& last) { 979 | std::string err; 980 | pos = parse(out, pos, last, &err); 981 | return err; 982 | } 983 | 984 | template inline Iter _parse(Context& ctx, const Iter& first, const Iter& last, std::string* err) { 985 | input in(first, last); 986 | if (! _parse(ctx, in) && err != NULL) { 987 | char buf[64]; 988 | SNPRINTF(buf, sizeof(buf), "syntax error at line %d near: ", in.line()); 989 | *err = buf; 990 | while (1) { 991 | int ch = in.getc(); 992 | if (ch == -1 || ch == '\n') { 993 | break; 994 | } else if (ch >= ' ') { 995 | err->push_back(ch); 996 | } 997 | } 998 | } 999 | return in.cur(); 1000 | } 1001 | 1002 | template inline Iter parse(value& out, const Iter& first, const Iter& last, std::string* err) { 1003 | default_parse_context ctx(&out); 1004 | return _parse(ctx, first, last, err); 1005 | } 1006 | 1007 | inline std::string parse(value& out, const std::string& s) { 1008 | std::string err; 1009 | parse(out, s.begin(), s.end(), &err); 1010 | return err; 1011 | } 1012 | 1013 | inline std::string parse(value& out, std::istream& is) { 1014 | std::string err; 1015 | parse(out, std::istreambuf_iterator(is.rdbuf()), 1016 | std::istreambuf_iterator(), &err); 1017 | return err; 1018 | } 1019 | 1020 | template struct last_error_t { 1021 | static std::string s; 1022 | }; 1023 | template std::string last_error_t::s; 1024 | 1025 | inline void set_last_error(const std::string& s) { 1026 | last_error_t::s = s; 1027 | } 1028 | 1029 | inline const std::string& get_last_error() { 1030 | return last_error_t::s; 1031 | } 1032 | 1033 | inline bool operator==(const value& x, const value& y) { 1034 | if (x.is()) 1035 | return y.is(); 1036 | #define PICOJSON_CMP(type) \ 1037 | if (x.is()) \ 1038 | return y.is() && x.get() == y.get() 1039 | PICOJSON_CMP(bool); 1040 | PICOJSON_CMP(double); 1041 | PICOJSON_CMP(std::string); 1042 | PICOJSON_CMP(array); 1043 | PICOJSON_CMP(object); 1044 | #undef PICOJSON_CMP 1045 | PICOJSON_ASSERT(0); 1046 | #ifdef _MSC_VER 1047 | __assume(0); 1048 | #endif 1049 | return false; 1050 | } 1051 | 1052 | inline bool operator!=(const value& x, const value& y) { 1053 | return ! (x == y); 1054 | } 1055 | } 1056 | 1057 | #if !PICOJSON_USE_RVALUE_REFERENCE 1058 | namespace std { 1059 | template<> inline void swap(picojson::value& x, picojson::value& y) 1060 | { 1061 | x.swap(y); 1062 | } 1063 | } 1064 | #endif 1065 | 1066 | inline std::istream& operator>>(std::istream& is, picojson::value& x) 1067 | { 1068 | picojson::set_last_error(std::string()); 1069 | std::string err = picojson::parse(x, is); 1070 | if (! err.empty()) { 1071 | picojson::set_last_error(err); 1072 | is.setstate(std::ios::failbit); 1073 | } 1074 | return is; 1075 | } 1076 | 1077 | inline std::ostream& operator<<(std::ostream& os, const picojson::value& x) 1078 | { 1079 | x.serialize(std::ostream_iterator(os)); 1080 | return os; 1081 | } 1082 | #ifdef _MSC_VER 1083 | #pragma warning(pop) 1084 | #endif 1085 | 1086 | #endif 1087 | -------------------------------------------------------------------------------- /src/urdf_loader.cpp: -------------------------------------------------------------------------------- 1 | /** \file urdf_loader.cpp 2 | * \brief Implementation of a URDF loading plugin for OpenRAVE 3 | * \author Michael Koval 4 | * \date 2013 5 | */ 6 | #include "urdf_loader.h" 7 | #include "boostfs_helpers.h" 8 | #include "picojson.h" 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | OpenRAVE::InterfaceBasePtr CreateInterfaceValidated( 21 | OpenRAVE::InterfaceType type, const std::string& interfacename, 22 | std::istream& sinput, OpenRAVE::EnvironmentBasePtr env) 23 | { 24 | if (type == OpenRAVE::PT_Module && interfacename == "urdf") { 25 | return OpenRAVE::InterfaceBasePtr(new or_urdf::URDFLoader(env)); 26 | } else { 27 | return OpenRAVE::InterfaceBasePtr(); 28 | } 29 | } 30 | 31 | void GetURDFRootLinks( 32 | std::vector > const &links, 33 | std::vector > *roots) 34 | { 35 | typedef boost::shared_ptr LinkConstPtr; 36 | 37 | BOOST_ASSERT(roots); 38 | std::set const links_set(links.begin(), links.end()); 39 | 40 | BOOST_FOREACH (LinkConstPtr link, links) { 41 | LinkConstPtr parent_link = link->getParent(); 42 | if (links_set.count(parent_link) == 0) { 43 | roots->push_back(link); 44 | } 45 | } 46 | } 47 | 48 | srdf::Model::Group const &GetSRDFGroup( 49 | std::map const &groups, 50 | std::string const &name) 51 | { 52 | BOOST_AUTO(it, groups.find(name)); 53 | if (it == groups.end()) { 54 | throw std::runtime_error(boost::str( 55 | boost::format("Group '%s' does not exist.") % name)); 56 | } 57 | return it->second; 58 | } 59 | 60 | void ExtractSRDFGroup(urdf::Model const &urdf, srdf::Model::Group const &group, 61 | std::vector > *links, 62 | std::vector > *joints) 63 | { 64 | typedef boost::shared_ptr LinkConstPtr; 65 | typedef boost::shared_ptr JointConstPtr; 66 | 67 | BOOST_ASSERT(links); 68 | BOOST_ASSERT(joints); 69 | 70 | if (!group.chains_.empty()) { 71 | std::string base_link_name, tip_link_name; 72 | BOOST_FOREACH (boost::tie(base_link_name, tip_link_name), group.chains_) { 73 | // Crawl the kinematic chain from bottom-up. 74 | LinkConstPtr base_link = urdf.getLink(base_link_name); 75 | LinkConstPtr tip_link = urdf.getLink(tip_link_name); 76 | LinkConstPtr link_ptr = tip_link; 77 | BOOST_ASSERT(base_link); 78 | BOOST_ASSERT(tip_link); 79 | 80 | do { 81 | JointConstPtr parent_joint = link_ptr->parent_joint; 82 | links->push_back(link_ptr); 83 | joints->push_back(parent_joint); 84 | link_ptr = link_ptr->getParent(); 85 | } while (link_ptr && link_ptr != base_link); 86 | 87 | if (!link_ptr) { 88 | throw std::runtime_error(boost::str( 89 | boost::format("Invalid chain in group '%s':" 90 | " Link '%s' is not a parent of '%s'.") 91 | % group.name_ % base_link_name % tip_link_name)); 92 | } 93 | 94 | links->push_back(link_ptr); 95 | } 96 | } 97 | 98 | if (!group.joints_.empty()) { 99 | // Joints implicitly include their child link. 100 | BOOST_FOREACH (std::string const &joint_name, group.joints_) { 101 | JointConstPtr joint = urdf.getJoint(joint_name); 102 | LinkConstPtr child_link = urdf.getLink(joint->child_link_name); 103 | links->push_back(child_link); 104 | joints->push_back(joint); 105 | } 106 | } 107 | 108 | if (!group.links_.empty()) { 109 | // Links implicitly include their parent joint. 110 | BOOST_FOREACH (std::string const &link_name, group.links_) { 111 | LinkConstPtr link = urdf.getLink(link_name); 112 | JointConstPtr parent_joint = link->parent_joint; 113 | 114 | links->push_back(link); 115 | if (parent_joint) { 116 | joints->push_back(parent_joint); 117 | } 118 | } 119 | } 120 | } 121 | 122 | void GetPluginAttributesValidated(OpenRAVE::PLUGININFO& info) 123 | { 124 | info.interfacenames[OpenRAVE::PT_Module].push_back("URDF"); 125 | } 126 | 127 | void DestroyPlugin() 128 | { 129 | } 130 | 131 | namespace or_urdf { 132 | 133 | template 134 | std::vector > MakeConst( 135 | std::vector > const &vconst) 136 | { 137 | std::vector > v; 138 | v.reserve(vconst.size()); 139 | 140 | BOOST_FOREACH (boost::shared_ptr const &x, vconst) { 141 | v.push_back(x); 142 | } 143 | return v; 144 | } 145 | 146 | /** Converts from URDF 3D vector to OpenRAVE 3D vector. */ 147 | OpenRAVE::Vector URDFVectorToRaveVector(const urdf::Vector3 &vector) 148 | { 149 | return OpenRAVE::Vector(vector.x, vector.y, vector.z); 150 | } 151 | 152 | /** Converts from URDF 3D rotation to OpenRAVE 3D vector. */ 153 | OpenRAVE::Vector URDFRotationToRaveVector(const urdf::Rotation &rotation) 154 | { 155 | return OpenRAVE::Vector(rotation.w, rotation.x, rotation.y, rotation.z); 156 | } 157 | 158 | OpenRAVE::Vector URDFColorToRaveVector(const urdf::Color &color) 159 | { 160 | return OpenRAVE::Vector(color.r, color.g, color.b, color.a); 161 | } 162 | 163 | OpenRAVE::Transform URDFPoseToRaveTransform(const urdf::Pose &pose) 164 | { 165 | return OpenRAVE::Transform(URDFRotationToRaveVector(pose.rotation), 166 | URDFVectorToRaveVector(pose.position)); 167 | } 168 | 169 | /** Converts URDF joint to an OpenRAVE joint string and a boolean 170 | representing whether the joint is moving or fixed */ 171 | std::pair URDFJointTypeToRaveJointType(int type) 172 | { 173 | switch(type) { 174 | case urdf::Joint::REVOLUTE: 175 | return std::make_pair(OpenRAVE::KinBody::JointRevolute, true); 176 | 177 | case urdf::Joint::PRISMATIC: 178 | return std::make_pair(OpenRAVE::KinBody::JointSlider, true); 179 | 180 | case urdf::Joint::FIXED: 181 | return std::make_pair(OpenRAVE::KinBody::JointHinge, false); 182 | 183 | case urdf::Joint::CONTINUOUS: 184 | return std::make_pair(OpenRAVE::KinBody::JointHinge, true); 185 | 186 | // TODO: Fill the rest of these joint types in! 187 | case urdf::Joint::PLANAR: 188 | throw std::runtime_error("Planar joints are not supported."); 189 | 190 | case urdf::Joint::FLOATING: 191 | throw std::runtime_error("Floating joints are not supported."); 192 | 193 | case urdf::Joint::UNKNOWN: 194 | throw std::runtime_error("Unknown joints are not supported."); 195 | 196 | default: 197 | throw std::runtime_error(boost::str( 198 | boost::format("Unkonwn type of joint %d.") % type)); 199 | } 200 | } 201 | 202 | void URDFLoader::ParseURDF( 203 | urdf::Model &model, 204 | std::vector &link_infos, 205 | std::vector &joint_infos) 206 | { 207 | // Populate list of links from URDF model. We'll force the root link to be 208 | // first. 209 | std::vector > link_vector; 210 | model.getLinks(link_vector); 211 | 212 | std::list > link_list; 213 | std::set finished_links; 214 | 215 | link_list.insert(link_list.begin(), model.getRoot()); 216 | BOOST_FOREACH (boost::shared_ptr link, link_vector) { 217 | if (link != model.getRoot()) { 218 | link_list.insert(link_list.end(), link); 219 | } 220 | } 221 | 222 | // TODO: prevent infinite loops here 223 | // Iterate through all links, allowing deferred evaluation (putting links 224 | // back on the list) if their parents do not exist yet 225 | boost::shared_ptr link_ptr; 226 | 227 | while (!link_list.empty()) { 228 | // Get next element in list 229 | link_ptr = link_list.front(); 230 | link_list.pop_front(); 231 | 232 | OpenRAVE::KinBody::LinkInfoPtr link_info 233 | = boost::make_shared(); 234 | 235 | // TODO: Set "type" to "dynamic". 236 | link_info->_name = link_ptr->name; 237 | 238 | // Set inertial parameters 239 | boost::shared_ptr inertial = link_ptr->inertial; 240 | if (inertial) { 241 | // XXX: We should also specify the off-diagonal terms (ixy, iyz, ixz) 242 | // of the inertia tensor. We can do this in KinBody XML files, but I 243 | // cannot figure out how to do so through this API. 244 | link_info->_mass = inertial->mass; 245 | link_info->_tMassFrame = URDFPoseToRaveTransform(inertial->origin); 246 | link_info->_vinertiamoments = OpenRAVE::Vector( 247 | inertial->ixx, inertial->iyy, inertial->izz); 248 | } 249 | 250 | // Set local transformation to be same as parent joint 251 | boost::shared_ptr parent_joint = link_ptr->parent_joint; 252 | while (parent_joint) { 253 | link_info->_t = URDFPoseToRaveTransform( 254 | parent_joint->parent_to_joint_origin_transform) * link_info->_t; 255 | boost::shared_ptr parent_link 256 | = model.getLink(parent_joint->parent_link_name); 257 | parent_joint = parent_link->parent_joint; 258 | } 259 | 260 | // Set information for collision geometry 261 | BOOST_FOREACH (boost::shared_ptr collision, 262 | link_ptr->collision_array) { 263 | OpenRAVE::KinBody::GeometryInfoPtr geom_info 264 | = boost::make_shared(); 265 | 266 | // TODO: Shouldn't we apply this transform? 267 | geom_info->_t = URDFPoseToRaveTransform(collision->origin); 268 | geom_info->_bVisible = false; 269 | geom_info->_bModifiable = false; 270 | 271 | switch (collision->geometry->type) { 272 | case urdf::Geometry::MESH: { 273 | urdf::Mesh const &mesh 274 | = dynamic_cast(*collision->geometry); 275 | geom_info->_filenamecollision = resolveURI(mesh.filename); 276 | geom_info->_type = OpenRAVE::GT_TriMesh; 277 | 278 | // This doesn't seem to do anything, but we'll set it anyway. 279 | geom_info->_vCollisionScale = URDFVectorToRaveVector(mesh.scale); 280 | 281 | boost::shared_ptr trimesh 282 | = boost::make_shared(); 283 | trimesh = GetEnv()->ReadTrimeshURI(trimesh, 284 | geom_info->_filenamecollision); 285 | if (trimesh) { 286 | // The _vCollisionScale property does nothing, so we have to 287 | // manually scale the mesh. 288 | BOOST_FOREACH(OpenRAVE::Vector &vertex, trimesh->vertices) { 289 | vertex *= geom_info->_vCollisionScale; 290 | } 291 | 292 | geom_info->_meshcollision = *trimesh; 293 | } else { 294 | RAVELOG_WARN("Link[%s]: Failed loading collision mesh %s\n", 295 | link_ptr->name.c_str(), geom_info->_filenamecollision.c_str()); 296 | } 297 | break; 298 | } 299 | 300 | case urdf::Geometry::SPHERE: { 301 | RAVELOG_WARN("Creating sphere!\n"); 302 | urdf::Sphere const &sphere 303 | = dynamic_cast(*collision->geometry); 304 | geom_info->_vGeomData = sphere.radius * OpenRAVE::Vector(1, 1, 1); 305 | geom_info->_type = OpenRAVE::GT_Sphere; 306 | break; 307 | } 308 | 309 | case urdf::Geometry::BOX: { 310 | urdf::Box const &box 311 | = dynamic_cast(*collision->geometry); 312 | geom_info->_vGeomData = 0.5 * OpenRAVE::Vector(box.dim.x, box.dim.y, 313 | box.dim.z); 314 | geom_info->_type = OpenRAVE::GT_Box; 315 | break; 316 | } 317 | 318 | case urdf::Geometry::CYLINDER: { 319 | urdf::Cylinder const &cylinder 320 | = dynamic_cast(*collision->geometry); 321 | geom_info->_vGeomData = OpenRAVE::Vector(cylinder.radius, 322 | cylinder.length, 0); 323 | geom_info->_type = OpenRAVE::GT_Cylinder; 324 | break; 325 | } 326 | } 327 | 328 | link_info->_vgeometryinfos.push_back(geom_info); 329 | } 330 | 331 | // Add the render geometry. We can't create a link with no collision 332 | // geometry, so we'll instead create a zero-radius sphere with the 333 | // desired render mesh. 334 | // TODO: Why does GT_None crash OpenRAVE? 335 | boost::shared_ptr visual = link_ptr->visual; 336 | if (visual) { 337 | OpenRAVE::KinBody::GeometryInfoPtr geom_info 338 | = boost::make_shared(); 339 | geom_info->_t = URDFPoseToRaveTransform(visual->origin); 340 | geom_info->_type = OpenRAVE::GT_Sphere; 341 | geom_info->_vGeomData = OpenRAVE::Vector(0.0, 0.0, 0.0); 342 | geom_info->_bModifiable = false; 343 | geom_info->_bVisible = true; 344 | 345 | switch (visual->geometry->type) { 346 | case urdf::Geometry::MESH: { 347 | const urdf::Mesh &mesh = dynamic_cast(*visual->geometry); 348 | geom_info->_filenamerender = resolveURI(mesh.filename); 349 | geom_info->_vRenderScale = URDFVectorToRaveVector(mesh.scale); 350 | 351 | // If a material color is specified, use it. 352 | boost::shared_ptr material = visual->material; 353 | if (material) { 354 | geom_info->_vDiffuseColor = URDFColorToRaveVector(material->color); 355 | geom_info->_vAmbientColor = URDFColorToRaveVector(material->color); 356 | } 357 | 358 | // Add the render-only geometry to the standard geometry group for 359 | // backwards compatability with QtCoin. 360 | link_info->_vgeometryinfos.push_back(geom_info); 361 | 362 | // Create a group dedicated to visual geometry for or_rviz. 363 | OpenRAVE::KinBody::GeometryInfoPtr geom_info_clone 364 | = boost::make_shared(*geom_info); 365 | link_info->_mapExtraGeometries["visual"].push_back(geom_info_clone); 366 | break; 367 | } 368 | 369 | default: 370 | RAVELOG_WARN("Link[%s]: Only trimeshes are supported for visual geometry.\n", link_ptr->name.c_str()); 371 | } 372 | } 373 | 374 | // Verify that the "visual" and "spheres" groups always exist. Recall 375 | // that accessing an element with operator[] creates it using the default 376 | // no-arg constructor if it does not already exist. 377 | link_info->_mapExtraGeometries["visual"]; 378 | link_info->_mapExtraGeometries["spheres"]; 379 | 380 | link_infos.push_back(link_info); 381 | } 382 | 383 | // Populate vector of joints 384 | std::string joint_name; 385 | boost::shared_ptr joint_ptr; 386 | 387 | // Parse the joint properties 388 | std::vector > ordered_joints; 389 | BOOST_FOREACH(boost::tie(joint_name, joint_ptr), model.joints_) { 390 | ordered_joints.push_back(joint_ptr); 391 | } 392 | 393 | BOOST_FOREACH(boost::shared_ptr joint_ptr, ordered_joints) { 394 | OpenRAVE::KinBody::JointInfoPtr joint_info = boost::make_shared(); 395 | joint_info->_name = joint_ptr->name; 396 | joint_info->_linkname0 = joint_ptr->parent_link_name; 397 | joint_info->_linkname1 = joint_ptr->child_link_name; 398 | joint_info->_vanchor = URDFVectorToRaveVector(joint_ptr->parent_to_joint_origin_transform.position); 399 | 400 | int const urdf_joint_type = joint_ptr->type; 401 | 402 | // Set the joint type. Some URDF joints correspond to disabled OpenRAVE 403 | // joints, so we'll appropriately set the corresponding IsActive flag. 404 | OpenRAVE::KinBody::JointType joint_type; 405 | bool enabled; 406 | boost::tie(joint_type, enabled) = URDFJointTypeToRaveJointType(urdf_joint_type); 407 | joint_info->_type = joint_type; 408 | joint_info->_bIsActive = enabled; 409 | joint_info->_bIsCircular[0] = (urdf_joint_type == urdf::Joint::CONTINUOUS); 410 | 411 | // URDF only supports linear mimic joints with a constant offset. We map 412 | // that into the correct position (index 0) and velocity (index 1) 413 | // equations for OpenRAVE. 414 | boost::shared_ptr mimic = joint_ptr->mimic; 415 | if (mimic) { 416 | joint_info->_vmimic[0] = boost::make_shared(); 417 | joint_info->_vmimic[0]->_equations[0] = boost::str(boost::format("%s*%0.6f+%0.6f") 418 | % mimic->joint_name % mimic->multiplier % mimic->offset); 419 | joint_info->_vmimic[0]->_equations[1] = boost::str(boost::format("|%s %0.6f") 420 | % mimic->joint_name % mimic->multiplier); 421 | } 422 | 423 | // Configure joint axis. Add an arbitrary axis if the joint is disabled. 424 | urdf::Vector3 joint_axis; 425 | if (enabled) { 426 | joint_axis = joint_ptr->parent_to_joint_origin_transform.rotation * joint_ptr->axis; 427 | } else { 428 | joint_axis = urdf::Vector3(1, 0, 0); 429 | } 430 | joint_info->_vaxes[0] = URDFVectorToRaveVector(joint_axis); 431 | 432 | // Configure joint limits. 433 | boost::shared_ptr limits = joint_ptr->limits; 434 | if (limits) { 435 | // TODO: What about acceleration? 436 | joint_info->_vlowerlimit[0] = limits->lower; 437 | joint_info->_vupperlimit[0] = limits->upper; 438 | joint_info->_vmaxvel[0] = limits->velocity; 439 | joint_info->_vmaxtorque[0] = limits->effort; 440 | } 441 | // Fixed joints are just revolute joints with zero limits. 442 | else if (!enabled) { 443 | joint_info->_vlowerlimit[0] = 0; 444 | joint_info->_vupperlimit[0] = 0; 445 | } 446 | // Default to +/- 2*PI. This is the same default used by OpenRAVE for 447 | // revolute joints. 448 | else { 449 | joint_info->_vlowerlimit[0] = -M_PI; 450 | joint_info->_vupperlimit[0] = M_PI; 451 | } 452 | 453 | // Force continuous joints to have +/- PI limits. Otherwise, the internal 454 | // _vcircularlowerlimit and _vcircularupperlimit values will be set to 455 | // zero. This causes OpenRAVE::utils::NormalizeCircularAngle to crash. 456 | if (urdf_joint_type == urdf::Joint::CONTINUOUS) { 457 | joint_info->_vlowerlimit[0] = -M_PI; 458 | joint_info->_vupperlimit[0] = M_PI; 459 | } 460 | 461 | joint_infos.push_back(joint_info); 462 | } 463 | } 464 | 465 | void URDFLoader::ParseSRDF(urdf::Model const &urdf, srdf::Model const &srdf, 466 | std::vector &link_infos, 467 | std::vector &joint_infos, 468 | std::vector &manip_infos) 469 | { 470 | std::map link_map; 471 | BOOST_FOREACH (OpenRAVE::KinBody::LinkInfoPtr link_info, link_infos) { 472 | link_map[link_info->_name] = link_info; 473 | } 474 | 475 | // Link adjacencies. 476 | size_t num_adjacent = 0; 477 | BOOST_FOREACH (srdf::Model::DisabledCollision const &link_pair, 478 | srdf.getDisabledCollisionPairs()) { 479 | std::string const &link1_name = link_pair.link1_; 480 | OpenRAVE::KinBody::LinkInfoPtr link1_info = link_map[link1_name]; 481 | if (!link1_info) { 482 | throw OPENRAVE_EXCEPTION_FORMAT("There is no link named %s.", link1_name.c_str(), 483 | OpenRAVE::ORE_Failed); 484 | } 485 | 486 | std::string const &link2_name = link_pair.link2_; 487 | OpenRAVE::KinBody::LinkInfoPtr link2_info = link_map[link2_name]; 488 | if (!link2_info) { 489 | throw OPENRAVE_EXCEPTION_FORMAT("There is no link named %s.", link2_name.c_str(), 490 | OpenRAVE::ORE_Failed); 491 | } 492 | 493 | link1_info->_vForcedAdjacentLinks.push_back(link2_name); 494 | link2_info->_vForcedAdjacentLinks.push_back(link1_name); 495 | num_adjacent++; 496 | } 497 | RAVELOG_DEBUG("Disabled collisions between %d pairs of joints.\n", 498 | num_adjacent); 499 | 500 | // TODO: Passive joints. 501 | 502 | // Build an index of the SRDF groups. This is necessary because the srdf 503 | // package provides very low-level access to the file's contents. 504 | std::vector const &raw_groups = srdf.getGroups(); 505 | std::map groups; 506 | 507 | BOOST_FOREACH(srdf::Model::Group const &group, raw_groups) { 508 | BOOST_AUTO(result, groups.insert(std::make_pair(group.name_, group))); 509 | if (!result.second) { 510 | throw std::runtime_error(boost::str( 511 | boost::format("Duplicate SRDF group '%s'.") % group.name_)); 512 | } 513 | } 514 | 515 | // Create manipulators. 516 | BOOST_FOREACH (srdf::Model::EndEffector const &end_effector, srdf.getEndEffectors()) { 517 | typedef boost::shared_ptr LinkConstPtr; 518 | typedef boost::shared_ptr JointConstPtr; 519 | 520 | // Get the end-effector group its associated mainpulator. We assume 521 | // that the parent group of an end-effector is a manipulator. 522 | RAVELOG_DEBUG("Loading '%s' end-effector group '%s'.\n", 523 | end_effector.name_.c_str(), 524 | end_effector.component_group_.c_str()); 525 | srdf::Model::Group const &ee_group = GetSRDFGroup(groups, end_effector.component_group_); 526 | 527 | if (end_effector.parent_group_.empty()) { 528 | throw std::runtime_error(boost::str( 529 | boost::format("End-effector '%s' has no parent group.\n") 530 | % end_effector.name_)); 531 | } 532 | RAVELOG_DEBUG("Loading '%s' manipulator group '%s'.\n", 533 | end_effector.name_.c_str(), 534 | end_effector.parent_group_.c_str()); 535 | srdf::Model::Group const &manip_group = GetSRDFGroup(groups, end_effector.parent_group_); 536 | 537 | // Compute the root link of the manipulator. 538 | std::vector manip_links, manip_root_links; 539 | std::vector manip_joints; 540 | ExtractSRDFGroup(urdf, manip_group, &manip_links, &manip_joints); 541 | GetURDFRootLinks(manip_links, &manip_root_links); 542 | RAVELOG_DEBUG("Manipulator group '%s' contains %d links and %d joints.\n", 543 | manip_group.name_.c_str(), manip_links.size(), manip_joints.size()); 544 | 545 | if (manip_root_links.size() != 1) { 546 | throw std::runtime_error(boost::str( 547 | boost::format("Manipulator '%s' has %d root links; must have exactly one.") 548 | % manip_group.name_.c_str() % manip_root_links.size())); 549 | } 550 | LinkConstPtr manip_root_link = manip_root_links.front(); 551 | BOOST_ASSERT(manip_root_link); 552 | RAVELOG_DEBUG("Detected '%s' as root link of manipulator group '%s'.\n", 553 | manip_root_link->name.c_str(), manip_group.name_.c_str()); 554 | 555 | // Find the parent link of the end-effector. This serves as the tip 556 | // link of the manipulator. 557 | // TODO: std::map link_map; 558 | OpenRAVE::KinBody::LinkInfoPtr ee_root_link; 559 | BOOST_AUTO(it, link_map.find(end_effector.parent_link_)); 560 | if (it != link_map.end()) { 561 | ee_root_link = it->second; 562 | } else { 563 | throw std::runtime_error(boost::str( 564 | boost::format("Unable to find end-effector parent " 565 | " link '%s'.") % end_effector.parent_link_)); 566 | } 567 | RAVELOG_DEBUG("Found manipulator tip link '%s'.\n", 568 | ee_root_link->_name.c_str()); 569 | 570 | // Find active joints in the end-effector group. We will treat these as 571 | // gripper joints. 572 | std::vector ee_links, ee_root_links; 573 | std::vector ee_joints; 574 | ExtractSRDFGroup(urdf, ee_group, &ee_links, &ee_joints); 575 | 576 | std::set gripper_joints; 577 | size_t num_rejected_gripper_joints = 0; 578 | 579 | BOOST_FOREACH (JointConstPtr ee_joint, ee_joints) { 580 | bool const is_mimic = !!ee_joint->mimic; 581 | bool const is_fixed = ee_joint->type == urdf::Joint::FIXED; 582 | 583 | if (!is_mimic && !is_fixed) { 584 | gripper_joints.insert(gripper_joints.begin(), ee_joint); 585 | } else { 586 | num_rejected_gripper_joints++; 587 | } 588 | } 589 | 590 | RAVELOG_DEBUG("Found %d gripper joints for manipulator '%s';" 591 | " ignored %d passive/mimic joints.\n", 592 | gripper_joints.size(), manip_group.name_.c_str(), 593 | num_rejected_gripper_joints); 594 | 595 | // Generate the OpenRAVE manipulator. 596 | // TODO: What about the closing direction? 597 | // TODO: What about the end-effector direction? 598 | BOOST_AUTO(manip_info, 599 | boost::make_shared()); 600 | manip_info->_name = end_effector.name_; 601 | manip_info->_sBaseLinkName = manip_root_link->name; 602 | manip_info->_vdirection = OpenRAVE::Vector(0, 0, 1); 603 | manip_info->_vChuckingDirection.resize(gripper_joints.size(), 0.0); 604 | manip_info->_sEffectorLinkName = ee_root_link->_name; 605 | 606 | BOOST_FOREACH (JointConstPtr joint, gripper_joints) { 607 | manip_info->_vGripperJointNames.push_back(joint->name); 608 | } 609 | manip_infos.push_back(manip_info); 610 | } 611 | 612 | // Add the sphere approximation to a geometry group. 613 | BOOST_FOREACH (srdf::Model::LinkSpheres const &spheres, 614 | srdf.getLinkSphereApproximations()) { 615 | // Find the corresponding OpenRAVE link. 616 | OpenRAVE::KinBody::LinkInfoPtr link_info; 617 | BOOST_FOREACH (OpenRAVE::KinBody::LinkInfoPtr candidate, link_infos) { 618 | if (candidate->_name == spheres.link_) { 619 | link_info = candidate; 620 | break; 621 | } 622 | } 623 | 624 | if (!link_info) { 625 | throw std::runtime_error(boost::str( 626 | boost::format("Unable to create sphere geometry for link '%s':" 627 | " Link does not exist.") % spheres.link_)); 628 | } 629 | 630 | // Get the transform for the collision geometry. Spheres are specified 631 | // in the collision frame. 632 | typedef boost::shared_ptr LinkConstPtr; 633 | LinkConstPtr const urdf_link = urdf.getLink(spheres.link_); 634 | BOOST_ASSERT(urdf_link); 635 | boost::shared_ptr const collision = urdf_link->collision; 636 | BOOST_ASSERT(collision); 637 | OpenRAVE::Transform const collision_transform 638 | = URDFPoseToRaveTransform(collision->origin); 639 | 640 | // Add the spheres to a separate collision group. 641 | std::vector &sphere_infos 642 | = link_info->_mapExtraGeometries["spheres"]; 643 | BOOST_FOREACH (srdf::Model::Sphere const &sphere, spheres.spheres_) { 644 | // TODO: Spheres should be in the collision frame. 645 | OpenRAVE::KinBody::GeometryInfoPtr sphere_info 646 | = boost::make_shared(); 647 | sphere_info->_bVisible = true; 648 | sphere_info->_bModifiable = false; 649 | sphere_info->_type = OpenRAVE::GT_Sphere; 650 | sphere_info->_vGeomData = sphere.radius_ * OpenRAVE::Vector(1, 1, 1); 651 | sphere_info->_t.trans = collision_transform * OpenRAVE::Vector( 652 | sphere.center_x_, sphere.center_y_, sphere.center_z_); 653 | sphere_infos.push_back(sphere_info); 654 | } 655 | } 656 | } 657 | 658 | void URDFLoader::ProcessGeometryGroupTagsFromURDF( 659 | TiXmlDocument &xml_doc, 660 | std::vector &link_infos) 661 | { 662 | TiXmlHandle xml_handle(&xml_doc); 663 | 664 | // element 665 | TiXmlHandle robot_handle(0); 666 | { 667 | TiXmlElement* robot_element = xml_handle.FirstChild("robot").Element(); 668 | if (!robot_element) { 669 | throw std::runtime_error("Could not find the 'robot' element in the xml file."); 670 | } 671 | 672 | robot_handle = TiXmlHandle(robot_element); 673 | } 674 | 675 | // Store a set of unique names found in the URDF 676 | std::set geometry_group_names_set; 677 | 678 | // Parse the URDF, 679 | // if a specific link has a tag 680 | // then add that group to the OpenRAVE link_infos: 681 | 682 | // elements 683 | for (TiXmlElement* link_element = robot_handle.FirstChild("link").Element(); 684 | link_element; 685 | link_element = link_element->NextSiblingElement("link")) 686 | { 687 | const char* link_name = link_element->Attribute("name"); 688 | if (!link_name) { 689 | throw std::runtime_error("Link is missing 'name' attribute."); 690 | } 691 | 692 | TiXmlHandle link_handle = TiXmlHandle(link_element); 693 | 694 | // Find the corresponding OpenRAVE link 695 | OpenRAVE::KinBody::LinkInfoPtr link_info; 696 | BOOST_FOREACH (OpenRAVE::KinBody::LinkInfoPtr candidate, 697 | link_infos) { 698 | if (candidate->_name == link_name) { 699 | link_info = candidate; 700 | break; 701 | } 702 | } 703 | if (!link_info) { 704 | throw std::runtime_error(boost::str( 705 | boost::format("Unable to find a corresponding link '%s'") 706 | % link_name)); 707 | } 708 | 709 | // element 710 | const char* geometry_group_name; 711 | for (TiXmlElement* geometry_group_element = link_handle.FirstChild("geometry_group").Element(); 712 | geometry_group_element; 713 | geometry_group_element = geometry_group_element->NextSiblingElement("geometry_group")) 714 | { 715 | geometry_group_name = geometry_group_element->Attribute("name"); 716 | if (!geometry_group_name) { 717 | throw std::runtime_error(boost::str( 718 | boost::format("Unable to get geometry_group name for link '%s':") 719 | % link_name)); 720 | } 721 | 722 | // Add this geometry group to our set of groups 723 | geometry_group_names_set.insert(geometry_group_name); 724 | 725 | TiXmlHandle geometry_group_handle = TiXmlHandle(geometry_group_element); 726 | 727 | // element 728 | urdf::Pose origin_pose; 729 | for (TiXmlElement* origin_element = geometry_group_handle.FirstChild("origin").Element(); 730 | origin_element; 731 | origin_element = origin_element->NextSiblingElement("origin")) 732 | { 733 | std::string rpy_str = std::string("0 0 0"); 734 | const char* rpy = origin_element->Attribute("rpy"); 735 | if (rpy) { 736 | rpy_str = std::string(rpy); 737 | } 738 | 739 | std::string xyz_str = std::string("0 0 0"); 740 | const char* xyz = origin_element->Attribute("xyz"); 741 | if (xyz) { 742 | xyz_str = std::string(xyz); 743 | } 744 | 745 | origin_pose.rotation.init(rpy_str); 746 | origin_pose.position.init(xyz_str); 747 | 748 | break; // only find first element 749 | } 750 | 751 | // element 752 | for (TiXmlElement* geometry_element = geometry_group_handle.FirstChild("geometry").Element(); 753 | geometry_element; 754 | geometry_element = geometry_element->NextSiblingElement("geometry")) 755 | { 756 | TiXmlHandle geometry_handle = TiXmlHandle(geometry_element); 757 | 758 | // element 759 | for (TiXmlElement* mesh_element = geometry_handle.FirstChild("mesh").Element(); 760 | mesh_element; 761 | mesh_element = mesh_element->NextSiblingElement("mesh")) 762 | { 763 | const char* mesh_filename = mesh_element->Attribute("filename"); 764 | if (!mesh_filename) { 765 | throw std::runtime_error(boost::str( 766 | boost::format("Unable to get 'filename' attribute for" 767 | " mesh element for link '%s':") 768 | % link_name)); 769 | } 770 | 771 | // Add this mesh to a separate collision geometry group 772 | std::vector &geom_infos 773 | = link_info->_mapExtraGeometries[geometry_group_name]; 774 | 775 | OpenRAVE::KinBody::GeometryInfoPtr geom_info 776 | = boost::make_shared(); 777 | geom_info->_t = URDFPoseToRaveTransform(origin_pose); 778 | geom_info->_bModifiable = false; 779 | 780 | // Set collision geometry to be visible, so it can be 781 | // seen in the 'visual' group in or_rviz 782 | geom_info->_bVisible = true; 783 | 784 | geom_info->_type = OpenRAVE::GT_TriMesh; 785 | geom_info->_filenamecollision = resolveURI(mesh_filename); 786 | 787 | // The mesh tag is not parsed, also apparently it 788 | // doesn't seem to do anything 789 | //geom_info->_vCollisionScale = URDFVectorToRaveVector(mesh_scale); 790 | 791 | boost::shared_ptr trimesh = 792 | boost::make_shared(); 793 | trimesh = GetEnv()->ReadTrimeshURI(trimesh, 794 | geom_info->_filenamecollision); 795 | if (trimesh) { 796 | // The _vCollisionScale property does nothing, so we have to 797 | // manually scale the mesh. 798 | BOOST_FOREACH(OpenRAVE::Vector &vertex, trimesh->vertices) { 799 | vertex *= geom_info->_vCollisionScale; 800 | } 801 | geom_info->_meshcollision = *trimesh; 802 | } 803 | else { 804 | RAVELOG_WARN("Link %s: Failed loading collision mesh %s \n", 805 | link_name, geom_info->_filenamecollision.c_str()); 806 | } 807 | 808 | geom_infos.push_back(geom_info); 809 | 810 | break; // only find first element 811 | } 812 | 813 | break; // only find first element 814 | } 815 | } // end element 816 | } // end element 817 | 818 | // Each OpenRAVE link must be a member of each geometry group, 819 | // so iterate over all the links and add them to each of the 820 | // geometry groups. 821 | // If a specific geometry group (and mesh) was already specified for 822 | // a link, calling calling mapExtraGeometries[] again does nothing. 823 | BOOST_FOREACH(OpenRAVE::KinBody::LinkInfoPtr link_info, link_infos) { 824 | BOOST_FOREACH(std::string geometry_group_name, geometry_group_names_set) { 825 | link_info->_mapExtraGeometries[geometry_group_name]; 826 | } 827 | } 828 | } 829 | 830 | /** Opens a URDF file and returns a robot in OpenRAVE */ 831 | bool URDFLoader::loadURI(std::ostream &soutput, std::istream &sinput) 832 | { 833 | try { 834 | // Get filename from input arguments 835 | std::string input_urdf_uri, input_srdf_uri; 836 | sinput >> input_urdf_uri >> input_srdf_uri; 837 | 838 | std::string const input_urdf = resolveURIorPath(input_urdf_uri); 839 | std::string const input_srdf = resolveURIorPath(input_srdf_uri); 840 | 841 | // Parse the URDF file 842 | urdf::Model urdf_model; 843 | if (!urdf_model.initFile(input_urdf)) { 844 | throw std::runtime_error("Failed to open URDF file."); 845 | } 846 | 847 | // Parse the SRDF file, if specified 848 | std::shared_ptr srdf_model; 849 | if (!input_srdf.empty()) { 850 | srdf_model = std::make_shared(); 851 | srdf_model->initFile(urdf_model, input_srdf); 852 | } 853 | 854 | // Parse the URDF again to find elements 855 | TiXmlDocument xml_doc(input_urdf.c_str()); 856 | if (!xml_doc.LoadFile()) { 857 | throw std::runtime_error("Could not load the URDF file."); 858 | } 859 | 860 | std::string uri = srdf_model == nullptr ? 861 | input_urdf_uri : 862 | input_urdf_uri + " " + input_srdf_uri; 863 | soutput << loadModel(urdf_model, xml_doc, srdf_model, uri); 864 | return true; 865 | } catch (std::runtime_error const &e) { 866 | RAVELOG_ERROR("Failed loading URDF model: %s\n", e.what()); 867 | return false; 868 | } 869 | } 870 | 871 | /** load URDF and SRDF from file/URI with deprecated warning for "load" command 872 | * name 873 | */ 874 | bool URDFLoader::deprecatedLoad(std::ostream &soutput, std::istream &sinput) 875 | { 876 | RAVELOG_WARN("URDFLoader 'load' command is deprecated. Use 'LoadURI' instead.\n"); 877 | return loadURI(soutput, sinput); 878 | } 879 | 880 | /** Loads a JSON-wrapped URDF and optionally SRDF string and returns a robot in 881 | * OpenRAVE. 882 | * 883 | * The JSON object must have the following schema (srdf property is optional): 884 | * 885 | * { "urdf": "(sinput), {}); 892 | picojson::value json_v; 893 | std::string err = picojson::parse(json_v, json_wrapper); 894 | if (!err.empty()) { 895 | throw std::runtime_error("Failed to parse JSON wrapper: " + err); 896 | } 897 | 898 | if (!json_v.is()) { 899 | throw std::runtime_error("JSON wrapper not JSON object"); 900 | } 901 | picojson::object &obj = json_v.get(); 902 | 903 | // parse and and load URDF 904 | auto urdf_v = obj.find("urdf"); 905 | if (urdf_v == obj.end()) { 906 | throw std::runtime_error("JSON wrapper has no \"urdf\" property"); 907 | } 908 | if (!urdf_v->second.is()) { 909 | throw std::runtime_error( 910 | "JSON wrapper \"urdf\" property is not a string"); 911 | } 912 | std::string urdf_string = urdf_v->second.get(); 913 | 914 | urdf::Model urdf_model; 915 | if (!urdf_model.initString(urdf_string)) { 916 | throw std::runtime_error("Failed to load URDF from string."); 917 | } 918 | 919 | // optionally parse and load SRDF 920 | std::shared_ptr srdf_model; 921 | auto srdf_v = obj.find("srdf"); 922 | if (srdf_v == obj.end()) { 923 | RAVELOG_INFO("JSON wrapper has no \"srdf\" property"); 924 | } else { 925 | if (!srdf_v->second.is()) { 926 | throw std::runtime_error( 927 | "JSON wrapper \"srdf\" property is not a string"); 928 | } 929 | std::string srdf_string = srdf_v->second.get(); 930 | srdf_model = std::make_shared(); 931 | 932 | if (!srdf_model->initString(urdf_model, srdf_string)) { 933 | throw std::runtime_error("Failed to load SRDF from string."); 934 | } 935 | } 936 | 937 | // Parse the URDF again to find elements 938 | TiXmlDocument xml_doc; 939 | xml_doc.Parse(urdf_string.c_str()); 940 | if (xml_doc.Error()) { 941 | std::string xmlerr = xml_doc.ErrorDesc(); 942 | throw std::runtime_error("Could not parse the URDF file: " + xmlerr); 943 | } 944 | 945 | soutput << loadModel(urdf_model, xml_doc, srdf_model); 946 | return true; 947 | 948 | } catch (std::runtime_error const &e) { 949 | RAVELOG_ERROR("Failed loading URDF/SRDF model: %s\n", e.what()); 950 | return false; 951 | } 952 | } 953 | 954 | std::string URDFLoader::loadModel(urdf::Model &urdf_model, 955 | TiXmlDocument &xml_doc, 956 | std::shared_ptr srdf_model, 957 | std::string uri) 958 | { 959 | OpenRAVE::KinBodyPtr body; 960 | std::string name; 961 | 962 | std::vector link_infos; 963 | std::vector joint_infos; 964 | ParseURDF(urdf_model, link_infos, joint_infos); 965 | 966 | std::vector manip_infos; 967 | std::vector sensor_infos; 968 | if (srdf_model != nullptr) { 969 | ParseSRDF(urdf_model, *srdf_model, link_infos, joint_infos, manip_infos); 970 | } 971 | 972 | ProcessGeometryGroupTagsFromURDF(xml_doc, link_infos); 973 | 974 | if (srdf_model != nullptr) { 975 | // Cast all of the vector contents to const. 976 | std::vector link_infos_const = 977 | MakeConst(link_infos); 978 | std::vector joint_infos_const = 979 | MakeConst(joint_infos); 980 | std::vector 981 | manip_infos_const = MakeConst(manip_infos); 982 | std::vector 983 | sensor_infos_const = MakeConst(sensor_infos); 984 | 985 | // TODO: Sort the joints to guarantee contiguous manipulators. 986 | 987 | OpenRAVE::RobotBasePtr robot = OpenRAVE::RaveCreateRobot(GetEnv(), ""); 988 | robot->Init(link_infos_const, joint_infos_const, manip_infos_const, 989 | sensor_infos_const, uri); 990 | body = robot; 991 | } 992 | // It's just a URDF file, so create a KinBody. 993 | else { 994 | std::vector link_infos_const = 995 | MakeConst(link_infos); 996 | std::vector joint_infos_const = 997 | MakeConst(joint_infos); 998 | 999 | body = OpenRAVE::RaveCreateKinBody(GetEnv(), ""); 1000 | body->Init(link_infos_const, joint_infos_const, uri); 1001 | } 1002 | 1003 | body->SetName(urdf_model.getName()); 1004 | GetEnv()->Add(body, true); 1005 | return body->GetName(); 1006 | } 1007 | 1008 | /** Resolves URIs for file:// and package:// paths */ 1009 | std::string URDFLoader::resolveURI(const std::string &path) const 1010 | { 1011 | std::string uri = path; 1012 | 1013 | if (uri.find("file://") == 0) { 1014 | // Strip off the file:// 1015 | uri.erase(0, strlen("file://")); 1016 | 1017 | // Resolve the mesh path as a file URI 1018 | boost::filesystem::path file_path(uri); 1019 | return file_path.string(); 1020 | } else if (uri.find("package://") == 0) { 1021 | return _catkin_finder.find(uri); 1022 | } else { 1023 | RAVELOG_WARN("Cannot handle this type of URI.\n"); 1024 | return ""; 1025 | } 1026 | } 1027 | 1028 | std::string URDFLoader::resolveURIorPath(const std::string &path) const 1029 | { 1030 | using boost::algorithm::starts_with; 1031 | 1032 | if (starts_with(path, "package://") || starts_with(path, "file://")) { 1033 | return resolveURI(path); 1034 | } else { 1035 | return path; 1036 | } 1037 | } 1038 | 1039 | } 1040 | --------------------------------------------------------------------------------