[sdformat] 01/01: Imported Upstream version 4.0.0
Jose Luis Rivero
jrivero-guest at moszumanska.debian.org
Fri Jan 15 19:42:08 UTC 2016
This is an automated email from the git hooks/post-receive script.
jrivero-guest pushed a commit to annotated tag upstream/4.0.0
in repository sdformat.
commit d1586450e96fc7332150d54589f03dc4e4bcae5d
Author: Jose Luis Rivero <jrivero at osrfoundation.org>
Date: Fri Jan 15 00:17:49 2016 +0000
Imported Upstream version 4.0.0
---
.hg_archival.txt | 8 +-
.hgtags | 8 +
CMakeLists.txt | 6 +-
Changelog.md | 65 +-
cmake/SDFUtils.cmake | 25 +-
cmake/SearchForStuff.cmake | 24 +-
cmake/sdf_config.h.in | 4 +-
include/sdf/Console.hh | 6 +-
include/sdf/Converter.hh | 9 +-
include/sdf/Element.hh | 15 +-
include/sdf/Param.hh | 18 +-
include/sdf/SDFExtension.hh | 38 +-
include/sdf/SDFImpl.hh | 9 +-
include/sdf/Types.hh | 1 -
sdf/1.0/CMakeLists.txt | 2 +-
sdf/1.2/CMakeLists.txt | 2 +-
sdf/1.3/CMakeLists.txt | 2 +-
sdf/1.4/CMakeLists.txt | 2 +-
sdf/1.5/CMakeLists.txt | 8 +-
sdf/1.5/actor.sdf | 9 +-
sdf/1.5/audio_source.sdf | 5 +-
sdf/1.5/camera.sdf | 7 +-
sdf/1.5/collision.sdf | 5 +-
sdf/1.5/frame.sdf | 11 +
sdf/1.5/gui.sdf | 9 +-
sdf/1.5/inertial.sdf | 8 +-
sdf/1.5/joint.sdf | 6 +-
sdf/1.5/light.sdf | 5 +-
sdf/1.5/light_state.sdf | 5 +-
sdf/1.5/link.sdf | 6 +-
sdf/1.5/link_state.sdf | 11 +-
sdf/1.5/model.sdf | 6 +-
sdf/1.5/model_state.sdf | 7 +-
sdf/1.5/population.sdf | 9 +-
sdf/1.5/pose.sdf | 9 +
sdf/1.5/projector.sdf | 7 +-
sdf/1.5/sensor.sdf | 6 +-
sdf/1.5/visual.sdf | 7 +-
sdf/1.5/world.sdf | 6 +-
sdf/1.6/1_5.convert | 331 ++++++
sdf/{1.5 => 1.6}/CMakeLists.txt | 9 +-
sdf/{1.5 => 1.6}/actor.sdf | 9 +-
sdf/1.6/altimeter.sdf | 18 +
sdf/1.6/atmosphere.sdf | 25 +
sdf/1.6/audio_sink.sdf | 4 +
sdf/{1.5 => 1.6}/audio_source.sdf | 5 +-
sdf/1.6/battery.sdf | 12 +
sdf/1.6/box_shape.sdf | 6 +
sdf/{1.5 => 1.6}/camera.sdf | 7 +-
sdf/{1.5 => 1.6}/collision.sdf | 5 +-
sdf/1.6/collision_engine.sdf | 17 +
sdf/1.6/contact.sdf | 12 +
sdf/1.6/cylinder_shape.sdf | 9 +
sdf/1.6/forcetorque.sdf | 20 +
sdf/1.6/frame.sdf | 11 +
sdf/1.6/geometry.sdf | 18 +
sdf/1.6/gps.sdf | 40 +
sdf/1.6/gripper.sdf | 30 +
sdf/{1.5 => 1.6}/gui.sdf | 9 +-
sdf/1.6/heightmap_shape.sdf | 40 +
sdf/1.6/image_shape.sdf | 18 +
sdf/1.6/imu.sdf | 42 +
sdf/{1.5 => 1.6}/inertial.sdf | 8 +-
sdf/{1.5 => 1.6}/joint.sdf | 6 +-
sdf/{1.5 => 1.6}/light.sdf | 5 +-
sdf/{1.5 => 1.6}/light_state.sdf | 5 +-
sdf/{1.5 => 1.6}/link.sdf | 10 +-
sdf/{1.5 => 1.6}/link_state.sdf | 11 +-
sdf/1.6/logical_camera.sdf | 19 +
sdf/1.6/magnetometer.sdf | 21 +
sdf/1.6/material.sdf | 47 +
sdf/1.6/mesh_shape.sdf | 20 +
sdf/{1.5 => 1.6}/model.sdf | 10 +-
sdf/{1.5 => 1.6}/model_state.sdf | 11 +-
sdf/1.6/noise.sdf | 31 +
sdf/1.6/physics.sdf | 205 ++++
sdf/1.6/plane_shape.sdf | 9 +
sdf/1.6/plugin.sdf | 13 +
sdf/1.6/polyline_shape.sdf | 14 +
sdf/{1.5 => 1.6}/population.sdf | 9 +-
sdf/1.6/pose.sdf | 9 +
sdf/{1.5 => 1.6}/projector.sdf | 7 +-
sdf/1.6/ray.sdf | 73 ++
sdf/1.6/rfid.sdf | 2 +
sdf/1.6/rfidtag.sdf | 2 +
sdf/1.6/road.sdf | 16 +
sdf/1.6/root.sdf | 13 +
sdf/1.6/scene.sdf | 82 ++
sdf/1.6/schema/types.xsd | 44 +
sdf/{1.5 => 1.6}/sensor.sdf | 6 +-
sdf/1.6/sonar.sdf | 13 +
sdf/1.6/sphere_shape.sdf | 6 +
sdf/1.6/spherical_coordinates.sdf | 39 +
sdf/1.6/state.sdf | 40 +
sdf/1.6/surface.sdf | 185 +++
sdf/1.6/transceiver.sdf | 34 +
sdf/1.6/urdf.sdf | 19 +
sdf/{1.5 => 1.6}/visual.sdf | 7 +-
sdf/{1.5 => 1.6}/world.sdf | 22 +-
sdf/CMakeLists.txt | 1 +
src/Console.cc | 2 +-
src/Converter.cc | 183 ++-
src/Element.cc | 7 +-
src/Param.cc | 49 +-
src/Param_TEST.cc | 44 +-
src/SDF.cc | 5 +-
src/SDFExtension.cc | 10 +
src/SDF_TEST.cc | 18 +-
src/parser.cc | 22 +-
src/parser_urdf.cc | 1130 ++++++++++++++----
test/integration/CMakeLists.txt | 4 +
test/integration/converter.cc | 181 +++
test/integration/fixed_joint_reduction.cc | 434 +++++++
...joint_reduction_collision_visual_empty_root.sdf | 59 +
...oint_reduction_collision_visual_empty_root.urdf | 57 +
..._joint_reduction_collision_visual_extension.sdf | 144 +++
...joint_reduction_collision_visual_extension.urdf | 170 +++
test/integration/frame.cc | 1251 ++++++++++++++++++++
test/integration/include.cc | 54 +
test/integration/include_description.sdf | 12 +
test/integration/model/box/model.config | 15 +
test/integration/model/box/model.sdf | 22 +
test/integration/urdf_gazebo_extensions.cc | 100 ++
test/integration/urdf_gazebo_extensions.urdf | 140 +++
tools/code_check.sh | 4 +-
125 files changed, 5683 insertions(+), 601 deletions(-)
diff --git a/.hg_archival.txt b/.hg_archival.txt
index c7dd77e..6a65f80 100644
--- a/.hg_archival.txt
+++ b/.hg_archival.txt
@@ -1,5 +1,5 @@
repo: 17049bd77df5bd6cd56a76edba4a54afb5647740
-node: 0a8c9f3ab18c2aeda4c0a561ece7a9c3fbe61ded
-branch: sdf3
-latesttag: sdformat3_3.5.0
-latesttagdistance: 8
+node: 708122d23ecbe1ca98780b0621802af0c1fb8b55
+branch: default
+latesttag: sdformat4_4.0.0~pre3
+latesttagdistance: 3
diff --git a/.hgtags b/.hgtags
index fac3e8d..02bdcfa 100644
--- a/.hgtags
+++ b/.hgtags
@@ -102,3 +102,11 @@ b26bb75fa846b25bedd6726615fd7fb6877b65df sdformat3_3.2.0
a2a653f9545b08cbf4da18bf6aff441daa95447f sdformat3_3.3.0
54b4f790ab1feac11f100928159ddf1aa50200e0 sdformat3_3.4.0
346771c498661f0e773a0a8a3ecd877d2b0a000c sdformat3_3.5.0
+0a8c9f3ab18c2aeda4c0a561ece7a9c3fbe61ded sdformat3_3.6.0
+2c63e75816e977fff2031a676da5c0d69638e3ff sdformat3_3.7.0
+8de03dc62332fca224964c807a81382e5334b7f6 sdformat4-prerelease_4.0.0
+8de03dc62332fca224964c807a81382e5334b7f6 sdformat4-prerelease_4.0.0
+b70f62b6a41c7dc1a78bef56d507f2c68f1e1402 sdformat4-prerelease_4.0.0
+83d87555c4230cf212ba42ed8fac6d123eb4125d sdformat4_4.0.0~pre1
+bb3fff1c17397bada85e3aad11f508c23cead6fd sdformat4_4.0.0~pre2
+0dd0fdbc39b94e9039fa98c5e816972d2584b66c sdformat4_4.0.0~pre3
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 56caf8e..abbdc83 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,10 +25,10 @@ string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
# The protocol version has nothing to do with the package version set below.
# It represents the current version of sdformat implement by the software
-set (SDF_PROTOCOL_VERSION 1.5)
+set (SDF_PROTOCOL_VERSION 1.6)
-set (SDF_MAJOR_VERSION 3)
-set (SDF_MINOR_VERSION 6)
+set (SDF_MAJOR_VERSION 4)
+set (SDF_MINOR_VERSION 0)
set (SDF_PATCH_VERSION 0)
set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION})
diff --git a/Changelog.md b/Changelog.md
index a329838..e9c5ac1 100644
--- a/Changelog.md
+++ b/Changelog.md
@@ -1,16 +1,59 @@
## SDFormat 3.0
-### SDFormat 3.x.x (xxxx-xx-xx)
+### SDFormat 3.7.0 (2015-11-20)
-1. Added lens element to camera SDF
- * [Pull request 215](https://bitbucket.org/osrf/sdformat/pull-request/215)
-1. Added torsional friction to SDF
- * [Pull request 211](https://bitbucket.org/osrf/sdformat/pull-request/211)
- * [Pull request 217](https://bitbucket.org/osrf/sdformat/pull-request/217)
+1. Add spring pass through for sdf3
+ * [Design document](https://bitbucket.org/osrf/gazebo_design/pull-requests/23)
+ * [Pull request 242](https://bitbucket.org/osrf/sdformat/pull-request/242)
-1. Added support for nested models
- * [Pull request 221](https://bitbucket.org/osrf/sdformat/pull-request/221)
- * [Pull request 223](https://bitbucket.org/osrf/sdformat/pull-request/223)
+1. Support frame specification in SDF
+ * [Pull request 237](https://bitbucket.org/osrf/sdformat/pull-request/237)
+
+1. Remove boost from SDFExtension
+ * [Pull request 229](https://bitbucket.org/osrf/sdformat/pull-request/229)
+
+### SDFormat 3.6.0 (2015-10-27)
+
+1. Add light state
+ * [Pull request 227](https://bitbucket.org/osrf/sdformat/pull-request/227)
+1. redo pull request #222 for sdf3 branch
+ * [Pull request 232](https://bitbucket.org/osrf/sdformat/pull-request/232)
+1. Fix links in API documentation
+ * [Pull request 231](https://bitbucket.org/osrf/sdformat/pull-request/231)
+
+### SDFormat 3.5.0 (2015-10-07)
+
+1. Camera lens description (Replaces #213)
+ * [Pull request 215](https://bitbucket.org/osrf/sdformat/pull-request/215)
+1. Fix shared pointer reference loop in Element and memory leak (#104)
+ * [Pull request 230](https://bitbucket.org/osrf/sdformat/pull-request/230)
+
+### SDFormat 3.4.0 (2015-10-05)
+
+1. Support nested model states
+ * [Pull request 223](https://bitbucket.org/osrf/sdformat/pull-request/223)
+1. Cleaner way to set SDF_PATH for tests
+ * [Pull request 226](https://bitbucket.org/osrf/sdformat/pull-request/226)
+
+### SDFormat 3.3.0 (2015-09-15)
+
+1. Windows Boost linking errors
+ * [Pull request 206](https://bitbucket.org/osrf/sdformat/pull-request/206)
+1. Nested SDF -> sdf3
+ * [Pull request 221](https://bitbucket.org/osrf/sdformat/pull-request/221)
+1. Pointer types
+ * [Pull request 218](https://bitbucket.org/osrf/sdformat/pull-request/218)
+1. Torsional friction default surface radius not infinity
+ * [Pull request 217](https://bitbucket.org/osrf/sdformat/pull-request/217)
+
+### SDFormat 3.2.2 (2015-08-24)
+
+1. Added battery element (contribution from Olivier Crave)
+ * [Pull request #204](https://bitbucket.org/osrf/sdformat/pull-request/204)
+1. Torsional friction backport
+ * [Pull request #211](https://bitbucket.org/osrf/sdformat/pull-request/211)
+1. Allow Visual Studio 2015
+ * [Pull request #208](https://bitbucket.org/osrf/sdformat/pull-request/208)
### SDFormat 3.1.1 (2015-08-03)
@@ -20,14 +63,14 @@
### SDFormat 3.1.0 (2015-08-02)
1. Added logical camera sensor to SDF
- * [Pull request 207](https://bitbucket.org/osrf/sdformat/pull-request/207)
+ * [Pull request #207](https://bitbucket.org/osrf/sdformat/pull-request/207)
### SDFormat 3.0.0 (2015-07-24)
1. Added battery to SDF
* [Pull request 204](https://bitbucket.org/osrf/sdformat/pull-request/204)
1. Added altimeter sensor to SDF
- * [Pull request 197](https://bitbucket.org/osrf/sdformat/pull-request/197)
+ * [Pull request #197](https://bitbucket.org/osrf/sdformat/pull-request/197)
1. Added magnetometer sensor to SDF
* [Pull request 198](https://bitbucket.org/osrf/sdformat/pull-request/198)
1. Fix detection of XML parsing errors
diff --git a/cmake/SDFUtils.cmake b/cmake/SDFUtils.cmake
index a3aa467..71b40e1 100644
--- a/cmake/SDFUtils.cmake
+++ b/cmake/SDFUtils.cmake
@@ -165,8 +165,24 @@ macro (sdf_build_tests)
add_test(${BINARY_NAME} ${CMAKE_CURRENT_BINARY_DIR}/${BINARY_NAME}
--gtest_output=xml:${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
- set(_env_vars)
- list(APPEND _env_vars "SDF_PATH=${PROJECT_SOURCE_DIR}/sdf/${SDF_PROTOCOL_VERSION}")
+ set (_env_vars)
+ set (sdf_paths)
+
+ # Get all the sdf protocol directory names
+ file(GLOB dirs RELATIVE "${PROJECT_SOURCE_DIR}/sdf"
+ "${PROJECT_SOURCE_DIR}/sdf/*")
+ list(SORT dirs)
+
+ # Add each sdf protocol to the sdf_path variable
+ foreach(dir ${dirs})
+ if (IS_DIRECTORY ${PROJECT_SOURCE_DIR}/sdf/${dir})
+ set(sdf_paths "${PROJECT_SOURCE_DIR}/sdf/${dir}:${sdf_paths}")
+ endif()
+ endforeach()
+
+ # Set the SDF_PATH environment variable
+ list(APPEND _env_vars "SDF_PATH=${sdf_paths}")
+
set_tests_properties(${BINARY_NAME} PROPERTIES
TIMEOUT 240
ENVIRONMENT "${_env_vars}")
@@ -177,6 +193,11 @@ macro (sdf_build_tests)
add_test(check_${BINARY_NAME} ${PYTHON_EXECUTABLE} ${PROJECT_SOURCE_DIR}/tools/check_test_ran.py
${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
endif()
+
+ if(SDFORMAT_RUN_VALGRIND_TESTS AND VALGRIND_PROGRAM)
+ add_test(memcheck_${BINARY_NAME} ${VALGRIND_PROGRAM} --leak-check=full
+ --error-exitcode=1 --show-leak-kinds=all ${CMAKE_CURRENT_BINARY_DIR}/${BINARY_NAME})
+ endif()
endforeach()
endmacro()
diff --git a/cmake/SearchForStuff.cmake b/cmake/SearchForStuff.cmake
index 73bd031..f9895a2 100644
--- a/cmake/SearchForStuff.cmake
+++ b/cmake/SearchForStuff.cmake
@@ -9,7 +9,7 @@ message(STATUS "Building for arch: ${ARCH}")
########################################
# Find Boost, if not specified manually
if (WIN32)
- set(Boost_USE_STATIC_LIBS OFF)
+ set(Boost_USE_STATIC_LIBS OFF)
set(Boost_USE_MULTITHREADED ON)
set(Boost_USE_STATIC_RUNTIME OFF)
endif()
@@ -20,7 +20,7 @@ find_package(Boost ${MIN_BOOST_VERSION} REQUIRED system filesystem program_optio
if (NOT Boost_FOUND)
set (BUILD_SDF OFF CACHE INTERNAL "Build SDF" FORCE)
BUILD_ERROR ("Boost not found. Please install thread signals system filesystem program_options regex boost version ${MIN_BOOST_VERSION} or higher.")
-endif()
+endif()
if (USE_EXTERNAL_TINYXML)
#################################################
@@ -30,17 +30,17 @@ if (USE_EXTERNAL_TINYXML)
if (NOT tinyxml_FOUND)
find_path (tinyxml_include_dirs tinyxml.h ${tinyxml_include_dirs} ENV CPATH)
find_library(tinyxml_LIBRARIES NAMES tinyxml)
- set (tinyxml_FAIL False)
+ set (tinyxml_FAIL False)
if (NOT tinyxml_include_dirs)
message (STATUS "Looking for tinyxml headers - not found")
- set (tinyxml_FAIL True)
+ set (tinyxml_FAIL True)
endif()
if (NOT tinyxml_LIBRARIES)
message (STATUS "Looking for tinyxml library - not found")
- set (tinyxml_FAIL True)
+ set (tinyxml_FAIL True)
endif()
endif()
-
+
if (tinyxml_FAIL)
message (STATUS "Looking for tinyxml.h - not found")
BUILD_ERROR("Missing: tinyxml")
@@ -102,6 +102,16 @@ if(NOT PY_PSUTIL)
endif()
################################################
+# Find Valgrind for checking memory leaks in the
+# tests
+find_program(VALGRIND_PROGRAM NAMES valgrind PATH ${VALGRIND_ROOT}/bin)
+option(SDFORMAT_RUN_VALGRIND_TESTS "Run sdformat tests with Valgrind" FALSE)
+mark_as_advanced(SDFORMAT_RUN_VALGRIND_TESTS)
+if (SDFORMAT_RUN_VALGRIND_TESTS AND NOT VALGRIND_PROGRAM)
+ BUILD_WARNING("valgrind not found. Memory check tests will be skipped.")
+endif()
+
+################################################
# Find ruby executable to produce xml schemas
find_program(RUBY ruby)
if (NOT RUBY)
@@ -112,7 +122,7 @@ endif()
#################################################
# Macro to check for visibility capability in compiler
-# Original idea from: https://gitorious.org/ferric-cmake-stuff/
+# Original idea from: https://gitorious.org/ferric-cmake-stuff/
macro (check_gcc_visibility)
include (CheckCXXCompilerFlag)
check_cxx_compiler_flag(-fvisibility=hidden GCC_SUPPORTS_VISIBILITY)
diff --git a/cmake/sdf_config.h.in b/cmake/sdf_config.h.in
index 95443ed..77479cc 100644
--- a/cmake/sdf_config.h.in
+++ b/cmake/sdf_config.h.in
@@ -28,5 +28,5 @@
#cmakedefine USE_EXTERNAL_URDF 1
#cmakedefine URDF_GE_0P3 1
-#define SDF_SHARE_PATH "${CMAKE_INSTALL_PREFIX}/share/"
-#define SDF_VERSION_PATH "${CMAKE_INSTALL_PREFIX}/share/sdformat/${SDF_PKG_VERSION}"
+#define SDF_SHARE_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/"
+#define SDF_VERSION_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat/${SDF_PKG_VERSION}"
diff --git a/include/sdf/Console.hh b/include/sdf/Console.hh
index c87329b..e72863e 100644
--- a/include/sdf/Console.hh
+++ b/include/sdf/Console.hh
@@ -22,7 +22,7 @@
#include <fstream>
#include <string>
-#include <boost/shared_ptr.hpp>
+#include <memory>
#include "sdf/system_util.hh"
@@ -52,7 +52,7 @@ namespace sdf
/// \def ConsolePtr
/// \brief Shared pointer to a Console Element
- typedef boost::shared_ptr<Console> ConsolePtr;
+ typedef std::shared_ptr<Console> ConsolePtr;
/// \brief Message, error, warning, and logging functionality
class SDFORMAT_VISIBLE Console
@@ -115,7 +115,7 @@ namespace sdf
const std::string &file,
unsigned int line);
- private: ConsolePrivate *dataPtr;
+ private: std::unique_ptr<ConsolePrivate> dataPtr;
};
/// \internal
diff --git a/include/sdf/Converter.hh b/include/sdf/Converter.hh
index 6f58e34..3c8c87c 100644
--- a/include/sdf/Converter.hh
+++ b/include/sdf/Converter.hh
@@ -61,8 +61,10 @@ namespace sdf
/// be moved.
/// \param[in] _moveElem A 'convert' element that describes the move
/// operation.
+ /// \param[in] _copy True to copy the element
private: static void Move(TiXmlElement *_elem,
- TiXmlElement *_moveElem);
+ TiXmlElement *_moveElem,
+ const bool _copy);
/// \brief Add an element or attribute to an element.
/// \param[in] _elem The element to receive the value.
@@ -71,6 +73,11 @@ namespace sdf
private: static void Add(TiXmlElement *_elem,
TiXmlElement *_addElem);
+ /// \brief Remove an element.
+ /// \param[in] _elem The element that has the _removeElem child.
+ /// \param[in] _removeElem The element to remove.
+ private: static void Remove(TiXmlElement *_elem, TiXmlElement *_removeElem);
+
private: static const char *GetValue(const char *_valueElem,
const char *_valueAttr,
TiXmlElement *_elem);
diff --git a/include/sdf/Element.hh b/include/sdf/Element.hh
index f1f6bac..f2c0d09 100644
--- a/include/sdf/Element.hh
+++ b/include/sdf/Element.hh
@@ -19,8 +19,7 @@
#include <string>
#include <vector>
-#include <boost/shared_ptr.hpp>
-#include <boost/enable_shared_from_this.hpp>
+#include <memory>
#include "sdf/Param.hh"
#include "sdf/system_util.hh"
@@ -37,7 +36,7 @@
#ifdef _WIN32
// Disable warning C4251 which is triggered by
-// boost::enable_shared_from_this
+// std::enable_shared_from_this
#pragma warning(push)
#pragma warning(disable: 4251)
#endif
@@ -50,16 +49,16 @@ namespace sdf
class SDFORMAT_VISIBLE Element;
/// \def ElementPtr
- /// \brief boost shared pointer to an SDF Element
- typedef boost::shared_ptr<Element> ElementPtr;
+ /// \brief Shared pointer to an SDF Element
+ typedef std::shared_ptr<Element> ElementPtr;
/// \def ElementWeakPtr
/// \brief Weak pointer to an SDF Element
- typedef boost::weak_ptr<Element> ElementWeakPtr;
+ typedef std::weak_ptr<Element> ElementWeakPtr;
/// \def ElementPtr_V
/// \brief Vector of ElementPtr
- typedef std::vector< ElementPtr > ElementPtr_V;
+ typedef std::vector<ElementPtr> ElementPtr_V;
/// \addtogroup sdf
/// \{
@@ -67,7 +66,7 @@ namespace sdf
/// \class Element Element.hh sdf/sdf.hh
/// \brief SDF Element class
class SDFORMAT_VISIBLE Element :
- public boost::enable_shared_from_this<Element>
+ public std::enable_shared_from_this<Element>
{
/// \brief Constructor.
public: Element();
diff --git a/include/sdf/Param.hh b/include/sdf/Param.hh
index 51ed27e..7cf15d5 100644
--- a/include/sdf/Param.hh
+++ b/include/sdf/Param.hh
@@ -21,14 +21,13 @@
// See: https://bugreports.qt-project.org/browse/QTBUG-22829
#ifndef Q_MOC_RUN
#include <boost/lexical_cast.hpp>
- #include <boost/bind.hpp>
- #include <boost/algorithm/string.hpp>
#include <boost/any.hpp>
- #include <boost/shared_ptr.hpp>
#include <boost/variant.hpp>
- #include <boost/function.hpp>
#endif
+#include <memory>
+#include <functional>
+#include <algorithm>
#include <typeinfo>
#include <string>
#include <vector>
@@ -52,11 +51,11 @@ namespace sdf
class SDFORMAT_VISIBLE Param;
/// \def ParamPtr
- /// \brief boost shared_ptr to a Param
- typedef boost::shared_ptr<Param> ParamPtr;
+ /// \brief Shared pointer to a Param
+ typedef std::shared_ptr<Param> ParamPtr;
/// \def Param_V
- /// \brief vector or boost shared_ptrs to a Param
+ /// \brief vector of shared pointers to a Param
typedef std::vector<ParamPtr> Param_V;
/// \internal
@@ -219,7 +218,7 @@ namespace sdf
public: std::string description;
/// \brief Update function pointer.
- public: boost::function<boost::any ()> updateFunc;
+ public: std::function<boost::any ()> updateFunc;
/// \todo Remove this diagnositic push/pop in version 5
#ifndef _WIN32
@@ -339,7 +338,8 @@ namespace sdf
if (this->dataPtr->typeName == "bool")
{
std::string strValue = _value;
- boost::algorithm::to_lower(strValue);
+ std::transform(strValue.begin(), strValue.end(),
+ strValue.begin(), ::tolower);
if (strValue == "true" || strValue == "1")
this->dataPtr->value = true;
else
diff --git a/include/sdf/SDFExtension.hh b/include/sdf/SDFExtension.hh
index c076aeb..96b39b6 100644
--- a/include/sdf/SDFExtension.hh
+++ b/include/sdf/SDFExtension.hh
@@ -19,6 +19,7 @@
#define _SDFORMAT_SDFEXTENSION_HH_
#include <tinyxml.h>
+#include <memory>
#include <string>
#include <vector>
#include <ignition/math/Pose3.hh>
@@ -53,9 +54,38 @@ namespace sdf
public: std::string oldLinkName;
public: ignition::math::Pose3d reductionTransform;
- // visual
+ // visual material
public: std::string material;
- public: std::vector<boost::shared_ptr<TiXmlElement> > visual_blobs;
+
+ /// \brief blobs of xml to be copied into the visual sdf element
+ public: std::vector<std::shared_ptr<TiXmlElement> > visual_blobs;
+
+ /// \brief blobs of xml to be copied into the collision sdf element
+ /// An example might be:
+ /// <gazebo reference="link-1">
+ /// <collision>
+ /// <max_contacts>10</max_contacts>
+ /// <surface>
+ /// <contact>
+ /// <ode>
+ /// <kp>1e+06</kp>
+ /// <kd>100</kd>
+ /// <max_vel>100</max_vel>
+ /// <min_depth>0.001</min_depth>
+ /// </ode>
+ /// </contact>
+ /// <friction>
+ /// <ode>
+ /// <mu>1</mu>
+ /// <mu2>1</mu2>
+ /// </ode>
+ /// </friction>
+ /// </surface>
+ /// </collision>
+ /// </gazebo>
+ /// where all the contents of `<collision>` element is copied into the
+ /// resulting collision sdf.
+ public: std::vector<std::shared_ptr<TiXmlElement> > collision_blobs;
// body, default off
public: bool setStaticFlag;
@@ -80,6 +110,8 @@ namespace sdf
// joint, joint limit dynamics
public: bool isStopCfm, isStopErp, isInitialJointPosition, isFudgeFactor;
public: double stopCfm, stopErp, initialJointPosition, fudgeFactor;
+ public: bool isSpringReference, isSpringStiffness;
+ public: double springReference, springStiffness;
public: bool isProvideFeedback;
public: bool provideFeedback;
public: bool isImplicitSpringDamper;
@@ -88,7 +120,7 @@ namespace sdf
public: double stopKp, stopKd;
// blobs into body or robot
- public: std::vector<boost::shared_ptr<TiXmlElement> > blobs;
+ public: std::vector<std::shared_ptr<TiXmlElement> > blobs;
friend class SDFORMAT_VISIBLE URDF2SDF;
};
diff --git a/include/sdf/SDFImpl.hh b/include/sdf/SDFImpl.hh
index 2db9918..96963a2 100644
--- a/include/sdf/SDFImpl.hh
+++ b/include/sdf/SDFImpl.hh
@@ -18,7 +18,8 @@
#define _SDFIMPL_HH_
#include <string>
-#include <boost/shared_ptr.hpp>
+#include <memory>
+#include <functional>
#include "sdf/Param.hh"
#include "sdf/Element.hh"
@@ -41,8 +42,8 @@ namespace sdf
class SDFORMAT_VISIBLE SDF;
/// \def SDFPtr
- /// \brief boost shared pointer to SDF
- typedef boost::shared_ptr<SDF> SDFPtr;
+ /// \brief Shared pointer to SDF
+ typedef std::shared_ptr<SDF> SDFPtr;
/// \addtogroup sdf
/// \{
@@ -70,7 +71,7 @@ namespace sdf
/// and empty string if the file was not found in the callback.
/// \param[in] _cb The callback function.
SDFORMAT_VISIBLE
- void setFindCallback(boost::function<std::string (const std::string &)> _cb);
+ void setFindCallback(std::function<std::string (const std::string &)> _cb);
/// \brief Base SDF class
class SDFORMAT_VISIBLE SDF
diff --git a/include/sdf/Types.hh b/include/sdf/Types.hh
index c51fae9..b800329 100644
--- a/include/sdf/Types.hh
+++ b/include/sdf/Types.hh
@@ -23,7 +23,6 @@
#include <cstdint>
#include <sstream>
#include <vector>
-#include <boost/shared_ptr.hpp>
#include "sdf/system_util.hh"
diff --git a/sdf/1.0/CMakeLists.txt b/sdf/1.0/CMakeLists.txt
index 93f8ae1..fb1b5fb 100644
--- a/sdf/1.0/CMakeLists.txt
+++ b/sdf/1.0/CMakeLists.txt
@@ -27,4 +27,4 @@ set (sdfs
world.sdf
)
-install(FILES ${sdfs} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/sdformat/1.0/)
+install(FILES ${sdfs} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat/1.0/)
diff --git a/sdf/1.2/CMakeLists.txt b/sdf/1.2/CMakeLists.txt
index 96026de..e4a15b3 100644
--- a/sdf/1.2/CMakeLists.txt
+++ b/sdf/1.2/CMakeLists.txt
@@ -28,4 +28,4 @@ set (sdfs
world.sdf
)
-install(FILES ${sdfs} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/sdformat/1.2)
+install(FILES ${sdfs} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat/1.2)
diff --git a/sdf/1.3/CMakeLists.txt b/sdf/1.3/CMakeLists.txt
index e78a1b3..6eecf92 100644
--- a/sdf/1.3/CMakeLists.txt
+++ b/sdf/1.3/CMakeLists.txt
@@ -29,4 +29,4 @@ set (sdfs
world.sdf
)
-install(FILES ${sdfs} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/sdformat/1.3)
+install(FILES ${sdfs} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat/1.3)
diff --git a/sdf/1.4/CMakeLists.txt b/sdf/1.4/CMakeLists.txt
index 21ee3eb..b617d80 100644
--- a/sdf/1.4/CMakeLists.txt
+++ b/sdf/1.4/CMakeLists.txt
@@ -44,4 +44,4 @@ set (sdfs
world.sdf
)
-install(FILES ${sdfs} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/sdformat/1.4)
+install(FILES ${sdfs} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat/1.4)
diff --git a/sdf/1.5/CMakeLists.txt b/sdf/1.5/CMakeLists.txt
index 38bfc52..581b9c6 100644
--- a/sdf/1.5/CMakeLists.txt
+++ b/sdf/1.5/CMakeLists.txt
@@ -9,6 +9,7 @@ set (sdfs
collision.sdf
contact.sdf
cylinder_shape.sdf
+ frame.sdf
forcetorque.sdf
geometry.sdf
gps.sdf
@@ -35,6 +36,7 @@ set (sdfs
plugin.sdf
polyline_shape.sdf
population.sdf
+ pose.sdf
projector.sdf
ray.sdf
rfidtag.sdf
@@ -70,9 +72,9 @@ foreach(FIL ${sdfs})
VERBATIM)
endforeach()
-add_custom_target(schema ALL DEPENDS ${SDF_SCHEMA})
+add_custom_target(schema1_5 ALL DEPENDS ${SDF_SCHEMA})
set_source_files_properties(${SDF_SCHEMA} PROPERTIES GENERATED TRUE)
-install(FILES 1_4.convert ${sdfs} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/sdformat/1.5)
-install(FILES ${SDF_SCHEMA} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/sdformat/1.5)
+install(FILES 1_4.convert ${sdfs} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat/1.5)
+install(FILES ${SDF_SCHEMA} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat/1.5)
diff --git a/sdf/1.5/actor.sdf b/sdf/1.5/actor.sdf
index d144eee..bb1fa41 100644
--- a/sdf/1.5/actor.sdf
+++ b/sdf/1.5/actor.sdf
@@ -10,9 +10,8 @@
<description>Actors should be static, in terms of physics simulation.</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>Origin of the actor</description>
- </element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<element name="skin" required="1">
<description></description>
@@ -21,7 +20,7 @@
<description></description>
</element>
- <element name="scale" type="double" default="1.0" required="0">
+ <element name="scale" type="double" default="1.0" required="0">
<description></description>
</element>
</element> <!-- End Skin -->
@@ -80,7 +79,7 @@
</element> <!-- End Waypoint -->
</element> <!-- End Action -->
</element> <!-- End Script -->
-
+
<include filename="link.sdf" required="+"/>
<include filename="joint.sdf" required="*"/>
<include filename="plugin.sdf" required="*"/>
diff --git a/sdf/1.5/audio_source.sdf b/sdf/1.5/audio_source.sdf
index 01c6159..8f8c23d 100644
--- a/sdf/1.5/audio_source.sdf
+++ b/sdf/1.5/audio_source.sdf
@@ -25,8 +25,7 @@
<description>True to make the audio source loop playback.</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>A position and orientation in the parent coordinate frame for the audio source. Position(x,y,z) and rotation (roll, pitch yaw) in the parent coordinate frame.</description>
- </element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
</element>
diff --git a/sdf/1.5/camera.sdf b/sdf/1.5/camera.sdf
index 9192e13..cf46793 100644
--- a/sdf/1.5/camera.sdf
+++ b/sdf/1.5/camera.sdf
@@ -5,10 +5,6 @@
<description>An optional name for the camera.</description>
</attribute>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>A position and orientation in the parent coordinate frame for the camera.</description>
- </element>
-
<element name="horizontal_fov" type="double" default="1.047" min="0.1" max="6.283186" required="1">
<description>Horizontal field of view</description>
</element> <!-- End Horizontal_FOV -->
@@ -128,4 +124,7 @@
</element>
</element> <!-- End Lens -->
+
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
</element> <!-- End Camera -->
diff --git a/sdf/1.5/collision.sdf b/sdf/1.5/collision.sdf
index 051732d..09c4eac 100644
--- a/sdf/1.5/collision.sdf
+++ b/sdf/1.5/collision.sdf
@@ -14,9 +14,8 @@
<description>Maximum number of contacts allowed between two entities. This value overrides the max_contacts element defined in physics.</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>The reference frame of the collision element, relative to the reference frame of the link.</description>
- </element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<include filename="geometry.sdf" required="1"/>
<include filename="surface.sdf" required="0"/>
diff --git a/sdf/1.5/frame.sdf b/sdf/1.5/frame.sdf
new file mode 100644
index 0000000..28eefc5
--- /dev/null
+++ b/sdf/1.5/frame.sdf
@@ -0,0 +1,11 @@
+<!-- Frame -->
+<element name="frame" required="*">
+ <description>A frame of reference to which a pose is relative.</description>
+
+ <attribute name="name" type="string" default="" required="1">
+ <description>Name of the frame. This name must not match another frame defined inside the parent that this frame is attached to.</description>
+ </attribute>
+
+ <include filename="pose.sdf" required="0"/>
+
+</element> <!-- End Frame -->
diff --git a/sdf/1.5/gui.sdf b/sdf/1.5/gui.sdf
index cfe6389..ecbe364 100644
--- a/sdf/1.5/gui.sdf
+++ b/sdf/1.5/gui.sdf
@@ -21,10 +21,6 @@
<description>Set the type of projection for the camera. Valid values are "perspective" and "orthographic".</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description></description>
- </element>
-
<element name="track_visual" required="0">
<description></description>
@@ -39,6 +35,11 @@
<element name="max_dist" type="double" default="0" required="0">
<description></description>
</element>
+
</element>
+
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
+
</element>
</element>
diff --git a/sdf/1.5/inertial.sdf b/sdf/1.5/inertial.sdf
index 417489f..b8160bf 100644
--- a/sdf/1.5/inertial.sdf
+++ b/sdf/1.5/inertial.sdf
@@ -6,9 +6,11 @@
<description>The mass of the link.</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>This is the pose of the inertial reference frame, relative to the link reference frame. The origin of the inertial reference frame needs to be at the center of gravity. The axes of the inertial reference frame do not need to be aligned with the principal axes of the inertia.</description>
- </element>
+ <include filename="frame.sdf" required="*"/>
+
+ <include filename="pose.sdf" required="0">
+ <description>This is the pose of the inertial reference frame, relative to the specified reference frame. The origin of the inertial reference frame needs to be at the center of gravity. The axes of the inertial reference frame do not need to be aligned with the principal axes of the inertia.</description>
+ </include>
<element name="inertia" required="0">
<description>The 3x3 rotational inertia matrix. Because the rotational inertia matrix is symmetric, only 6 above-diagonal elements of this matrix are specified here, using the attributes ixx, ixy, ixz, iyy, iyz, izz.</description>
diff --git a/sdf/1.5/joint.sdf b/sdf/1.5/joint.sdf
index 978e2c7..abc9750 100644
--- a/sdf/1.5/joint.sdf
+++ b/sdf/1.5/joint.sdf
@@ -27,10 +27,6 @@
<description>Name of the child link</description>
</element> <!-- End Child -->
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>Pose offset from child link frame to joint frame (expressed in child link frame).</description>
- </element>
-
<element name="gearbox_ratio" type="double" default="1.0" required="0">
<description>Parameter for gearbox joints. Given theta_1 and theta_2 defined in description for gearbox_reference_body, theta_2 = -gearbox_ratio * theta_1.</description>
</element>
@@ -230,5 +226,7 @@
</element>
</element> <!-- End Physics -->
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<include filename="sensor.sdf" required="*"/>
</element> <!-- End Joint -->
diff --git a/sdf/1.5/light.sdf b/sdf/1.5/light.sdf
index 745308c..336cd7f 100644
--- a/sdf/1.5/light.sdf
+++ b/sdf/1.5/light.sdf
@@ -14,9 +14,8 @@
<description>When true, the light will cast shadows.</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>A position and orientation in the global coordinate frame for the light.</description>
- </element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<element name="diffuse" type="color" default="1 1 1 1" required="1">
<description>Diffuse light color</description>
diff --git a/sdf/1.5/light_state.sdf b/sdf/1.5/light_state.sdf
index 9614cfc..af128fd 100644
--- a/sdf/1.5/light_state.sdf
+++ b/sdf/1.5/light_state.sdf
@@ -6,7 +6,6 @@
<description>Name of the light</description>
</attribute>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
- <description>Pose of the light</description>
- </element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
</element> <!-- End Light -->
diff --git a/sdf/1.5/link.sdf b/sdf/1.5/link.sdf
index e2c6ca9..d19c01b 100644
--- a/sdf/1.5/link.sdf
+++ b/sdf/1.5/link.sdf
@@ -18,10 +18,6 @@
<description>If true, the link is kinematic only</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>This is the pose of the link reference frame, relative to the model reference frame.</description>
- </element>
-
<element name="must_be_base_link" type="bool" default="false" required="0">
<description>If true, the link will have 6DOF and be a direct child of world.</description>
</element>
@@ -36,6 +32,8 @@
</element>
</element> <!-- End velocity decay -->
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<include filename="inertial.sdf" required="0"/>
<include filename="collision.sdf" required="*"/>
<include filename="visual.sdf" required="*"/>
diff --git a/sdf/1.5/link_state.sdf b/sdf/1.5/link_state.sdf
index c324fb9..5269415 100644
--- a/sdf/1.5/link_state.sdf
+++ b/sdf/1.5/link_state.sdf
@@ -6,10 +6,6 @@
<description>Name of the link</description>
</attribute>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
- <description>Pose of the link relative to the model</description>
- </element>
-
<element name="velocity" type="pose" default="0 0 0 0 0 0" required="0">
<description>Velocity of the link. The x, y, z components of the pose
correspond to the linear velocity of the link, and the roll, pitch, yaw
@@ -37,10 +33,9 @@
<attribute name="name" type="string" default="__default__" required="1">
<description>Name of the collision</description>
</attribute>
-
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
- <description>Pose of the link relative to the model</description>
- </element>
</element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
+
</element> <!-- End Link -->
diff --git a/sdf/1.5/model.sdf b/sdf/1.5/model.sdf
index 8c3765c..5f7b583 100644
--- a/sdf/1.5/model.sdf
+++ b/sdf/1.5/model.sdf
@@ -18,10 +18,8 @@
<description>Allows a model to auto-disable, which is means the physics engine can skip updating the model when the model is at rest. This parameter is only used by models with no joints.</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>A position and orientation in the global coordinate frame for the model. Position(x,y,z) and rotation (roll, pitch yaw) in the global coordinate frame.</description>
- </element>
-
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<include filename="link.sdf" required="*"/>
<include filename="joint.sdf" required="*"/>
<include filename="plugin.sdf" required="*"/>
diff --git a/sdf/1.5/model_state.sdf b/sdf/1.5/model_state.sdf
index cb3b6db..27b65e8 100644
--- a/sdf/1.5/model_state.sdf
+++ b/sdf/1.5/model_state.sdf
@@ -6,10 +6,6 @@
<description>Name of the model</description>
</attribute>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
- <description>Pose of the model</description>
- </element>
-
<element name="joint" required="*">
<description>Joint angle</description>
@@ -33,6 +29,9 @@
</attribute>
</element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
+
<include filename="link_state.sdf" required="*"/>
</element> <!-- End Model -->
diff --git a/sdf/1.5/population.sdf b/sdf/1.5/population.sdf
index c1ee322..2f97e61 100644
--- a/sdf/1.5/population.sdf
+++ b/sdf/1.5/population.sdf
@@ -12,10 +12,6 @@
</description>
</attribute>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>The reference frame of the population's region.</description>
- </element><!-- End Pose -->
-
<include filename="box_shape.sdf" required="0"/>
<include filename="cylinder_shape.sdf" required="0"/>
@@ -55,6 +51,9 @@
</element><!-- End Distribution -->
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
+
<include filename="model.sdf" required="1"/>
-</element> <!-- End Population -->
\ No newline at end of file
+</element> <!-- End Population -->
diff --git a/sdf/1.5/pose.sdf b/sdf/1.5/pose.sdf
new file mode 100644
index 0000000..7cee8ed
--- /dev/null
+++ b/sdf/1.5/pose.sdf
@@ -0,0 +1,9 @@
+<!-- Pose -->
+<element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
+ <description>A position(x,y,z) and orientation(roll, pitch yaw) with respect to the specified frame.</description>
+
+ <attribute name="frame" type="string" default="" required="*">
+ <description>Name of frame which the pose is defined relative to.</description>
+ </attribute>
+
+</element> <!-- End Pose -->
diff --git a/sdf/1.5/projector.sdf b/sdf/1.5/projector.sdf
index 9eabca9..67de493 100644
--- a/sdf/1.5/projector.sdf
+++ b/sdf/1.5/projector.sdf
@@ -8,11 +8,6 @@
<description>Texture name</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>Pose of the projector</description>
- </element>
-
-
<element name="fov" type="double" default="0.785" required="0">
<description>Field of view</description>
</element>
@@ -27,6 +22,8 @@
<description>far clip distance</description>
</element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<include filename="plugin.sdf" required="*"/>
</element>
diff --git a/sdf/1.5/sensor.sdf b/sdf/1.5/sensor.sdf
index d8feb3f..1c503e3 100644
--- a/sdf/1.5/sensor.sdf
+++ b/sdf/1.5/sensor.sdf
@@ -39,14 +39,12 @@
<description>If true, the sensor is visualized in the GUI</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>This is the pose of the sensor, relative to the parent (link or joint) reference frame.</description>
- </element>
-
<element name="topic" type="string" default="__default__" required="0">
<description>Name of the topic on which data is published. This is necessary for visualization</description>
</element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<include filename="plugin.sdf" required="*"/>
<include filename="altimeter.sdf" required="0"/>
<include filename="camera.sdf" required="0"/>
diff --git a/sdf/1.5/visual.sdf b/sdf/1.5/visual.sdf
index bf807ad..04b29c7 100644
--- a/sdf/1.5/visual.sdf
+++ b/sdf/1.5/visual.sdf
@@ -18,10 +18,6 @@
<description>The amount of transparency( 0=opaque, 1 = fully transparent)</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>The reference frame of the visual element, relative to the reference frame of the link.</description>
- </element>
-
<element name="meta" required="0">
<description>Optional meta information for the visual. The information contained within this element should be used to provide additional feedback to an end user.</description>
@@ -30,6 +26,9 @@
</element>
</element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
+
<include filename="material.sdf" required="0"/>
<include filename="geometry.sdf" required="1"/>
<include filename="plugin.sdf" required="*"/>
diff --git a/sdf/1.5/world.sdf b/sdf/1.5/world.sdf
index 9f537d0..c906a40 100644
--- a/sdf/1.5/world.sdf
+++ b/sdf/1.5/world.sdf
@@ -19,10 +19,6 @@
<description>URI to a resource, such as a model</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>Override the pose of the included model. A position and orientation in the global coordinate frame for the model. Position(x,y,z) and rotation (roll, pitch yaw) in the global coordinate frame.</description>
- </element>
-
<element name="name" type="string" default="" required="0">
<description>Override the name of the included model.</description>
</element>
@@ -30,6 +26,8 @@
<element name="static" type="bool" default="false" required="0">
<description>Override the static value of the included model.</description>
</element>
+
+ <include filename="pose.sdf" required="0"/>
</element>
diff --git a/sdf/1.6/1_5.convert b/sdf/1.6/1_5.convert
new file mode 100644
index 0000000..121091d
--- /dev/null
+++ b/sdf/1.6/1_5.convert
@@ -0,0 +1,331 @@
+<convert name="sdf">
+
+ <!-- Move <gravity> and <magnetic_field> -->
+ <convert name="world">
+ <move>
+ <from element="physics::gravity"/>
+ <to element="gravity"/>
+ </move>
+ <move>
+ <from element="physics::magnetic_field"/>
+ <to element="magnetic_field"/>
+ </move>
+ </convert>
+
+ <convert name="world">
+ <convert name="model">
+ <convert name="link">
+ <convert name="sensor">
+ <convert name="imu">
+ <!-- Add <angular_velocity> and <linear_acceleration> -->
+ <add element="angular_velocity"/>
+ <add element="linear_acceleration"/>
+
+
+ <!-- Move the noise type into <angular_velocity> and
+ <linear_acceleration> -->
+ <copy>
+ <from element="noise::type"/>
+ <to element="angular_velocity::x::noise" attribute="type"/>
+ </copy>
+ <copy>
+ <from element="noise::type"/>
+ <to element="angular_velocity::y::noise" attribute="type"/>
+ </copy>
+ <copy>
+ <from element="noise::type"/>
+ <to element="angular_velocity::z::noise" attribute="type"/>
+ </copy>
+
+ <!-- Copy <noise><type> to <linear_acceleration> -->
+ <copy>
+ <from element="noise::type"/>
+ <to element="linear_acceleration::x::noise" attribute="type"/>
+ </copy>
+ <copy>
+ <from element="noise::type"/>
+ <to element="linear_acceleration::y::noise" attribute="type"/>
+ </copy>
+ <move>
+ <from element="noise::type"/>
+ <to element="linear_acceleration::z::noise" attribute="type"/>
+ </move>
+
+ <!-- Copy <rate><mean> into <angular_velocity> -->
+ <copy>
+ <from element="noise::rate::mean"/>
+ <to element="angular_velocity::x::noise::mean"/>
+ </copy>
+ <copy>
+ <from element="noise::rate::mean"/>
+ <to element="angular_velocity::y::noise::mean"/>
+ </copy>
+ <move>
+ <from element="noise::rate::mean"/>
+ <to element="angular_velocity::z::noise::mean"/>
+ </move>
+
+ <!-- Copy <rate><stddev> into <angular_velocity> -->
+ <copy>
+ <from element="noise::rate::stddev"/>
+ <to element="angular_velocity::x::noise::stddev"/>
+ </copy>
+ <copy>
+ <from element="noise::rate::stddev"/>
+ <to element="angular_velocity::y::noise::stddev"/>
+ </copy>
+ <move>
+ <from element="noise::rate::stddev"/>
+ <to element="angular_velocity::z::noise::stddev"/>
+ </move>
+
+ <!-- Copy <rate><bias_mean> into <angular_velocity> -->
+ <copy>
+ <from element="noise::rate::bias_mean"/>
+ <to element="angular_velocity::x::noise::bias_mean"/>
+ </copy>
+ <copy>
+ <from element="noise::rate::bias_mean"/>
+ <to element="angular_velocity::y::noise::bias_mean"/>
+ </copy>
+ <move>
+ <from element="noise::rate::bias_mean"/>
+ <to element="angular_velocity::z::noise::bias_mean"/>
+ </move>
+
+ <!-- Copy <rate><bias_stddev> into <angular_velocity> -->
+ <copy>
+ <from element="noise::rate::bias_stddev"/>
+ <to element="angular_velocity::x::noise::bias_stddev"/>
+ </copy>
+ <copy>
+ <from element="noise::rate::bias_stddev"/>
+ <to element="angular_velocity::y::noise::bias_stddev"/>
+ </copy>
+ <move>
+ <from element="noise::rate::bias_stddev"/>
+ <to element="angular_velocity::z::noise::bias_stddev"/>
+ </move>
+
+ <!-- Copy <accel><mean> into <linear_acceleration> -->
+ <copy>
+ <from element="noise::accel::mean"/>
+ <to element="linear_acceleration::x::noise::mean"/>
+ </copy>
+ <copy>
+ <from element="noise::accel::mean"/>
+ <to element="linear_acceleration::y::noise::mean"/>
+ </copy>
+ <move>
+ <from element="noise::accel::mean"/>
+ <to element="linear_acceleration::z::noise::mean"/>
+ </move>
+
+ <!-- Copy <accel><stddev> into <linear_acceleration> -->
+ <copy>
+ <from element="noise::accel::stddev"/>
+ <to element="linear_acceleration::x::noise::stddev"/>
+ </copy>
+ <copy>
+ <from element="noise::accel::stddev"/>
+ <to element="linear_acceleration::y::noise::stddev"/>
+ </copy>
+ <move>
+ <from element="noise::accel::stddev"/>
+ <to element="linear_acceleration::z::noise::stddev"/>
+ </move>
+
+ <!-- Copy <accel><bias_mean> into <linear_acceleration> -->
+ <copy>
+ <from element="noise::accel::bias_mean"/>
+ <to element="linear_acceleration::x::noise::bias_mean"/>
+ </copy>
+ <copy>
+ <from element="noise::accel::bias_mean"/>
+ <to element="linear_acceleration::y::noise::bias_mean"/>
+ </copy>
+ <move>
+ <from element="noise::accel::bias_mean"/>
+ <to element="linear_acceleration::z::noise::bias_mean"/>
+ </move>
+
+ <!-- Copy <accel><bias_stddev> into <linear_acceleration> -->
+ <copy>
+ <from element="noise::accel::bias_stddev"/>
+ <to element="linear_acceleration::x::noise::bias_stddev"/>
+ </copy>
+ <copy>
+ <from element="noise::accel::bias_stddev"/>
+ <to element="linear_acceleration::y::noise::bias_stddev"/>
+ </copy>
+ <move>
+ <from element="noise::accel::bias_stddev"/>
+ <to element="linear_acceleration::z::noise::bias_stddev"/>
+ </move>
+
+ <!-- Remove <accel> and <rate> -->
+ <remove element="noise"/>
+ </convert>
+ </convert>
+ </convert>
+ </convert>
+ </convert>
+
+ <!-- for model sdfs -->
+ <convert name="model">
+ <convert name="link">
+ <convert name="sensor">
+ <convert name="imu">
+ <!-- Add <angular_velocity> and <linear_acceleration -->
+ <add element="angular_velocity"/>
+ <add element="linear_acceleration"/>
+
+
+ <!-- Move the noise type into <angular_velocity> and
+ <linear_acceleration> -->
+ <copy>
+ <from element="noise::type"/>
+ <to element="angular_velocity::x::noise" attribute="type"/>
+ </copy>
+ <copy>
+ <from element="noise::type"/>
+ <to element="angular_velocity::y::noise" attribute="type"/>
+ </copy>
+ <copy>
+ <from element="noise::type"/>
+ <to element="angular_velocity::z::noise" attribute="type"/>
+ </copy>
+
+ <!-- Copy <noise><type> to <linear_acceleration> -->
+ <copy>
+ <from element="noise::type"/>
+ <to element="linear_acceleration::x::noise" attribute="type"/>
+ </copy>
+ <copy>
+ <from element="noise::type"/>
+ <to element="linear_acceleration::y::noise" attribute="type"/>
+ </copy>
+ <move>
+ <from element="noise::type"/>
+ <to element="linear_acceleration::z::noise" attribute="type"/>
+ </move>
+
+ <!-- Copy <rate><mean> into <angular_velocity> -->
+ <copy>
+ <from element="noise::rate::mean"/>
+ <to element="angular_velocity::x::noise::mean"/>
+ </copy>
+ <copy>
+ <from element="noise::rate::mean"/>
+ <to element="angular_velocity::y::noise::mean"/>
+ </copy>
+ <move>
+ <from element="noise::rate::mean"/>
+ <to element="angular_velocity::z::noise::mean"/>
+ </move>
+
+ <!-- Copy <rate><stddev> into <angular_velocity> -->
+ <copy>
+ <from element="noise::rate::stddev"/>
+ <to element="angular_velocity::x::noise::stddev"/>
+ </copy>
+ <copy>
+ <from element="noise::rate::stddev"/>
+ <to element="angular_velocity::y::noise::stddev"/>
+ </copy>
+ <move>
+ <from element="noise::rate::stddev"/>
+ <to element="angular_velocity::z::noise::stddev"/>
+ </move>
+
+ <!-- Copy <rate><bias_mean> into <angular_velocity> -->
+ <copy>
+ <from element="noise::rate::bias_mean"/>
+ <to element="angular_velocity::x::noise::bias_mean"/>
+ </copy>
+ <copy>
+ <from element="noise::rate::bias_mean"/>
+ <to element="angular_velocity::y::noise::bias_mean"/>
+ </copy>
+ <move>
+ <from element="noise::rate::bias_mean"/>
+ <to element="angular_velocity::z::noise::bias_mean"/>
+ </move>
+
+ <!-- Copy <rate><bias_stddev> into <angular_velocity> -->
+ <copy>
+ <from element="noise::rate::bias_stddev"/>
+ <to element="angular_velocity::x::noise::bias_stddev"/>
+ </copy>
+ <copy>
+ <from element="noise::rate::bias_stddev"/>
+ <to element="angular_velocity::y::noise::bias_stddev"/>
+ </copy>
+ <move>
+ <from element="noise::rate::bias_stddev"/>
+ <to element="angular_velocity::z::noise::bias_stddev"/>
+ </move>
+
+ <!-- Copy <accel><mean> into <linear_acceleration> -->
+ <copy>
+ <from element="noise::accel::mean"/>
+ <to element="linear_acceleration::x::noise::mean"/>
+ </copy>
+ <copy>
+ <from element="noise::accel::mean"/>
+ <to element="linear_acceleration::y::noise::mean"/>
+ </copy>
+ <move>
+ <from element="noise::accel::mean"/>
+ <to element="linear_acceleration::z::noise::mean"/>
+ </move>
+
+ <!-- Copy <accel><stddev> into <linear_acceleration> -->
+ <copy>
+ <from element="noise::accel::stddev"/>
+ <to element="linear_acceleration::x::noise::stddev"/>
+ </copy>
+ <copy>
+ <from element="noise::accel::stddev"/>
+ <to element="linear_acceleration::y::noise::stddev"/>
+ </copy>
+ <move>
+ <from element="noise::accel::stddev"/>
+ <to element="linear_acceleration::z::noise::stddev"/>
+ </move>
+
+ <!-- Copy <accel><bias_mean> into <linear_acceleration> -->
+ <copy>
+ <from element="noise::accel::bias_mean"/>
+ <to element="linear_acceleration::x::noise::bias_mean"/>
+ </copy>
+ <copy>
+ <from element="noise::accel::bias_mean"/>
+ <to element="linear_acceleration::y::noise::bias_mean"/>
+ </copy>
+ <move>
+ <from element="noise::accel::bias_mean"/>
+ <to element="linear_acceleration::z::noise::bias_mean"/>
+ </move>
+
+ <!-- Copy <accel><bias_stddev> into <linear_acceleration> -->
+ <copy>
+ <from element="noise::accel::bias_stddev"/>
+ <to element="linear_acceleration::x::noise::bias_stddev"/>
+ </copy>
+ <copy>
+ <from element="noise::accel::bias_stddev"/>
+ <to element="linear_acceleration::y::noise::bias_stddev"/>
+ </copy>
+ <move>
+ <from element="noise::accel::bias_stddev"/>
+ <to element="linear_acceleration::z::noise::bias_stddev"/>
+ </move>
+
+ <!-- Remove <accel> and <rate> -->
+ <remove element="noise"/>
+ </convert>
+ </convert>
+ </convert>
+ </convert>
+</convert> <!-- End SDF -->
diff --git a/sdf/1.5/CMakeLists.txt b/sdf/1.6/CMakeLists.txt
similarity index 83%
copy from sdf/1.5/CMakeLists.txt
copy to sdf/1.6/CMakeLists.txt
index 38bfc52..5e1884e 100644
--- a/sdf/1.5/CMakeLists.txt
+++ b/sdf/1.6/CMakeLists.txt
@@ -1,6 +1,7 @@
set (sdfs
actor.sdf
altimeter.sdf
+ atmosphere.sdf
audio_source.sdf
audio_sink.sdf
battery.sdf
@@ -9,6 +10,7 @@ set (sdfs
collision.sdf
contact.sdf
cylinder_shape.sdf
+ frame.sdf
forcetorque.sdf
geometry.sdf
gps.sdf
@@ -35,6 +37,7 @@ set (sdfs
plugin.sdf
polyline_shape.sdf
population.sdf
+ pose.sdf
projector.sdf
ray.sdf
rfidtag.sdf
@@ -70,9 +73,9 @@ foreach(FIL ${sdfs})
VERBATIM)
endforeach()
-add_custom_target(schema ALL DEPENDS ${SDF_SCHEMA})
+add_custom_target(schema1_6 ALL DEPENDS ${SDF_SCHEMA})
set_source_files_properties(${SDF_SCHEMA} PROPERTIES GENERATED TRUE)
-install(FILES 1_4.convert ${sdfs} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/sdformat/1.5)
-install(FILES ${SDF_SCHEMA} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/sdformat/1.5)
+install(FILES 1_5.convert ${sdfs} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat/1.6)
+install(FILES ${SDF_SCHEMA} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat/1.6)
diff --git a/sdf/1.5/actor.sdf b/sdf/1.6/actor.sdf
similarity index 95%
copy from sdf/1.5/actor.sdf
copy to sdf/1.6/actor.sdf
index d144eee..bb1fa41 100644
--- a/sdf/1.5/actor.sdf
+++ b/sdf/1.6/actor.sdf
@@ -10,9 +10,8 @@
<description>Actors should be static, in terms of physics simulation.</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>Origin of the actor</description>
- </element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<element name="skin" required="1">
<description></description>
@@ -21,7 +20,7 @@
<description></description>
</element>
- <element name="scale" type="double" default="1.0" required="0">
+ <element name="scale" type="double" default="1.0" required="0">
<description></description>
</element>
</element> <!-- End Skin -->
@@ -80,7 +79,7 @@
</element> <!-- End Waypoint -->
</element> <!-- End Action -->
</element> <!-- End Script -->
-
+
<include filename="link.sdf" required="+"/>
<include filename="joint.sdf" required="*"/>
<include filename="plugin.sdf" required="*"/>
diff --git a/sdf/1.6/altimeter.sdf b/sdf/1.6/altimeter.sdf
new file mode 100644
index 0000000..66ddee5
--- /dev/null
+++ b/sdf/1.6/altimeter.sdf
@@ -0,0 +1,18 @@
+<element name="altimeter" required="0">
+ <description>These elements are specific to an altimeter sensor.</description>
+
+ <element name="vertical_position" required="0">
+ <description>
+ Noise parameters for vertical position
+ </description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+
+ <element name="vertical_velocity" required="0">
+ <description>
+ Noise parameters for vertical velocity
+ </description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+
+</element>
diff --git a/sdf/1.6/atmosphere.sdf b/sdf/1.6/atmosphere.sdf
new file mode 100644
index 0000000..447a4d4
--- /dev/null
+++ b/sdf/1.6/atmosphere.sdf
@@ -0,0 +1,25 @@
+<!-- Atmosphere -->
+<element name="atmosphere" required="1">
+ <description>The atmosphere tag specifies the type and properties of the atmosphere model.</description>
+
+ <attribute name="type" type="string" default="adiabatic" required="1">
+ <description>The type of the atmosphere engine. Current options are adiabatic. Defaults to adiabatic if left unspecified.</description>
+ </attribute>
+
+ <element name="temperature" type="double" default="288.15" required="0">
+ <description>Temperature at sea level in kelvins.</description>
+ </element>
+
+ <element name="pressure" type="double" default="101325" required="0">
+ <description>Pressure at sea level in pascals.</description>
+ </element>
+
+ <element name="mass_density" type="double" default="1.225" required="0">
+ <description>Mass density of the air at sea level in kg/m^3.</description>
+ </element>
+
+ <element name="temperature_gradient" type="double" default="-0.0065" required="0">
+ <description>Temperature gradient with respect to increasing altitude at sea level in units of K/m.</description>
+ </element>
+
+</element> <!-- Atmosphere -->
diff --git a/sdf/1.6/audio_sink.sdf b/sdf/1.6/audio_sink.sdf
new file mode 100644
index 0000000..d3bd071
--- /dev/null
+++ b/sdf/1.6/audio_sink.sdf
@@ -0,0 +1,4 @@
+<!-- Audio Sink -->
+<element name="audio_sink" required="*">
+ <description>An audio sink.</description>
+</element>
diff --git a/sdf/1.5/audio_source.sdf b/sdf/1.6/audio_source.sdf
similarity index 79%
copy from sdf/1.5/audio_source.sdf
copy to sdf/1.6/audio_source.sdf
index 01c6159..8f8c23d 100644
--- a/sdf/1.5/audio_source.sdf
+++ b/sdf/1.6/audio_source.sdf
@@ -25,8 +25,7 @@
<description>True to make the audio source loop playback.</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>A position and orientation in the parent coordinate frame for the audio source. Position(x,y,z) and rotation (roll, pitch yaw) in the parent coordinate frame.</description>
- </element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
</element>
diff --git a/sdf/1.6/battery.sdf b/sdf/1.6/battery.sdf
new file mode 100644
index 0000000..dc6f8de
--- /dev/null
+++ b/sdf/1.6/battery.sdf
@@ -0,0 +1,12 @@
+<!-- Battery -->
+<element name="battery" required="*">
+ <description>Description of a battery.</description>
+
+ <attribute name="name" type="string" default="__default__" required="1">
+ <description>Unique name for the battery.</description>
+ </attribute>
+
+ <element name="voltage" type="double" default="0.0" required="1">
+ <description>Initial voltage in volts.</description>
+ </element>
+</element>
diff --git a/sdf/1.6/box_shape.sdf b/sdf/1.6/box_shape.sdf
new file mode 100644
index 0000000..826ab37
--- /dev/null
+++ b/sdf/1.6/box_shape.sdf
@@ -0,0 +1,6 @@
+<element name="box" required="0">
+ <description>Box shape</description>
+ <element name="size" type="vector3" default="1 1 1" required="1">
+ <description>The three side lengths of the box. The origin of the box is in its geometric center (inside the center of the box).</description>
+ </element>
+</element>
diff --git a/sdf/1.5/camera.sdf b/sdf/1.6/camera.sdf
similarity index 97%
copy from sdf/1.5/camera.sdf
copy to sdf/1.6/camera.sdf
index 9192e13..cf46793 100644
--- a/sdf/1.5/camera.sdf
+++ b/sdf/1.6/camera.sdf
@@ -5,10 +5,6 @@
<description>An optional name for the camera.</description>
</attribute>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>A position and orientation in the parent coordinate frame for the camera.</description>
- </element>
-
<element name="horizontal_fov" type="double" default="1.047" min="0.1" max="6.283186" required="1">
<description>Horizontal field of view</description>
</element> <!-- End Horizontal_FOV -->
@@ -128,4 +124,7 @@
</element>
</element> <!-- End Lens -->
+
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
</element> <!-- End Camera -->
diff --git a/sdf/1.5/collision.sdf b/sdf/1.6/collision.sdf
similarity index 82%
copy from sdf/1.5/collision.sdf
copy to sdf/1.6/collision.sdf
index 051732d..09c4eac 100644
--- a/sdf/1.5/collision.sdf
+++ b/sdf/1.6/collision.sdf
@@ -14,9 +14,8 @@
<description>Maximum number of contacts allowed between two entities. This value overrides the max_contacts element defined in physics.</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>The reference frame of the collision element, relative to the reference frame of the link.</description>
- </element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<include filename="geometry.sdf" required="1"/>
<include filename="surface.sdf" required="0"/>
diff --git a/sdf/1.6/collision_engine.sdf b/sdf/1.6/collision_engine.sdf
new file mode 100644
index 0000000..67dbc70
--- /dev/null
+++ b/sdf/1.6/collision_engine.sdf
@@ -0,0 +1,17 @@
+<!-- Collision Engine -->
+<element name="collision_engine" required="1">
+ <description>The collision_engine tag specifies the type and properties of the collision detection engine.</description>
+
+ <element name="ode" required="0">
+ <attribute name="type" type="string" default="__default__" required="0">
+ <description>The type of the collision detection engine. Current default in ODE is OPCODE.</description>
+ </attribute>
+ </element>
+
+ <element name="bullet" required="0">
+ <attribute name="type" type="string" default="__default__" required="0">
+ <description>The type of the collision detection engine.</description>
+ </attribute>
+ </element>
+
+</element> <!-- Collision Engine -->
diff --git a/sdf/1.6/contact.sdf b/sdf/1.6/contact.sdf
new file mode 100644
index 0000000..d1cde50
--- /dev/null
+++ b/sdf/1.6/contact.sdf
@@ -0,0 +1,12 @@
+<element name="contact" required="0">
+ <description>These elements are specific to the contact sensor.</description>
+
+ <element name="collision" type="string" default="__default__" required="1">
+ <description>name of the collision element within a link that acts as the contact sensor.</description>
+ </element> <!-- End Collision -->
+
+ <element name="topic" type="string" default="__default_topic__" required="1">
+ <description>Topic on which contact data is published.</description>
+ </element>
+
+</element> <!-- End Contact -->
diff --git a/sdf/1.6/cylinder_shape.sdf b/sdf/1.6/cylinder_shape.sdf
new file mode 100644
index 0000000..8e25a53
--- /dev/null
+++ b/sdf/1.6/cylinder_shape.sdf
@@ -0,0 +1,9 @@
+<element name="cylinder" required="0">
+ <description>Cylinder shape</description>
+ <element name="radius" type="double" default="1" required="1">
+ <description>Radius of the cylinder</description>
+ </element>
+ <element name="length" type="double" default="1" required="1">
+ <description>Length of the cylinder</description>
+ </element>
+</element>
diff --git a/sdf/1.6/forcetorque.sdf b/sdf/1.6/forcetorque.sdf
new file mode 100644
index 0000000..08388cf
--- /dev/null
+++ b/sdf/1.6/forcetorque.sdf
@@ -0,0 +1,20 @@
+<element name="force_torque" required="0">
+ <description>These elements are specific to the force torque sensor.</description>
+ <element name="frame" type="string" default="child" required="0">
+ <description>
+ Frame in which to report the wrench values. Currently supported frames are:
+ "parent" report the wrench expressed in the orientation of the parent link frame,
+ "child" report the wrench expressed in the orientation of the child link frame,
+ "sensor" report the wrench expressed in the orientation of the joint sensor frame.
+ Note that for each option the point with respect to which the
+ torque component of the wrench is expressed is the joint origin.
+ </description>
+ </element>
+ <element name="measure_direction" type="string" default="child_to_parent" required="0">
+ <description>
+ Direction of the wrench measured by the sensor. The supported options are:
+ "parent_to_child" if the measured wrench is the one applied by parent link on the child link,
+ "child_to_parent" if the measured wrench is the one applied by the child link on the parent link.
+ </description>
+ </element>
+</element>
diff --git a/sdf/1.6/frame.sdf b/sdf/1.6/frame.sdf
new file mode 100644
index 0000000..28eefc5
--- /dev/null
+++ b/sdf/1.6/frame.sdf
@@ -0,0 +1,11 @@
+<!-- Frame -->
+<element name="frame" required="*">
+ <description>A frame of reference to which a pose is relative.</description>
+
+ <attribute name="name" type="string" default="" required="1">
+ <description>Name of the frame. This name must not match another frame defined inside the parent that this frame is attached to.</description>
+ </attribute>
+
+ <include filename="pose.sdf" required="0"/>
+
+</element> <!-- End Frame -->
diff --git a/sdf/1.6/geometry.sdf b/sdf/1.6/geometry.sdf
new file mode 100644
index 0000000..5fe95ed
--- /dev/null
+++ b/sdf/1.6/geometry.sdf
@@ -0,0 +1,18 @@
+<!-- Geometry -->
+<element name="geometry" required="1">
+ <description>The shape of the visual or collision object.</description>
+
+ <element name="empty" required="0">
+ <description>You can use the empty tag to make empty geometries.</description>
+ </element> <!-- End empty -->
+
+ <include filename="box_shape.sdf" required="0"/>
+ <include filename="cylinder_shape.sdf" required="0"/>
+ <include filename="heightmap_shape.sdf" required="0"/>
+ <include filename="image_shape.sdf" required="0"/>
+ <include filename="mesh_shape.sdf" required="0"/>
+ <include filename="plane_shape.sdf" required="0"/>
+ <include filename="polyline_shape.sdf" required="0"/>
+ <include filename="sphere_shape.sdf" required="0"/>
+
+</element><!-- End Geometry -->
diff --git a/sdf/1.6/gps.sdf b/sdf/1.6/gps.sdf
new file mode 100644
index 0000000..95e0049
--- /dev/null
+++ b/sdf/1.6/gps.sdf
@@ -0,0 +1,40 @@
+<element name="gps" required="0">
+ <description>These elements are specific to the GPS sensor.</description>
+
+ <element name="position_sensing" required="0">
+ <description>
+ Parameters related to GPS position measurement.
+ </description>
+ <element name="horizontal" required="0">
+ <description>
+ Noise parameters for horizontal position measurement, in units of meters.
+ </description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+ <element name="vertical" required="0">
+ <description>
+ Noise parameters for vertical position measurement, in units of meters.
+ </description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+ </element>
+
+ <element name="velocity_sensing" required="0">
+ <description>
+ Parameters related to GPS position measurement.
+ </description>
+ <element name="horizontal" required="0">
+ <description>
+ Noise parameters for horizontal velocity measurement, in units of meters/second.
+ </description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+ <element name="vertical" required="0">
+ <description>
+ Noise parameters for vertical velocity measurement, in units of meters/second.
+ </description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+ </element>
+
+</element>
diff --git a/sdf/1.6/gripper.sdf b/sdf/1.6/gripper.sdf
new file mode 100644
index 0000000..12fd0b3
--- /dev/null
+++ b/sdf/1.6/gripper.sdf
@@ -0,0 +1,30 @@
+<!-- Gripper -->
+<element name="gripper" required="*">
+ <description></description>
+
+ <attribute name="name" type="string" default="__default__" required="1">
+ <description></description>
+ </attribute>
+
+ <element name="grasp_check" required="0">
+ <description></description>
+ <element name="detach_steps" type="int" default="40" required="0">
+ <description></description>
+ </element>
+ <element name="attach_steps" type="int" default="20" required="0">
+ <description></description>
+ </element>
+ <element name="min_contact_count" type="unsigned int" default="2" required="0">
+ <description></description>
+ </element>
+ </element>
+
+ <element name="gripper_link" type="string" default="__default__" required="+">
+ <description></description>
+ </element>
+
+ <element name="palm_link" type="string" default="__default__" required="1">
+ <description></description>
+ </element>
+
+</element>
diff --git a/sdf/1.5/gui.sdf b/sdf/1.6/gui.sdf
similarity index 91%
copy from sdf/1.5/gui.sdf
copy to sdf/1.6/gui.sdf
index cfe6389..ecbe364 100644
--- a/sdf/1.5/gui.sdf
+++ b/sdf/1.6/gui.sdf
@@ -21,10 +21,6 @@
<description>Set the type of projection for the camera. Valid values are "perspective" and "orthographic".</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description></description>
- </element>
-
<element name="track_visual" required="0">
<description></description>
@@ -39,6 +35,11 @@
<element name="max_dist" type="double" default="0" required="0">
<description></description>
</element>
+
</element>
+
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
+
</element>
</element>
diff --git a/sdf/1.6/heightmap_shape.sdf b/sdf/1.6/heightmap_shape.sdf
new file mode 100644
index 0000000..087f4ca
--- /dev/null
+++ b/sdf/1.6/heightmap_shape.sdf
@@ -0,0 +1,40 @@
+<element name="heightmap" required="0">
+ <description>A heightmap based on a 2d grayscale image.</description>
+ <element name="uri" type="string" default="__default__" required="1">
+ <description>URI to a grayscale image file</description>
+ </element>
+ <element name="size" type="vector3" default="1 1 1" required="0">
+ <description>The size of the heightmap in world units.
+ When loading an image: "size" is used if present, otherwise defaults to 1x1x1.
+ When loading a DEM: "size" is used if present, otherwise defaults to true size of DEM.
+ </description>
+ </element>
+ <element name="pos" type="vector3" default="0 0 0" required="0">
+ <description>A position offset.</description>
+ </element>
+
+ <element name="texture" required="*">
+ <description>The heightmap can contain multiple textures. The order of the texture matters. The first texture will appear at the lowest height, and the last texture at the highest height. Use blend to control the height thresholds and fade between textures.</description>
+ <element name="size" type="double" default="10" required="1">
+ <description>Size of the applied texture in meters.</description>
+ </element>
+ <element name="diffuse" type="string" default="__default__" required="1">
+ <description>Diffuse texture image filename</description>
+ </element>
+ <element name="normal" type="string" default="__default__" required="1">
+ <description>Normalmap texture image filename</description>
+ </element>
+ </element>
+ <element name="blend" required="*">
+ <description>The blend tag controls how two adjacent textures are mixed. The number of blend elements should equal one less than the number of textures.</description>
+ <element name="min_height" type="double" default="0" required="1">
+ <description>Min height of a blend layer</description>
+ </element>
+ <element name="fade_dist" type="double" default="0" required="1">
+ <description>Distance over which the blend occurs</description>
+ </element>
+ </element>
+ <element name="use_terrain_paging" type="bool" default="false" required="0">
+ <description>Set if the rendering engine will use terrain paging</description>
+ </element>
+</element>
diff --git a/sdf/1.6/image_shape.sdf b/sdf/1.6/image_shape.sdf
new file mode 100644
index 0000000..369de7c
--- /dev/null
+++ b/sdf/1.6/image_shape.sdf
@@ -0,0 +1,18 @@
+<element name="image" required="0">
+ <description>Extrude a set of boxes from a grayscale image.</description>
+ <element name="uri" type="string" default="__default__" required="1">
+ <description>URI of the grayscale image file</description>
+ </element>
+ <element name="scale" type="double" default="1" required="1">
+ <description>Scaling factor applied to the image</description>
+ </element>
+ <element name="threshold" type="int" default="200" required="1">
+ <description>Grayscale threshold</description>
+ </element>
+ <element name="height" type="double" default="1" required="1">
+ <description>Height of the extruded boxes</description>
+ </element>
+ <element name="granularity" type="int" default="1" required="1">
+ <description>The amount of error in the model</description>
+ </element>
+</element>
diff --git a/sdf/1.6/imu.sdf b/sdf/1.6/imu.sdf
new file mode 100644
index 0000000..f9a3067
--- /dev/null
+++ b/sdf/1.6/imu.sdf
@@ -0,0 +1,42 @@
+<element name="imu" required="0">
+ <description>These elements are specific to the IMU sensor.</description>
+
+ <element name="topic" type="string" default="__default_topic__" required="0">
+ <description>Topic on which data is published.</description>
+ </element>
+
+ <element name="angular_velocity" required="0">
+ <description>These elements are specific to body-frame angular velocity,
+ which is expressed in radians per second</description>
+ <element name="x" required="0">
+ <description>Angular velocity about the X axis</description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+ <element name="y" required="0">
+ <description>Angular velocity about the Y axis</description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+ <element name="z" required="0">
+ <description>Angular velocity about the Z axis</description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+ </element>
+
+ <element name="linear_acceleration" required="0">
+ <description>These elements are specific to body-frame linear acceleration,
+ which is expressed in meters per second squared</description>
+ <element name="x" required="0">
+ <description>Linear acceleration about the X axis</description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+ <element name="y" required="0">
+ <description>Linear acceleration about the Y axis</description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+ <element name="z" required="0">
+ <description>Linear acceleration about the Z axis</description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+ </element>
+
+</element>
diff --git a/sdf/1.5/inertial.sdf b/sdf/1.6/inertial.sdf
similarity index 79%
copy from sdf/1.5/inertial.sdf
copy to sdf/1.6/inertial.sdf
index 417489f..b8160bf 100644
--- a/sdf/1.5/inertial.sdf
+++ b/sdf/1.6/inertial.sdf
@@ -6,9 +6,11 @@
<description>The mass of the link.</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>This is the pose of the inertial reference frame, relative to the link reference frame. The origin of the inertial reference frame needs to be at the center of gravity. The axes of the inertial reference frame do not need to be aligned with the principal axes of the inertia.</description>
- </element>
+ <include filename="frame.sdf" required="*"/>
+
+ <include filename="pose.sdf" required="0">
+ <description>This is the pose of the inertial reference frame, relative to the specified reference frame. The origin of the inertial reference frame needs to be at the center of gravity. The axes of the inertial reference frame do not need to be aligned with the principal axes of the inertia.</description>
+ </include>
<element name="inertia" required="0">
<description>The 3x3 rotational inertia matrix. Because the rotational inertia matrix is symmetric, only 6 above-diagonal elements of this matrix are specified here, using the attributes ixx, ixy, ixz, iyy, iyz, izz.</description>
diff --git a/sdf/1.5/joint.sdf b/sdf/1.6/joint.sdf
similarity index 98%
copy from sdf/1.5/joint.sdf
copy to sdf/1.6/joint.sdf
index 978e2c7..abc9750 100644
--- a/sdf/1.5/joint.sdf
+++ b/sdf/1.6/joint.sdf
@@ -27,10 +27,6 @@
<description>Name of the child link</description>
</element> <!-- End Child -->
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>Pose offset from child link frame to joint frame (expressed in child link frame).</description>
- </element>
-
<element name="gearbox_ratio" type="double" default="1.0" required="0">
<description>Parameter for gearbox joints. Given theta_1 and theta_2 defined in description for gearbox_reference_body, theta_2 = -gearbox_ratio * theta_1.</description>
</element>
@@ -230,5 +226,7 @@
</element>
</element> <!-- End Physics -->
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<include filename="sensor.sdf" required="*"/>
</element> <!-- End Joint -->
diff --git a/sdf/1.5/light.sdf b/sdf/1.6/light.sdf
similarity index 93%
copy from sdf/1.5/light.sdf
copy to sdf/1.6/light.sdf
index 745308c..336cd7f 100644
--- a/sdf/1.5/light.sdf
+++ b/sdf/1.6/light.sdf
@@ -14,9 +14,8 @@
<description>When true, the light will cast shadows.</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>A position and orientation in the global coordinate frame for the light.</description>
- </element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<element name="diffuse" type="color" default="1 1 1 1" required="1">
<description>Diffuse light color</description>
diff --git a/sdf/1.5/light_state.sdf b/sdf/1.6/light_state.sdf
similarity index 68%
copy from sdf/1.5/light_state.sdf
copy to sdf/1.6/light_state.sdf
index 9614cfc..af128fd 100644
--- a/sdf/1.5/light_state.sdf
+++ b/sdf/1.6/light_state.sdf
@@ -6,7 +6,6 @@
<description>Name of the light</description>
</attribute>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
- <description>Pose of the light</description>
- </element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
</element> <!-- End Light -->
diff --git a/sdf/1.5/link.sdf b/sdf/1.6/link.sdf
similarity index 89%
copy from sdf/1.5/link.sdf
copy to sdf/1.6/link.sdf
index e2c6ca9..d1fb2e7 100644
--- a/sdf/1.5/link.sdf
+++ b/sdf/1.6/link.sdf
@@ -10,6 +10,10 @@
<description>If true, the link is affected by gravity.</description>
</element>
+ <element name="enable_wind" type="bool" default="false" required="0">
+ <description>If true, the link is affected by the wind.</description>
+ </element>
+
<element name="self_collide" type="bool" default="false" required="0">
<description>If true, the link can collide with other links in the model. Two links within a model will collide if link1.self_collide OR link2.self_collide. Links connected by a joint will never collide.</description>
</element>
@@ -18,10 +22,6 @@
<description>If true, the link is kinematic only</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>This is the pose of the link reference frame, relative to the model reference frame.</description>
- </element>
-
<element name="must_be_base_link" type="bool" default="false" required="0">
<description>If true, the link will have 6DOF and be a direct child of world.</description>
</element>
@@ -36,6 +36,8 @@
</element>
</element> <!-- End velocity decay -->
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<include filename="inertial.sdf" required="0"/>
<include filename="collision.sdf" required="*"/>
<include filename="visual.sdf" required="*"/>
diff --git a/sdf/1.5/link_state.sdf b/sdf/1.6/link_state.sdf
similarity index 82%
copy from sdf/1.5/link_state.sdf
copy to sdf/1.6/link_state.sdf
index c324fb9..5269415 100644
--- a/sdf/1.5/link_state.sdf
+++ b/sdf/1.6/link_state.sdf
@@ -6,10 +6,6 @@
<description>Name of the link</description>
</attribute>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
- <description>Pose of the link relative to the model</description>
- </element>
-
<element name="velocity" type="pose" default="0 0 0 0 0 0" required="0">
<description>Velocity of the link. The x, y, z components of the pose
correspond to the linear velocity of the link, and the roll, pitch, yaw
@@ -37,10 +33,9 @@
<attribute name="name" type="string" default="__default__" required="1">
<description>Name of the collision</description>
</attribute>
-
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
- <description>Pose of the link relative to the model</description>
- </element>
</element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
+
</element> <!-- End Link -->
diff --git a/sdf/1.6/logical_camera.sdf b/sdf/1.6/logical_camera.sdf
new file mode 100644
index 0000000..de47703
--- /dev/null
+++ b/sdf/1.6/logical_camera.sdf
@@ -0,0 +1,19 @@
+<element name="logical_camera" required="0">
+ <description>These elements are specific to logical camera sensors. A logical camera reports objects that fall within a frustum. Computation should be performed on the CPU.</description>
+
+ <element name="near" type="double" default="0" required="1">
+ <description>Near clipping distance of the view frustum</description>
+ </element>
+
+ <element name="far" type="double" default="1" required="1">
+ <description>Far clipping distance of the view frustum</description>
+ </element>
+
+ <element name="aspect_ratio" type="double" default="1" required="1">
+ <description>Aspect ratio of the near and far planes. This is the width divided by the height of the near or far planes.</description>
+ </element>
+
+ <element name="horizontal_fov" type="double" default="1" required="1">
+ <description>Horizontal field of view of the frustum, in radians. This is the angle between the frustum's vertex and the edges of the near or far plane.</description>
+ </element>
+</element>
diff --git a/sdf/1.6/magnetometer.sdf b/sdf/1.6/magnetometer.sdf
new file mode 100644
index 0000000..2f3bfee
--- /dev/null
+++ b/sdf/1.6/magnetometer.sdf
@@ -0,0 +1,21 @@
+<element name="magnetometer" required="0">
+ <description>These elements are specific to a Magnetometer sensor.</description>
+ <element name="x" required="0">
+ <description>
+ Parameters related to the body-frame X axis of the magnetometer
+ </description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+ <element name="y" required="0">
+ <description>
+ Parameters related to the body-frame Y axis of the magnetometer
+ </description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+ <element name="z" required="0">
+ <description>
+ Parameters related to the body-frame Z axis of the magnetometer
+ </description>
+ <include filename="noise.sdf" required="0"/>
+ </element>
+</element>
\ No newline at end of file
diff --git a/sdf/1.6/material.sdf b/sdf/1.6/material.sdf
new file mode 100644
index 0000000..137436e
--- /dev/null
+++ b/sdf/1.6/material.sdf
@@ -0,0 +1,47 @@
+<!-- Material -->
+<element name="material" required="0">
+ <description>The material of the visual element.</description>
+
+ <element name="script" required="0">
+ <description>Name of material from an installed script file. This will override the color element if the script exists.</description>
+
+ <element name="uri" type="string" default="__default__" required="+">
+ <description>URI of the material script file</description>
+ </element>
+
+ <element name="name" type="string" default="__default__" required="1">
+ <description>Name of the script within the script file</description>
+ </element>
+ </element>
+
+ <element name="shader" required="0">
+
+ <attribute name="type" type="string" default="pixel" required="1">
+ <description>vertex, pixel, normal_map_objectspace, normal_map_tangentspace</description>
+ </attribute>
+
+ <element name="normal_map" type="string" default="__default__" required="0">
+ <description>filename of the normal map</description>
+ </element>
+ </element>
+
+ <element name="lighting" type="bool" default="true" required="0">
+ <description>If false, dynamic lighting will be disabled</description>
+ </element>
+
+ <element name="ambient" type="color" default="0 0 0 1" required="0">
+ <description>The ambient color of a material specified by set of four numbers representing red/green/blue, each in the range of [0,1].</description>
+ </element>
+
+ <element name="diffuse" type="color" default="0 0 0 1" required="0">
+ <description>The diffuse color of a material specified by set of four numbers representing red/green/blue/alpha, each in the range of [0,1].</description>
+ </element>
+
+ <element name="specular" type="color" default="0 0 0 1" required="0">
+ <description>The specular color of a material specified by set of four numbers representing red/green/blue/alpha, each in the range of [0,1].</description>
+ </element>
+
+ <element name="emissive" type="color" default="0 0 0 1" required="0">
+ <description>The emissive color of a material specified by set of four numbers representing red/green/blue, each in the range of [0,1].</description>
+ </element>
+</element> <!-- End Material -->
diff --git a/sdf/1.6/mesh_shape.sdf b/sdf/1.6/mesh_shape.sdf
new file mode 100644
index 0000000..61bce76
--- /dev/null
+++ b/sdf/1.6/mesh_shape.sdf
@@ -0,0 +1,20 @@
+<element name="mesh" required="0">
+ <description>Mesh shape</description>
+ <element name="uri" type="string" default="__default__" required="1">
+ <description>Mesh uri</description>
+ </element>
+
+ <element name="submesh" required="0">
+ <description>Use a named submesh. The submesh must exist in the mesh specified by the uri</description>
+ <element name="name" type="string" default="__default__" required="1">
+ <description>Name of the submesh within the parent mesh</description>
+ </element>
+ <element name="center" type="bool" default="false" required="0">
+ <description>Set to true to center the vertices of the submesh at 0,0,0. This will effectively remove any transformations on the submesh before the poses from parent links and models are applied.</description>
+ </element>
+ </element> <!-- End submesh -->
+
+ <element name="scale" type="vector3" default="1 1 1" required="0">
+ <description>Scaling factor applied to the mesh</description>
+ </element>
+</element>
diff --git a/sdf/1.5/model.sdf b/sdf/1.6/model.sdf
similarity index 90%
copy from sdf/1.5/model.sdf
copy to sdf/1.6/model.sdf
index 8c3765c..54e13ff 100644
--- a/sdf/1.5/model.sdf
+++ b/sdf/1.6/model.sdf
@@ -18,10 +18,8 @@
<description>Allows a model to auto-disable, which is means the physics engine can skip updating the model when the model is at rest. This parameter is only used by models with no joints.</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>A position and orientation in the global coordinate frame for the model. Position(x,y,z) and rotation (roll, pitch yaw) in the global coordinate frame.</description>
- </element>
-
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<include filename="link.sdf" required="*"/>
<include filename="joint.sdf" required="*"/>
<include filename="plugin.sdf" required="*"/>
@@ -53,4 +51,8 @@
</attribute>
</element>
+ <element name="enable_wind" type="bool" default="false" required="0">
+ <description>If set to true, all links in the model will be affected by the wind. Can be overriden by the link wind property.</description>
+ </element>
+
</element> <!-- End Model -->
diff --git a/sdf/1.5/model_state.sdf b/sdf/1.6/model_state.sdf
similarity index 83%
copy from sdf/1.5/model_state.sdf
copy to sdf/1.6/model_state.sdf
index cb3b6db..3fd296f 100644
--- a/sdf/1.5/model_state.sdf
+++ b/sdf/1.6/model_state.sdf
@@ -6,10 +6,6 @@
<description>Name of the model</description>
</attribute>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="1">
- <description>Pose of the model</description>
- </element>
-
<element name="joint" required="*">
<description>Joint angle</description>
@@ -33,6 +29,13 @@
</attribute>
</element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
+
+ <element name="scale" type="vector3" default="1 1 1" required="0">
+ <description>Scale for the 3 dimensions of the model.</description>
+ </element>
+
<include filename="link_state.sdf" required="*"/>
</element> <!-- End Model -->
diff --git a/sdf/1.6/noise.sdf b/sdf/1.6/noise.sdf
new file mode 100644
index 0000000..0542422
--- /dev/null
+++ b/sdf/1.6/noise.sdf
@@ -0,0 +1,31 @@
+<element name="noise" required="1">
+ <description>The properties of a sensor noise model.</description>
+
+ <attribute name="type" type="string" default="none" required="1">
+ <description>
+ The type of noise. Currently supported types are:
+ "none" (no noise).
+ "gaussian" (draw noise values independently for each measurement from a Gaussian distribution).
+ "gaussian_quantized" ("gaussian" plus quantization of outputs (ie. rounding))
+ </description>
+ </attribute>
+ <element name="mean" type="double" default="0.0" required="0">
+ <description>For type "gaussian*", the mean of the Gaussian distribution from which noise values are drawn.</description>
+ </element>
+ <element name="stddev" type="double" default="0.0" required="0">
+ <description>For type "gaussian*", the standard deviation of the Gaussian distribution from which noise values are drawn.</description>
+ </element>
+ <element name="bias_mean" type="double" default="0.0" required="0">
+ <description>For type "gaussian*", the mean of the Gaussian distribution from which bias values are drawn.</description>
+ </element>
+ <element name="bias_stddev" type="double" default="0.0" required="0">
+ <description>For type "gaussian*", the standard deviation of the Gaussian distribution from which bias values are drawn.</description>
+ </element>
+ <element name="precision" type="double" default="0.0" required="0">
+ <description>
+ For type "gaussian_quantized", the precision of output signals. A value
+ of zero implies infinite precision / no quantization.
+ </description>
+ </element>
+
+</element>
diff --git a/sdf/1.6/physics.sdf b/sdf/1.6/physics.sdf
new file mode 100644
index 0000000..6f1c84f
--- /dev/null
+++ b/sdf/1.6/physics.sdf
@@ -0,0 +1,205 @@
+<!-- Physics -->
+<element name="physics" required="1">
+ <description>The physics tag specifies the type and properties of the dynamics engine.</description>
+
+ <attribute name="name" type="string" default="default_physics" required="0">
+ <description>The name of this set of physics parameters.</description>
+ </attribute>
+
+ <attribute name="default" type="bool" default="false" required="0">
+ <description>If true, this physics element is set as the default physics profile for the world. If multiple default physics elements exist, the first element marked as default is chosen. If no default physics element exists, the first physics element is chosen.</description>
+ </attribute>
+
+ <attribute name="type" type="string" default="ode" required="1">
+ <description>The type of the dynamics engine. Current options are ode, bullet, simbody and rtql8. Defaults to ode if left unspecified.</description>
+ </attribute>
+
+ <element name="max_step_size" type="double" default="0.001" required="1">
+ <description>Maximum time step size at which every system in simulation can interact with the states of the world. (was physics.sdf's dt).</description>
+ </element>
+
+ <!-- real_time_factor (simulation speedup) might be more intuitive to end
+ users than real_time_update_rate -->
+ <element name="real_time_factor" type="double" default="1.0" required="1">
+ <description>target simulation speedup factor, defined by ratio of simulation time to real-time.</description>
+ </element>
+
+ <!-- to be deprecated by real_time_factor -->
+ <element name="real_time_update_rate" type="double" default="1000" required="1">
+ <description>Rate at which to update the physics engine (UpdatePhysics calls per real-time second). (was physics.sdf's update_rate).</description>
+ </element>
+
+ <element name="max_contacts" type="int" default="20" required="0">
+ <description>Maximum number of contacts allowed between two entities. This value can be over ridden by a max_contacts element in a collision element.</description>
+ </element>
+
+ <element name="simbody" required="0">
+ <description>Simbody specific physics properties</description>
+ <element name="min_step_size" type="double" default="0.0001" required="0">
+ <description>(Currently not used in simbody) The time duration which advances with each iteration of the dynamics engine, this has to be no bigger than max_step_size under physics block. If left unspecified, min_step_size defaults to max_step_size.</description>
+ </element>
+ <element name="accuracy" type="double" default="1e-3" required="0">
+ <description>Roughly the relative error of the system.
+ -LOG(accuracy) is roughly the number of significant digits.</description>
+ </element>
+ <element name="max_transient_velocity" type="double"
+ default="0.01" required="0">
+ <description>Tolerable "slip" velocity allowed by the solver when static
+ friction is supposed to hold object in place.</description>
+ </element>
+ <element name="contact" required="0">
+ <description><![CDATA[
+ Relationship among dissipation, coef. restitution, etc.
+ d = dissipation coefficient (1/velocity)
+ vc = capture velocity (velocity where e=e_max)
+ vp = plastic velocity (smallest v where e=e_min) > vc
+ Assume real COR=1 when v=0.
+ e_min = given minimum COR, at v >= vp (a.k.a. plastic_coef_restitution)
+ d = slope = (1-e_min)/vp
+ OR, e_min = 1 - d*vp
+ e_max = maximum COR = 1-d*vc, reached at v=vc
+ e = 0, v <= vc
+ = 1 - d*v, vc < v < vp
+ = e_min, v >= vp
+
+ dissipation factor = d*min(v,vp) [compliant]
+ cor = e [rigid]
+
+ Combining rule e = 0, e1==e2==0
+ = 2*e1*e2/(e1+e2), otherwise]]>
+ </description>
+
+ <element name="stiffness" type="double" default="1e8" required="0">
+ <description>Default contact material stiffness
+ (force/dist or torque/radian).</description>
+ </element>
+ <element name="dissipation" type="double" default="100" required="0">
+ <description>dissipation coefficient to be used in compliant contact;
+ if not given it is (1-min_cor)/plastic_impact_velocity</description>
+ </element>
+
+ <element name="plastic_coef_restitution" type="double"
+ default="0.5" required="0">
+ <description>this is the COR to be used at high velocities for rigid
+ impacts; if not given it is 1 - dissipation*plastic_impact_velocity
+ </description>
+ </element>
+
+ <element name="plastic_impact_velocity" type="double"
+ default="0.5" required="0">
+ <description>smallest impact velocity at which min COR is reached; set
+ to zero if you want the min COR always to be used</description>
+ </element>
+
+ <element name="static_friction" type="double" default="0.9" required="0">
+ <description>static friction (mu_s) as described by this plot: http://gazebosim.org/wiki/File:Stribeck_friction.png</description>
+ </element>
+ <element name="dynamic_friction" type="double" default="0.9" required="0">
+ <description>dynamic friction (mu_d) as described by this plot: http://gazebosim.org/wiki/File:Stribeck_friction.png</description>
+ </element>
+ <element name="viscous_friction" type="double" default="0.0" required="0">
+ <description>viscous friction (mu_v) with units of (1/velocity) as described by this plot: http://gazebosim.org/wiki/File:Stribeck_friction.png</description>
+ </element>
+
+ <element name="override_impact_capture_velocity" type="double"
+ default="0.001" required="0">
+ <description>for rigid impacts only, impact velocity at which
+ COR is set to zero; normally inherited from global default but can
+ be overridden here. Combining rule: use larger velocity</description>
+ </element>
+
+ <element name="override_stiction_transition_velocity" type="double"
+ default="0.001" required="0">
+ <description>This is the largest slip velocity at which
+ we'll consider a transition to stiction. Normally inherited
+ from a global default setting. For a continuous friction model
+ this is the velocity at which the max static friction force
+ is reached. Combining rule: use larger velocity</description>
+ </element>
+
+ </element>
+ </element>
+
+ <element name="bullet" required="0">
+ <description>Bullet specific physics properties</description>
+ <element name="solver" required="1">
+ <description></description>
+ <element name="type" type="string" default="sequential_impulse" required="1">
+ <description>One of the following types: sequential_impulse only.</description>
+ </element>
+ <element name="min_step_size" type="double" default="0.0001" required="0">
+ <description>The time duration which advances with each iteration of the dynamics engine, this has to be no bigger than max_step_size under physics block. If left unspecified, min_step_size defaults to max_step_size.</description>
+ </element>
+ <element name="iters" type="int" default="50" required="1">
+ <description>Number of iterations for each step. A higher number produces greater accuracy at a performance cost.</description>
+ </element>
+ <element name="sor" type="double" default="1.3" required="1">
+ <description>Set the successive over-relaxation parameter.</description>
+ </element>
+ </element> <!-- End Solver -->
+
+ <element name="constraints" required="1">
+ <description>Bullet constraint parameters.</description>
+ <element name="cfm" type="double" default="0" required="1">
+ <description>Constraint force mixing parameter. See the ODE page for more information.</description>
+ </element>
+ <element name="erp" type="double" default="0.2" required="1">
+ <description>Error reduction parameter. See the ODE page for more information.</description>
+ </element>
+ <element name="contact_surface_layer" type="double" default="0.001" required="1">
+ <description>The depth of the surface layer around all geometry objects. Contacts are allowed to sink into the surface layer up to the given depth before coming to rest. The default value is zero. Increasing this to some small value (e.g. 0.001) can help prevent jittering problems due to contacts being repeatedly made and broken.</description>
+ </element>
+ <element name="split_impulse" type="bool" default="1" required="1">
+ <description>Similar to ODE's max_vel implementation. See http://web.archive.org/web/20120430155635/http://bulletphysics.org/mediawiki-1.5.8/index.php/BtContactSolverInfo#Split_Impulse for more information.</description>
+ </element>
+ <element name="split_impulse_penetration_threshold" type="double" default="-0.01" required="1">
+ <description>Similar to ODE's max_vel implementation. See http://web.archive.org/web/20120430155635/http://bulletphysics.org/mediawiki-1.5.8/index.php/BtContactSolverInfo#Split_Impulse for more information.</description>
+ </element>
+ </element> <!-- End Constraints -->
+ </element>
+
+ <element name="ode" required="0">
+ <description>ODE specific physics properties</description>
+ <element name="solver" required="1">
+ <description></description>
+ <element name="type" type="string" default="quick" required="1">
+ <description>One of the following types: world, quick</description>
+ </element>
+ <element name="min_step_size" type="double" default="0.0001" required="0">
+ <description>The time duration which advances with each iteration of the dynamics engine, this has to be no bigger than max_step_size under physics block. If left unspecified, min_step_size defaults to max_step_size.</description>
+ </element>
+ <element name="iters" type="int" default="50" required="1">
+ <description>Number of iterations for each step. A higher number produces greater accuracy at a performance cost.</description>
+ </element>
+ <element name="precon_iters" type="int" default="0" required="0">
+ <description>Experimental parameter.</description>
+ </element>
+ <element name="sor" type="double" default="1.3" required="1">
+ <description>Set the successive over-relaxation parameter.</description>
+ </element>
+ <element name="use_dynamic_moi_rescaling" type="bool" default="0" required="1">
+ <description>
+ Flag to enable dynamic rescaling of moment of inertia in constrained directions.
+ See gazebo pull request 1114 for the implementation of this feature.
+ https://bitbucket.org/osrf/gazebo/pull-request/1114
+ </description>
+ </element>
+ </element> <!-- End Solver -->
+
+ <element name="constraints" required="1">
+ <description>ODE constraint parameters.</description>
+ <element name="cfm" type="double" default="0" required="1">
+ <description>Constraint force mixing parameter. See the ODE page for more information.</description>
+ </element>
+ <element name="erp" type="double" default="0.2" required="1">
+ <description>Error reduction parameter. See the ODE page for more information.</description>
+ </element>
+ <element name="contact_max_correcting_vel" type="double" default="100.0" required="1">
+ <description>The maximum correcting velocities allowed when resolving contacts.</description>
+ </element>
+ <element name="contact_surface_layer" type="double" default="0.001" required="1">
+ <description>The depth of the surface layer around all geometry objects. Contacts are allowed to sink into the surface layer up to the given depth before coming to rest. The default value is zero. Increasing this to some small value (e.g. 0.001) can help prevent jittering problems due to contacts being repeatedly made and broken.</description>
+ </element>
+ </element> <!-- End Constraints -->
+ </element> <!-- ODE -->
+</element> <!-- Physics -->
diff --git a/sdf/1.6/plane_shape.sdf b/sdf/1.6/plane_shape.sdf
new file mode 100644
index 0000000..35bca28
--- /dev/null
+++ b/sdf/1.6/plane_shape.sdf
@@ -0,0 +1,9 @@
+<element name="plane" required="0">
+ <description>Plane shape</description>
+ <element name="normal" type="vector3" default="0 0 1" required="1">
+ <description>Normal direction for the plane</description>
+ </element>
+ <element name="size" type="vector2d" default="1 1" min="0 0" required="1">
+ <description>Length of each side of the plane</description>
+ </element>
+</element>
diff --git a/sdf/1.6/plugin.sdf b/sdf/1.6/plugin.sdf
new file mode 100644
index 0000000..26405ff
--- /dev/null
+++ b/sdf/1.6/plugin.sdf
@@ -0,0 +1,13 @@
+<!-- Plugin -->
+<element name="plugin" required="*">
+ <description>A plugin is a dynamically loaded chunk of code. It can exist as a child of world, model, and sensor.</description>
+ <attribute name="name" type="string" default="__default__" required="1">
+ <description>A unique name for the plugin, scoped to its parent.</description>
+ </attribute>
+ <attribute name="filename" type="string" default="__default__" required="1">
+ <description>Name of the shared library to load. If the filename is not a full path name, the file will be searched for in the configuration paths.</description>
+ </attribute>
+ <element copy_data="true" required="*">
+ <description>This is a special element that should not be specified in an SDF file. It automatically copies child elements into the SDF element so that a plugin can access the data.</description>
+ </element>
+</element> <!-- End Plugin -->
diff --git a/sdf/1.6/polyline_shape.sdf b/sdf/1.6/polyline_shape.sdf
new file mode 100644
index 0000000..6658843
--- /dev/null
+++ b/sdf/1.6/polyline_shape.sdf
@@ -0,0 +1,14 @@
+<element name="polyline" required="0">
+ <description>Defines an extruded polyline shape</description>
+
+ <element name="point" type="vector2d" default="0 0" required="+">
+ <description>
+ A series of points that define the path of the polyline.
+ </description>
+ </element>
+
+ <element name="height" type="double" default="1.0" required="1">
+ <description>Height of the polyline</description>
+ </element>
+
+</element>
diff --git a/sdf/1.5/population.sdf b/sdf/1.6/population.sdf
similarity index 91%
copy from sdf/1.5/population.sdf
copy to sdf/1.6/population.sdf
index c1ee322..2f97e61 100644
--- a/sdf/1.5/population.sdf
+++ b/sdf/1.6/population.sdf
@@ -12,10 +12,6 @@
</description>
</attribute>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>The reference frame of the population's region.</description>
- </element><!-- End Pose -->
-
<include filename="box_shape.sdf" required="0"/>
<include filename="cylinder_shape.sdf" required="0"/>
@@ -55,6 +51,9 @@
</element><!-- End Distribution -->
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
+
<include filename="model.sdf" required="1"/>
-</element> <!-- End Population -->
\ No newline at end of file
+</element> <!-- End Population -->
diff --git a/sdf/1.6/pose.sdf b/sdf/1.6/pose.sdf
new file mode 100644
index 0000000..7cee8ed
--- /dev/null
+++ b/sdf/1.6/pose.sdf
@@ -0,0 +1,9 @@
+<!-- Pose -->
+<element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
+ <description>A position(x,y,z) and orientation(roll, pitch yaw) with respect to the specified frame.</description>
+
+ <attribute name="frame" type="string" default="" required="*">
+ <description>Name of frame which the pose is defined relative to.</description>
+ </attribute>
+
+</element> <!-- End Pose -->
diff --git a/sdf/1.5/projector.sdf b/sdf/1.6/projector.sdf
similarity index 85%
copy from sdf/1.5/projector.sdf
copy to sdf/1.6/projector.sdf
index 9eabca9..67de493 100644
--- a/sdf/1.5/projector.sdf
+++ b/sdf/1.6/projector.sdf
@@ -8,11 +8,6 @@
<description>Texture name</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>Pose of the projector</description>
- </element>
-
-
<element name="fov" type="double" default="0.785" required="0">
<description>Field of view</description>
</element>
@@ -27,6 +22,8 @@
<description>far clip distance</description>
</element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<include filename="plugin.sdf" required="*"/>
</element>
diff --git a/sdf/1.6/ray.sdf b/sdf/1.6/ray.sdf
new file mode 100644
index 0000000..5ffeea3
--- /dev/null
+++ b/sdf/1.6/ray.sdf
@@ -0,0 +1,73 @@
+<element name="ray" required="0">
+ <description>These elements are specific to the ray (laser) sensor.</description>
+
+ <element name="scan" required="1">
+ <description></description>
+ <element name="horizontal" required="1">
+ <description></description>
+
+ <element name="samples" type="unsigned int" default="640" required="1">
+ <description>The number of simulated rays to generate per complete laser sweep cycle.</description>
+ </element>
+
+ <element name="resolution" type="double" default="1" required="1">
+ <description>This number is multiplied by samples to determine the number of range data points returned. If resolution is less than one, range data is interpolated. If resolution is greater than one, range data is averaged.</description>
+ </element>
+
+ <element name="min_angle" type="double" default="0" required="1">
+ <description></description>
+ </element>
+
+ <element name="max_angle" type="double" default="0" required="1">
+ <description>Must be greater or equal to min_angle</description>
+ </element>
+
+ </element> <!-- End Horizontal -->
+
+ <element name="vertical" required="0">
+ <description></description>
+ <element name="samples" type="unsigned int" default="1" required="1">
+ <description>The number of simulated rays to generate per complete laser sweep cycle.</description>
+ </element>
+
+ <element name="resolution" type="double" default="1" required="0">
+ <description>This number is multiplied by samples to determine the number of range data points returned. If resolution is less than one, range data is interpolated. If resolution is greater than one, range data is averaged.</description>
+ </element>
+
+ <element name="min_angle" type="double" default="0" required="1">
+ <description></description>
+ </element>
+
+ <element name="max_angle" type="double" default="0" required="1">
+ <description>Must be greater or equal to min_angle</description>
+ </element>
+
+ </element> <!-- End Vertical -->
+ </element> <!-- End Scan -->
+
+ <element name="range" required="1">
+ <description>specifies range properties of each simulated ray</description>
+ <element name="min" type="double" default="0" required="1">
+ <description>The minimum distance for each ray.</description>
+ </element>
+ <element name="max" type="double" default="0" required="1">
+ <description>The maximum distance for each ray.</description>
+ </element>
+ <element name="resolution" type="double" default="0" required="0">
+ <description>Linear resolution of each ray.</description>
+ </element>
+ </element> <!-- End Range -->
+
+ <element name="noise" required="0">
+ <description>The properties of the noise model that should be applied to generated scans</description>
+ <element name="type" type="string" default="gaussian" required="1">
+ <description>The type of noise. Currently supported types are: "gaussian" (draw noise values independently for each beam from a Gaussian distribution).</description>
+ </element>
+ <element name="mean" type="double" default="0.0" required="0">
+ <description>For type "gaussian," the mean of the Gaussian distribution from which noise values are drawn.</description>
+ </element>
+ <element name="stddev" type="double" default="0.0" required="0">
+ <description>For type "gaussian," the standard deviation of the Gaussian distribution from which noise values are drawn.</description>
+ </element>
+ </element> <!-- End Noise -->
+</element> <!-- End Ray -->
diff --git a/sdf/1.6/rfid.sdf b/sdf/1.6/rfid.sdf
new file mode 100644
index 0000000..61351dd
--- /dev/null
+++ b/sdf/1.6/rfid.sdf
@@ -0,0 +1,2 @@
+<element name="rfidtag" required="0">
+</element> <!-- End rfidtag -->
diff --git a/sdf/1.6/rfidtag.sdf b/sdf/1.6/rfidtag.sdf
new file mode 100644
index 0000000..55699dc
--- /dev/null
+++ b/sdf/1.6/rfidtag.sdf
@@ -0,0 +1,2 @@
+<element name="rfid" required="0">
+</element> <!-- End RFID -->
diff --git a/sdf/1.6/road.sdf b/sdf/1.6/road.sdf
new file mode 100644
index 0000000..172e480
--- /dev/null
+++ b/sdf/1.6/road.sdf
@@ -0,0 +1,16 @@
+<!-- Model -->
+<element name="road" required="*">
+ <attribute name="name" type="string" default="__default__" required="1">
+ <description>Name of the road</description>
+ </attribute>
+
+ <element name="width" type="double" default="1.0" required="1">
+ <description>Width of the road</description>
+ </element>
+
+ <element name="point" type="vector3" default="0 0 0" required="+">
+ <description>A series of points that define the path of the road.</description>
+ </element>
+
+ <include filename="material.sdf" required="0"/>
+</element>
diff --git a/sdf/1.6/root.sdf b/sdf/1.6/root.sdf
new file mode 100644
index 0000000..47d443a
--- /dev/null
+++ b/sdf/1.6/root.sdf
@@ -0,0 +1,13 @@
+<element name="sdf" required="1">
+ <description>SDF base element.</description>
+
+ <attribute name="version" type="string" default="1.5" required="1">
+ <description>Version number of the SDF format.</description>
+ </attribute>
+
+ <include filename="world.sdf" required="*"/>
+ <include filename="model.sdf" required="*"/>
+ <include filename="actor.sdf" required="*"/>
+ <include filename="light.sdf" required="*"/>
+
+</element> <!-- End SDF -->
diff --git a/sdf/1.6/scene.sdf b/sdf/1.6/scene.sdf
new file mode 100644
index 0000000..0cdb52e
--- /dev/null
+++ b/sdf/1.6/scene.sdf
@@ -0,0 +1,82 @@
+<!-- Scene -->
+<element name="scene" required="1">
+ <description>Specifies the look of the environment.</description>
+
+ <element name="ambient" type="color" default="0.4 0.4 0.4 1.0" required="1">
+ <description>Color of the ambient light.</description>
+ </element>
+
+ <element name="background" type="color" default=".7 .7 .7 1" required="1">
+ <description>Color of the background.</description>
+ </element>
+
+ <element name="sky" required="0">
+ <description>Properties for the sky</description>
+ <element name="time" type="double" default="10.0" required="0">
+ <description>Time of day [0..24]</description>
+ </element>
+ <element name="sunrise" type="double" default="6.0" required="0">
+ <description>Sunrise time [0..24]</description>
+ </element>
+ <element name="sunset" type="double" default="20.0" required="0">
+ <description>Sunset time [0..24]</description>
+ </element>
+
+ <element name="clouds" required="0">
+ <description>Sunset time [0..24]</description>
+ <element name="speed" type="double" default="0.6" min="0.0" required="0">
+ <description>Speed of the clouds</description>
+ </element>
+
+ <element name="direction" type="double" default="0.0"
+ min="0.0" max="3.1456" required="0">
+ <description>Direction of the cloud movement</description>
+ </element>
+ <element name="humidity" type="double" default="0.5"
+ min="0" max="1" required="0">
+ <description>Density of clouds</description>
+ </element>
+
+ <element name="mean_size" type="double" default="0.5"
+ min="0" max="1" required="0">
+ <description>Average size of the clouds</description>
+ </element>
+
+ <element name="ambient" type="color" default=".8 .8 .8 1" required="0">
+ <description>Ambient cloud color</description>
+ </element>
+ </element>
+ </element>
+
+ <element name="shadows" type="bool" default="true" required="1">
+ <description>Enable/disable shadows</description>
+ </element>
+
+ <element name="fog" required="0">
+ <description>Controls fog</description>
+ <element name="color" type="color" default="1 1 1 1" required="0">
+ <description>Fog color</description>
+ </element>
+ <element name="type" type="string" default="none" required="0">
+ <description>Fog type: constant, linear, quadratic</description>
+ </element>
+ <element name="start" type="double" default="1.0" min="0.0" required="0">
+ <description>Distance to start of fog</description>
+ </element>
+ <element name="end" type="double" default="100.0" min="0.0" required="0">
+ <description>Distance to end of fog</description>
+ </element>
+ <element name="density" type="double" default="1.0" min="0.0" required="0">
+ <description>Density of fog</description>
+ </element>
+ </element>
+
+ <element name="grid" type="bool" default="true" required="0">
+ <description>Enable/disable the grid</description>
+ </element>
+
+ <element name="origin_visual" type="bool" default="true" required="0">
+ <description>Show/hide world origin indicator</description>
+ </element>
+
+</element> <!-- End Scene -->
diff --git a/sdf/1.6/schema/types.xsd b/sdf/1.6/schema/types.xsd
new file mode 100644
index 0000000..53f3d8d
--- /dev/null
+++ b/sdf/1.6/schema/types.xsd
@@ -0,0 +1,44 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<xsd:schema xmlns:xsd='http://www.w3.org/2001/XMLSchema'>
+ <xsd:simpleType name="vector3">
+ <xsd:restriction base="xsd:string">
+ <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+)\s+){2}((-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+))\s*"/>
+ </xsd:restriction>
+ </xsd:simpleType>
+
+ <xsd:simpleType name="quaternion">
+ <xsd:restriction base="xsd:string">
+ <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+)\s+){3}((-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+))\s*"/>
+ </xsd:restriction>
+ </xsd:simpleType>
+
+ <xsd:simpleType name="vector2d">
+ <xsd:restriction base="xsd:string">
+ <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+)\s+)((-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+))\s*"/>
+ </xsd:restriction>
+ </xsd:simpleType>
+
+ <xsd:simpleType name="vector2i">
+ <xsd:restriction base="xsd:string">
+ <xsd:pattern value="\s*(-|\+)?\d+\s+(-|\+)?\d+\s*"/>
+ </xsd:restriction>
+ </xsd:simpleType>
+
+ <xsd:simpleType name="pose">
+ <xsd:restriction base="xsd:string">
+ <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+)\s+){5}((-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+))\s*"/>
+ </xsd:restriction>
+ </xsd:simpleType>
+
+ <xsd:simpleType name="time">
+ <xsd:restriction base="xsd:double">
+ </xsd:restriction>
+ </xsd:simpleType>
+
+ <xsd:simpleType name="color">
+ <xsd:restriction base="xsd:string">
+ <xsd:pattern value="(\s*\+?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+)\s+){3}\+?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+)\s*"/>
+ </xsd:restriction>
+ </xsd:simpleType>
+
+</xsd:schema>
diff --git a/sdf/1.5/sensor.sdf b/sdf/1.6/sensor.sdf
similarity index 92%
copy from sdf/1.5/sensor.sdf
copy to sdf/1.6/sensor.sdf
index d8feb3f..1c503e3 100644
--- a/sdf/1.5/sensor.sdf
+++ b/sdf/1.6/sensor.sdf
@@ -39,14 +39,12 @@
<description>If true, the sensor is visualized in the GUI</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>This is the pose of the sensor, relative to the parent (link or joint) reference frame.</description>
- </element>
-
<element name="topic" type="string" default="__default__" required="0">
<description>Name of the topic on which data is published. This is necessary for visualization</description>
</element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
<include filename="plugin.sdf" required="*"/>
<include filename="altimeter.sdf" required="0"/>
<include filename="camera.sdf" required="0"/>
diff --git a/sdf/1.6/sonar.sdf b/sdf/1.6/sonar.sdf
new file mode 100644
index 0000000..d27017f
--- /dev/null
+++ b/sdf/1.6/sonar.sdf
@@ -0,0 +1,13 @@
+<element name="sonar" required="0">
+ <description>These elements are specific to the sonar sensor.</description>
+ <element name="min" type="double" default="0" required="1">
+ <description>Minimum range</description>
+ </element>
+ <element name="max" type="double" default="1.0" required="1">
+ <description>Max range</description>
+ </element>
+
+ <element name="radius" type="double" default="0.5" required="1">
+ <description>Radius of the sonar cone at max range.</description>
+ </element>
+</element>
diff --git a/sdf/1.6/sphere_shape.sdf b/sdf/1.6/sphere_shape.sdf
new file mode 100644
index 0000000..73ad03e
--- /dev/null
+++ b/sdf/1.6/sphere_shape.sdf
@@ -0,0 +1,6 @@
+<element name="sphere" required="0">
+ <description>Sphere shape</description>
+ <element name="radius" type="double" default="1" required="1">
+ <description>radius of the sphere</description>
+ </element>
+</element>
diff --git a/sdf/1.6/spherical_coordinates.sdf b/sdf/1.6/spherical_coordinates.sdf
new file mode 100644
index 0000000..e6c8c96
--- /dev/null
+++ b/sdf/1.6/spherical_coordinates.sdf
@@ -0,0 +1,39 @@
+<element name="spherical_coordinates" required="0">
+ <element name="surface_model" type="string" default="EARTH_WGS84" required="1">
+ <description>
+ Name of planetary surface model, used to determine the surface altitude
+ at a given latitude and longitude. The default is an ellipsoid model of
+ the earth based on the WGS-84 standard. It is used in Gazebo's GPS sensor
+ implementation.
+ </description>
+ </element>
+
+ <element name="latitude_deg" type="double" default="0.0" required="1">
+ <description>
+ Geodetic latitude at origin of gazebo reference frame, specified
+ in units of degrees.
+ </description>
+ </element>
+
+ <element name="longitude_deg" type="double" default="0.0" required="1">
+ <description>
+ Longitude at origin of gazebo reference frame, specified in units
+ of degrees.
+ </description>
+ </element>
+
+ <element name="elevation" type="double" default="0.0" required="1">
+ <description>
+ Elevation of origin of gazebo reference frame, specified in meters.
+ </description>
+ </element>
+
+ <element name="heading_deg" type="double" default="0.0" required="1">
+ <description>
+ Heading offset of gazebo reference frame, measured as angle between
+ East and gazebo x axis, or equivalently, the angle between North and
+ gazebo y axis. The angle is specified in degrees.
+ </description>
+ </element>
+
+</element>
diff --git a/sdf/1.6/state.sdf b/sdf/1.6/state.sdf
new file mode 100644
index 0000000..9e23d81
--- /dev/null
+++ b/sdf/1.6/state.sdf
@@ -0,0 +1,40 @@
+<!-- State Info -->
+<element name="state" required="*">
+ <!-- Name of the world this state applies to -->
+ <attribute name="world_name" type="string" default="__default__" required="1">
+ <description>Name of the world this state applies to</description>
+ </attribute>
+
+ <element name="sim_time" type="time" default="0 0" required="0">
+ <description>Simulation time stamp of the state [seconds nanoseconds]</description>
+ </element>
+
+ <element name="wall_time" type="time" default="0 0" required="0">
+ <description>Wall time stamp of the state [seconds nanoseconds]</description>
+ </element>
+
+ <element name="real_time" type="time" default="0 0" required="0">
+ <description>Real time stamp of the state [seconds nanoseconds]</description>
+ </element>
+
+ <element name="iterations" type="unsigned int" default="0" required="1">
+ <description>Number of simulation iterations.</description>
+ </element>
+
+ <element name="insertions" required="0">
+ <description>A list of new model names</description>
+ <include filename="model.sdf" required="+"/>
+ </element>
+
+ <element name="deletions" required="0">
+ <description>A list of deleted model names</description>
+ <element name="name" type="string" default="__default__" required="+">
+ <description>The name of a deleted model</description>
+ </element>
+ </element>
+
+ <include filename="model_state.sdf" required="*"/>
+
+ <include filename="light_state.sdf" required="*"/>
+
+</element> <!-- End State -->
diff --git a/sdf/1.6/surface.sdf b/sdf/1.6/surface.sdf
new file mode 100644
index 0000000..7545103
--- /dev/null
+++ b/sdf/1.6/surface.sdf
@@ -0,0 +1,185 @@
+<element name="surface" required="0">
+ <description>The surface parameters</description>
+ <element name="bounce" required="0">
+ <description></description>
+ <element name="restitution_coefficient" type="double" default="0" min="0.0" max="1.0" required="0">
+ <description>Bounciness coefficient of restitution, from [0...1], where 0=no bounciness.</description>
+ </element>
+ <element name="threshold" type="double" default="100000" required="0">
+ <description>Bounce capture velocity, below which effective coefficient of restitution is 0.</description>
+ </element>
+ </element> <!-- End Bounce -->
+
+ <element name="friction" required="0">
+ <description></description>
+
+ <element name="torsional" required="0">
+ <description>Parameters for torsional friction</description>
+ <element name="coefficient" type="double" default="1.0" min="0.0" required="0">
+ <description>Torsional friction coefficient in the range of [0..1].</description>
+ </element>
+ <element name="use_patch_radius" type="bool" default="1" required="0">
+ <description>
+ If this flag is true,
+ torsional friction is calculated using the "patch_radius" parameter.
+ If this flag is set to false,
+ "surface_radius" (R) and contact depth (d)
+ are used to compute the patch radius as sqrt(R*d).
+ </description>
+ </element>
+ <element name="patch_radius" type="double" default="0" min="0.0" required="0">
+ <description>Radius of contact patch surface.</description>
+ </element>
+ <element name="surface_radius" type="double" default="0.0" min="0.0" required="0">
+ <description>Surface radius on the point of contact.</description>
+ </element>
+ <element name="ode" required="0">
+ <description>Torsional friction parameters for ODE</description>
+ <element name="slip" type="double" default="0.0" required="0">
+ <description>Force dependent slip for torsional friction, between the range of [0..1].</description>
+ </element>
+ </element> <!-- End ODE -->
+ </element> <!-- End torsional -->
+
+ <element name="ode" required="0">
+ <description>ODE friction parameters</description>
+ <element name="mu" type="double" default="1" min="0.0" required="0">
+ <description>Coefficient of friction in the range of [0..1].</description>
+ </element>
+ <element name="mu2" type="double" default="1" min="0.0" required="0">
+ <description>Second coefficient of friction in the range of [0..1]</description>
+ </element>
+ <element name="fdir1" type="vector3" default="0 0 0" required="0">
+ <description>3-tuple specifying direction of mu1 in the collision local reference frame.</description>
+ </element>
+ <element name="slip1" type="double" default="0.0" required="0">
+ <description>Force dependent slip direction 1 in collision local frame, between the range of [0..1].</description>
+ </element>
+ <element name="slip2" type="double" default="0.0" required="0">
+ <description>Force dependent slip direction 2 in collision local frame, between the range of [0..1].</description>
+ </element>
+ </element> <!-- End ODE -->
+ <element name="bullet" required="0">
+ <element name="friction" type="double" default="1" min="0.0" required="0">
+ <description>Coefficient of friction in the range of [0..1].</description>
+ </element>
+ <element name="friction2" type="double" default="1" min="0.0" required="0">
+ <description>Coefficient of friction in the range of [0..1].</description>
+ </element>
+ <element name="fdir1" type="vector3" default="0 0 0" required="0">
+ <description>3-tuple specifying direction of mu1 in the collision local reference frame.</description>
+ </element>
+ <element name="rolling_friction" type="double" default="1" required="0">
+ <description> coefficient of friction in the range of [0..1]</description>
+ </element>
+ </element> <!-- End Bullet -->
+ </element> <!-- End Friction -->
+
+ <element name="contact" required="0">
+ <description></description>
+ <element name="collide_without_contact" type="bool" default="0" required="0">
+ <description>Flag to disable contact force generation, while still allowing collision checks and contact visualization to occur.</description>
+ </element>
+ <element name="collide_without_contact_bitmask" type="unsigned int" default="1" required="0">
+ <description>Bitmask for collision filtering when collide_without_contact is on </description>
+ </element>
+
+ <element name="collide_bitmask" type="unsigned int" default="65535" required="0">
+ <description>Bitmask for collision filtering. This will override collide_without_contact</description>
+ </element>
+
+ <element name="poissons_ratio" type="double" default="0.3" required="0">
+ <description>
+ Poisson's ratio is the ratio between transverse and axial strain.
+ This value must lie between (-1, 0.5). Defaults to 0.3 for typical steel.
+ Note typical silicone elastomers have Poisson's ratio near 0.49 ~ 0.50.
+
+ For reference, approximate values for Material:(Young's Modulus, Poisson's Ratio)
+ for some of the typical materials are:
+ Plastic: (1e8 ~ 3e9 Pa, 0.35 ~ 0.41),
+ Wood: (4e9 ~ 1e10 Pa, 0.22 ~ 0.50),
+ Aluminum: (7e10 Pa, 0.32 ~ 0.35),
+ Steel: (2e11 Pa, 0.26 ~ 0.31).
+ </description>
+ </element>
+ <element name="elastic_modulus" type="double" default="-1" required="0">
+ <description>
+ Young's Modulus in SI derived unit Pascal.
+ Defaults to -1. If value is less or equal to zero,
+ contact using elastic modulus (with Poisson's Ratio) is disabled.
+
+ For reference, approximate values for Material:(Young's Modulus, Poisson's Ratio)
+ for some of the typical materials are:
+ Plastic: (1e8 ~ 3e9 Pa, 0.35 ~ 0.41),
+ Wood: (4e9 ~ 1e10 Pa, 0.22 ~ 0.50),
+ Aluminum: (7e10 Pa, 0.32 ~ 0.35),
+ Steel: (2e11 Pa, 0.26 ~ 0.31).
+ </description>
+ </element>
+
+ <element name="ode" required="0">
+ <description>ODE contact parameters</description>
+ <element name="soft_cfm" type="double" default="0" required="0">
+ <description>Soft constraint force mixing.</description>
+ </element>
+ <element name="soft_erp" type="double" default="0.2" required="0">
+ <description>Soft error reduction parameter</description>
+ </element>
+ <element name="kp" type="double" default="1000000000000.0" required="0">
+ <description>dynamically "stiffness"-equivalent coefficient for contact joints</description>
+ </element>
+ <element name="kd" type="double" default="1.0" required="0">
+ <description>dynamically "damping"-equivalent coefficient for contact joints</description>
+ </element>
+ <element name="max_vel" type="double" default="0.01" required="0">
+ <description>maximum contact correction velocity truncation term.</description>
+ </element>
+ <element name="min_depth" type="double" default="0" required="0">
+ <description>minimum allowable depth before contact correction impulse is applied</description>
+ </element>
+ </element> <!-- End ODE -->
+ <element name="bullet" required="0">
+ <description>Bullet contact parameters</description>
+ <element name="soft_cfm" type="double" default="0" required="0">
+ <description>Soft constraint force mixing.</description>
+ </element>
+ <element name="soft_erp" type="double" default="0.2" required="0">
+ <description>Soft error reduction parameter</description>
+ </element>
+ <element name="kp" type="double" default="1000000000000.0" required="0">
+ <description>dynamically "stiffness"-equivalent coefficient for contact joints</description>
+ </element>
+ <element name="kd" type="double" default="1.0" required="0">
+ <description>dynamically "damping"-equivalent coefficient for contact joints</description>
+ </element>
+ <element name="split_impulse" type="bool" default="1" required="1">
+ <description>Similar to ODE's max_vel implementation. See http://bulletphysics.org/mediawiki-1.5.8/index.php/BtContactSolverInfo#Split_Impulse for more information.</description>
+ </element>
+ <element name="split_impulse_penetration_threshold" type="double" default="-0.01" required="1">
+ <description>Similar to ODE's max_vel implementation. See http://bulletphysics.org/mediawiki-1.5.8/index.php/BtContactSolverInfo#Split_Impulse for more information.</description>
+ </element>
+ </element> <!-- End Bullet -->
+ </element> <!-- End Contact -->
+
+ <!-- for deformable bodies -->
+ <element name="soft_contact" required="0">
+ <element name="dart" required="0">
+ <description>soft contact pamameters based on paper:
+ http://www.cc.gatech.edu/graphics/projects/Sumit/homepage/papers/sigasia11/jain_softcontacts_siga11.pdf
+ </description>
+ <element name="bone_attachment" type="double" default="100.0" required="1">
+ <description>This is variable k_v in the soft contacts paper. Its unit is N/m.</description>
+ </element>
+ <element name="stiffness" type="double" default="100.0" required="1">
+ <description>This is variable k_e in the soft contacts paper. Its unit is N/m.</description>
+ </element>
+ <element name="damping" type="double" default="10.0" required="1">
+ <description>Viscous damping of point velocity in body frame. Its unit is N/m/s.</description>
+ </element>
+ <element name="flesh_mass_fraction" type="double" default="0.05" required="1">
+ <description>Fraction of mass to be distributed among deformable nodes.</description>
+ </element>
+ </element> <!-- dart -->
+ </element> <!-- soft_contact -->
+
+</element> <!-- End Surface -->
diff --git a/sdf/1.6/transceiver.sdf b/sdf/1.6/transceiver.sdf
new file mode 100644
index 0000000..9f05b06
--- /dev/null
+++ b/sdf/1.6/transceiver.sdf
@@ -0,0 +1,34 @@
+<element name="transceiver" required="0">
+ <description>These elements are specific to a wireless transceiver.</description>
+
+ <element name="essid" type="string" default="wireless" required="0">
+ <description>Service set identifier (network name)</description>
+ </element> <!-- End Essid -->
+
+ <element name="frequency" type="double" default="2442" required="0">
+ <description>Specifies the frequency of transmission in MHz</description>
+ </element> <!-- End Frequency -->
+
+ <element name="min_frequency" type="double" default="2412" required="0">
+ <description>Only a frequency range is filtered. Here we set the lower bound (MHz).
+ </description>
+ </element> <!-- End min_frequency -->
+
+ <element name="max_frequency" type="double" default="2484" required="0">
+ <description>Only a frequency range is filtered. Here we set the upper bound (MHz).
+ </description>
+ </element> <!-- End max_frequency -->
+
+ <element name="gain" type="double" default="2.5" required="1">
+ <description>Specifies the antenna gain in dBi</description>
+ </element> <!-- End Gain -->
+
+ <element name="power" type="double" default="14.50" required="1">
+ <description>Specifies the transmission power in dBm</description>
+ </element> <!-- End Power -->
+
+ <element name="sensitivity" type="double" default="-90" required="0">
+ <description>Mininum received signal power in dBm</description>
+ </element> <!-- End Sensitivity -->
+
+</element> <!-- End Transceiver -->
diff --git a/sdf/1.6/urdf.sdf b/sdf/1.6/urdf.sdf
new file mode 100644
index 0000000..6068d5c
--- /dev/null
+++ b/sdf/1.6/urdf.sdf
@@ -0,0 +1,19 @@
+<!-- Robot URDF -->
+<element name="robot" required="*">
+ <description>The robot element defines a complete robot or any other physical object using URDF.</description>
+
+ <attribute name="name" type="string" default="__default__" required="1">
+ <description>A unique name for the model. This name must not match another model in the world.</description>
+ </attribute>
+
+ <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
+ <description>A position and orientation in the global coordinate frame for the model. Position(x,y,z) and rotation (roll, pitch yaw) in the global coordinate frame.</description>
+ </element>
+
+ <include filename="link.sdf" required="+"/>
+ <include filename="joint.sdf" required="*"/>
+ <include filename="plugin.sdf" required="*"/>
+ <include filename="gripper.sdf" required="*"/>
+
+</element> <!-- End Model -->
+
diff --git a/sdf/1.5/visual.sdf b/sdf/1.6/visual.sdf
similarity index 88%
copy from sdf/1.5/visual.sdf
copy to sdf/1.6/visual.sdf
index bf807ad..04b29c7 100644
--- a/sdf/1.5/visual.sdf
+++ b/sdf/1.6/visual.sdf
@@ -18,10 +18,6 @@
<description>The amount of transparency( 0=opaque, 1 = fully transparent)</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>The reference frame of the visual element, relative to the reference frame of the link.</description>
- </element>
-
<element name="meta" required="0">
<description>Optional meta information for the visual. The information contained within this element should be used to provide additional feedback to an end user.</description>
@@ -30,6 +26,9 @@
</element>
</element>
+ <include filename="frame.sdf" required="*"/>
+ <include filename="pose.sdf" required="0"/>
+
<include filename="material.sdf" required="0"/>
<include filename="geometry.sdf" required="1"/>
<include filename="plugin.sdf" required="*"/>
diff --git a/sdf/1.5/world.sdf b/sdf/1.6/world.sdf
similarity index 68%
copy from sdf/1.5/world.sdf
copy to sdf/1.6/world.sdf
index 9f537d0..0591084 100644
--- a/sdf/1.5/world.sdf
+++ b/sdf/1.6/world.sdf
@@ -13,16 +13,20 @@
</element>
</element>
+ <element name="wind" required="0">
+ <description>The wind tag specifies the type and properties of the wind.</description>
+
+ <element name="linear_velocity" type="vector3" default="0 0 0" required="0">
+ <description>Linear velocity of the wind.</description>
+ </element>
+ </element>
+
<element name="include" required="*">
<description>Include resources from a URI</description>
<element name="uri" type="string" default="__default__" required="1">
<description>URI to a resource, such as a model</description>
</element>
- <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
- <description>Override the pose of the included model. A position and orientation in the global coordinate frame for the model. Position(x,y,z) and rotation (roll, pitch yaw) in the global coordinate frame.</description>
- </element>
-
<element name="name" type="string" default="" required="0">
<description>Override the name of the included model.</description>
</element>
@@ -30,9 +34,19 @@
<element name="static" type="bool" default="false" required="0">
<description>Override the static value of the included model.</description>
</element>
+
+ <include filename="pose.sdf" required="0"/>
</element>
+ <element name="gravity" type="vector3" default="0 0 -9.8" required="1">
+ <description>The gravity vector in m/s^2, expressed in a coordinate frame defined by the spherical_coordinates tag.</description>
+ </element> <!-- End Gravity -->
+
+ <element name="magnetic_field" type="vector3" default="5.5645e-6 22.8758e-6 -42.3884e-6" required="1">
+ <description>The magnetic vector in Tesla, expressed in a coordinate frame defined by the spherical_coordinates tag.</description>
+ </element> <!-- End Magnetic -->
+ <include filename="atmosphere.sdf" required="1"/>
<include filename="gui.sdf" required="0"/>
<include filename="physics.sdf" required="1"/>
<include filename="scene.sdf" required="1"/>
diff --git a/sdf/CMakeLists.txt b/sdf/CMakeLists.txt
index 7eb5078..19a8b6a 100644
--- a/sdf/CMakeLists.txt
+++ b/sdf/CMakeLists.txt
@@ -3,3 +3,4 @@ add_subdirectory(1.2)
add_subdirectory(1.3)
add_subdirectory(1.4)
add_subdirectory(1.5)
+add_subdirectory(1.6)
diff --git a/src/Console.cc b/src/Console.cc
index dd2e21f..1576b89 100644
--- a/src/Console.cc
+++ b/src/Console.cc
@@ -36,7 +36,7 @@
using namespace sdf;
/// Static pointer to the console.
-static boost::shared_ptr<Console> myself;
+static std::shared_ptr<Console> myself;
static std::mutex g_instance_mutex;
//////////////////////////////////////////////////
diff --git a/src/Converter.cc b/src/Converter.cc
index f48fd8a..8a33a62 100644
--- a/src/Converter.cc
+++ b/src/Converter.cc
@@ -163,22 +163,24 @@ void Converter::ConvertImpl(TiXmlElement *_elem, TiXmlElement *_convert)
}
}
- for (TiXmlElement *renameElem = _convert->FirstChildElement("rename");
- renameElem; renameElem = renameElem->NextSiblingElement("rename"))
+ for (TiXmlElement *childElem = _convert->FirstChildElement();
+ childElem; childElem = childElem->NextSiblingElement())
{
- Rename(_elem, renameElem);
- }
-
- for (TiXmlElement *moveElem = _convert->FirstChildElement("move");
- moveElem; moveElem = moveElem->NextSiblingElement("move"))
- {
- Move(_elem, moveElem);
- }
-
- for (TiXmlElement *addElem = _convert->FirstChildElement("add");
- addElem; addElem = addElem->NextSiblingElement("add"))
- {
- Add(_elem, addElem);
+ if (childElem->ValueStr() == "rename")
+ Rename(_elem, childElem);
+ else if (childElem->ValueStr() == "copy")
+ Move(_elem, childElem, true);
+ else if (childElem->ValueStr() == "move")
+ Move(_elem, childElem, false);
+ else if (childElem->ValueStr() == "add")
+ Add(_elem, childElem);
+ else if (childElem->ValueStr() == "remove")
+ Remove(_elem, childElem);
+ else if (childElem->ValueStr() != "convert")
+ {
+ std::cerr << "Unknown convert element[" << childElem->ValueStr()
+ << "]\n";
+ }
}
}
@@ -238,11 +240,6 @@ void Converter::Add(TiXmlElement *_elem, TiXmlElement *_addElem)
const char *elementName = _addElem->Attribute("element");
const char *value = _addElem->Attribute("value");
- if (!value)
- {
- sdferr << "No 'value' specified in <add>\n";
- return;
- }
if (!((attributeName == NULL) ^ (elementName == NULL)))
{
sdferr << "Exactly one 'element' or 'attribute'"
@@ -252,19 +249,71 @@ void Converter::Add(TiXmlElement *_elem, TiXmlElement *_addElem)
if (attributeName)
{
- _elem->SetAttribute(attributeName, value);
+ if (value)
+ _elem->SetAttribute(attributeName, value);
+ else
+ {
+ sdferr << "No 'value' specified in <add>\n";
+ return;
+ }
}
else
{
TiXmlElement *addElem = new TiXmlElement(elementName);
- TiXmlText *addText = new TiXmlText(value);
- addElem->LinkEndChild(addText);
+ if (value)
+ {
+ TiXmlText *addText = new TiXmlText(value);
+ addElem->LinkEndChild(addText);
+ }
_elem->LinkEndChild(addElem);
}
}
/////////////////////////////////////////////////
-void Converter::Move(TiXmlElement *_elem, TiXmlElement *_moveElem)
+void Converter::Remove(TiXmlElement *_elem, TiXmlElement *_removeElem)
+{
+ SDF_ASSERT(_elem != NULL, "SDF element is NULL");
+ SDF_ASSERT(_removeElem != NULL, "Move element is NULL");
+
+ const char *fromElemStr = _removeElem->Attribute("element");
+
+ // tokenize 'from' and 'to' strs
+ std::string fromStr = "";
+ if (fromElemStr)
+ fromStr = fromElemStr;
+
+ std::vector<std::string> fromTokens;
+ boost::algorithm::split_regex(fromTokens, fromStr, boost::regex("::"));
+
+ if (fromTokens.empty())
+ {
+ sdferr << "Incorrect 'from' string format\n";
+ return;
+ }
+
+ // get value of the 'from' element/attribute
+ TiXmlElement *fromElem = _elem;
+ for (unsigned int i = 0; i < fromTokens.size()-1; ++i)
+ {
+ fromElem = fromElem->FirstChildElement(fromTokens[i]);
+ if (!fromElem)
+ {
+ // Return when the tokens don't match. Don't output an error message
+ // because it spams the console.
+ return;
+ }
+ }
+
+ const char *fromName = fromTokens[fromTokens.size()-1].c_str();
+
+ TiXmlElement *moveFrom = fromElem->FirstChildElement(fromName);
+
+ fromElem->RemoveChild(moveFrom);
+}
+
+/////////////////////////////////////////////////
+void Converter::Move(TiXmlElement *_elem, TiXmlElement *_moveElem,
+ const bool _copy)
{
SDF_ASSERT(_elem != NULL, "SDF element is NULL");
SDF_ASSERT(_moveElem != NULL, "Move element is NULL");
@@ -311,34 +360,16 @@ void Converter::Move(TiXmlElement *_elem, TiXmlElement *_moveElem)
{
fromElem = fromElem->FirstChildElement(fromTokens[i]);
if (!fromElem)
+ {
+ // Return when the tokens don't match. Don't output an error message
+ // because it spams the console.
return;
+ }
}
const char *fromName = fromTokens[fromTokens.size()-1].c_str();
const char *value = NULL;
- // Get value, or return if no element/attribute found as they don't have to
- // be specified in the sdf.
- if (fromElemStr)
- value = GetValue(fromName, NULL, fromElem);
- else if (fromAttrStr)
- value = GetValue(NULL, fromName, fromElem);
- if (!value)
- return;
-
- std::string valueStr = value;
- // move by creating a new element/attribute and deleting the old one
- if (fromElemStr)
- {
- TiXmlElement *moveFrom =
- fromElem->FirstChildElement(fromName);
- fromElem->RemoveChild(moveFrom);
- }
- else if (fromAttrStr)
- {
- fromElem->RemoveAttribute(fromName);
- }
-
unsigned int newDirIndex = 0;
// get the new element/attribute name
const char *toName = toTokens[toTokens.size()-1].c_str();
@@ -359,7 +390,8 @@ void Converter::Move(TiXmlElement *_elem, TiXmlElement *_moveElem)
// elements
if (!childElem)
{
- while (newDirIndex < (toTokens.size() - 1))
+ int offset = toElemStr != NULL && toAttrStr != NULL ? 0 : 1;
+ while (newDirIndex < (toTokens.size()-offset))
{
TiXmlElement *newElem = new TiXmlElement(toTokens[newDirIndex]);
toElem->LinkEndChild(newElem);
@@ -368,16 +400,59 @@ void Converter::Move(TiXmlElement *_elem, TiXmlElement *_moveElem)
}
}
- if (toElemStr)
+ // Get value, or return if no element/attribute found as they don't have to
+ // be specified in the sdf.
+ if (fromElemStr)
{
- TiXmlElement *moveTo = new TiXmlElement(toName);
- TiXmlText *text = new TiXmlText(valueStr);
- moveTo->LinkEndChild(text);
- toElem->LinkEndChild(moveTo);
+ TiXmlElement *moveFrom =
+ fromElem->FirstChildElement(fromName);
+
+ // No matching element, so return.
+ if (!moveFrom)
+ return;
+
+ if (toElemStr && !toAttrStr)
+ {
+ TiXmlElement *moveTo = static_cast<TiXmlElement*>(moveFrom->Clone());
+ moveTo->SetValue(toName);
+ toElem->LinkEndChild(moveTo);
+ }
+ else
+ {
+ value = GetValue(fromName, NULL, fromElem);
+ if (!value)
+ return;
+ std::string valueStr = value;
+
+ toElem->SetAttribute(toAttrStr, valueStr);
+ }
+
+ if (!_copy)
+ fromElem->RemoveChild(moveFrom);
}
- else if (toAttrStr)
+ else if (fromAttrStr)
{
- toElem->SetAttribute(toName, valueStr);
+ value = GetValue(NULL, fromName, fromElem);
+
+ if (!value)
+ return;
+
+ std::string valueStr = value;
+
+ if (toElemStr)
+ {
+ TiXmlElement *moveTo = new TiXmlElement(toName);
+ TiXmlText *text = new TiXmlText(valueStr);
+ moveTo->LinkEndChild(text);
+ toElem->LinkEndChild(moveTo);
+ }
+ else if (toAttrStr)
+ {
+ toElem->SetAttribute(toName, valueStr);
+ }
+
+ if (!_copy && fromAttrStr)
+ fromElem->RemoveAttribute(fromName);
}
}
diff --git a/src/Element.cc b/src/Element.cc
index 0c78994..48428e7 100644
--- a/src/Element.cc
+++ b/src/Element.cc
@@ -105,12 +105,12 @@ void Element::AddValue(const std::string &_type,
}
/////////////////////////////////////////////////
-boost::shared_ptr<Param> Element::CreateParam(const std::string &_key,
+ParamPtr Element::CreateParam(const std::string &_key,
const std::string &_type, const std::string &_defaultValue, bool _required,
const std::string &_description)
{
- return boost::shared_ptr<Param>(
- new Param(_key, _type, _defaultValue, _required, _description));
+ return ParamPtr(
+ new Param(_key, _type, _defaultValue, _required, _description));
}
/////////////////////////////////////////////////
@@ -232,6 +232,7 @@ void Element::PrintDescription(const std::string &_prefix)
if (this->GetCopyChildren())
std::cout << _prefix << " <element copy_data ='true' required ='*'/>\n";
+
std::string refSDF = this->ReferenceSDF();
if (!refSDF.empty())
{
diff --git a/src/Param.cc b/src/Param.cc
index cd7cf67..03b5711 100644
--- a/src/Param.cc
+++ b/src/Param.cc
@@ -17,6 +17,7 @@
#include <math.h>
#include <locale.h>
+#include <boost/algorithm/string.hpp>
#include "sdf/Param.hh"
using namespace sdf;
@@ -385,7 +386,7 @@ bool Param::SetFromString(const std::string &_value)
std::string tmp(str);
std::string lowerTmp(str);
- boost::to_lower(lowerTmp);
+ std::transform(lowerTmp.begin(), lowerTmp.end(), lowerTmp.begin(), ::tolower);
// "true" and "false" doesn't work properly
if (lowerTmp == "true")
@@ -397,40 +398,28 @@ bool Param::SetFromString(const std::string &_value)
try
{
- // If the string is hex, try to use stoi and stoul, and then
- // lexical cast as a last resort.
+ // Try to use stoi and stoul for integers, and
+ // stof and stod for floating point values.
+ // Use boost lexical cast as a last resort.
+ int numericBase = 10;
if (isHex)
+ numericBase = 16;
+
+ if (this->dataPtr->typeName == "int")
+ this->dataPtr->value = std::stoi(tmp, NULL, numericBase);
+ else if (this->dataPtr->typeName == "unsigned int")
{
- if (this->dataPtr->typeName == "int")
- this->dataPtr->value = std::stoi(tmp, NULL, 16);
- else if (this->dataPtr->typeName == "unsigned int")
- {
- this->dataPtr->value = static_cast<unsigned int>(
- std::stoul(tmp, NULL, 16));
- }
- else
- {
- boost::apply_visitor(string_set(tmp), this->dataPtr->value);
- }
+ this->dataPtr->value = static_cast<unsigned int>(
+ std::stoul(tmp, NULL, numericBase));
}
- // Otherwise use stod, stof, and lexical cast
+ else if (this->dataPtr->typeName == "double")
+ this->dataPtr->value = std::stod(tmp);
+ else if (this->dataPtr->typeName == "float")
+ this->dataPtr->value = std::stof(tmp);
else
- {
- if (this->dataPtr->typeName == "int")
- this->dataPtr->value = std::stoi(tmp, NULL, 10);
- else if (this->dataPtr->typeName == "unsigned int")
- {
- this->dataPtr->value = static_cast<unsigned int>(
- std::stoul(tmp, NULL, 10));
- }
- else if (this->dataPtr->typeName == "double")
- this->dataPtr->value = std::stod(tmp);
- else if (this->dataPtr->typeName == "float")
- this->dataPtr->value = std::stof(tmp);
- else
- boost::apply_visitor(string_set(tmp), this->dataPtr->value);
- }
+ boost::apply_visitor(string_set(tmp), this->dataPtr->value);
}
+
// Catch invalid argument exception from std::stoi/stoul/stod/stof
catch(std::invalid_argument &)
{
diff --git a/src/Param_TEST.cc b/src/Param_TEST.cc
index acfc073..37dd914 100644
--- a/src/Param_TEST.cc
+++ b/src/Param_TEST.cc
@@ -147,21 +147,13 @@ TEST(Param, HexFloat)
EXPECT_TRUE(floatParam.Get<float>(value));
EXPECT_FLOAT_EQ(value, 0.0f);
- // Boost 1.58 and higher parses hex integers into floating point variables
- // successfully, while older versions don't
-#if (BOOST_VERSION >= 105800)
- {
- EXPECT_TRUE(floatParam.SetFromString("0x01"));
- EXPECT_TRUE(floatParam.Get<float>(value));
- EXPECT_FLOAT_EQ(value, 1.0f);
- }
-#else
- {
- EXPECT_FALSE(floatParam.SetFromString("0x01"));
- EXPECT_TRUE(floatParam.Get<float>(value));
- EXPECT_FLOAT_EQ(value, 0.0f);
- }
-#endif
+ EXPECT_TRUE(floatParam.SetFromString("0x01"));
+ EXPECT_TRUE(floatParam.Get<float>(value));
+ EXPECT_FLOAT_EQ(value, 1.0f);
+
+ EXPECT_TRUE(floatParam.SetFromString("0X2A"));
+ EXPECT_TRUE(floatParam.Get<float>(value));
+ EXPECT_FLOAT_EQ(value, 42.0f);
EXPECT_TRUE(floatParam.SetFromString("0.123"));
EXPECT_TRUE(floatParam.Get<float>(value));
@@ -181,21 +173,13 @@ TEST(Param, HexDouble)
EXPECT_TRUE(doubleParam.Get<double>(value));
EXPECT_DOUBLE_EQ(value, 0.0);
- // Boost 1.58 and higher parses hex integers into floating point variables
- // successfully, while older versions don't
-#if (BOOST_VERSION >= 105800)
- {
- EXPECT_TRUE(doubleParam.SetFromString("0x01"));
- EXPECT_TRUE(doubleParam.Get<double>(value));
- EXPECT_DOUBLE_EQ(value, 1.0);
- }
-#else
- {
- EXPECT_FALSE(doubleParam.SetFromString("0x01"));
- EXPECT_TRUE(doubleParam.Get<double>(value));
- EXPECT_DOUBLE_EQ(value, 0.0);
- }
-#endif
+ EXPECT_TRUE(doubleParam.SetFromString("0x01"));
+ EXPECT_TRUE(doubleParam.Get<double>(value));
+ EXPECT_DOUBLE_EQ(value, 1.0);
+
+ EXPECT_TRUE(doubleParam.SetFromString("0X2A"));
+ EXPECT_TRUE(doubleParam.Get<double>(value));
+ EXPECT_DOUBLE_EQ(value, 42.0);
EXPECT_TRUE(doubleParam.SetFromString("0.123"));
EXPECT_TRUE(doubleParam.Get<double>(value));
diff --git a/src/SDF.cc b/src/SDF.cc
index 0e52d72..cf2cd61 100644
--- a/src/SDF.cc
+++ b/src/SDF.cc
@@ -15,6 +15,7 @@
*
*/
+#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <map>
#include <list>
@@ -41,11 +42,11 @@ typedef std::map<std::string, PathList> URIPathMap;
URIPathMap g_uriPathMap;
-boost::function<std::string (const std::string &)> g_findFileCB;
+std::function<std::string (const std::string &)> g_findFileCB;
/////////////////////////////////////////////////
void sdf::setFindCallback(
- boost::function<std::string (const std::string &)> _cb)
+ std::function<std::string (const std::string &)> _cb)
{
g_findFileCB = _cb;
}
diff --git a/src/SDFExtension.cc b/src/SDFExtension.cc
index 1cb781c..1312609 100644
--- a/src/SDFExtension.cc
+++ b/src/SDFExtension.cc
@@ -24,6 +24,7 @@ SDFExtension::SDFExtension()
{
this->material.clear();
this->visual_blobs.clear();
+ this->collision_blobs.clear();
this->setStaticFlag = false;
this->gravity = true;
this->isDampingFactor = false;
@@ -37,6 +38,8 @@ SDFExtension::SDFExtension()
this->isKd = false;
this->selfCollide = false;
this->isLaserRetro = false;
+ this->isSpringReference = false;
+ this->isSpringStiffness = false;
this->isStopCfm = false;
this->isStopErp = false;
this->isStopKp = false;
@@ -56,6 +59,8 @@ SDFExtension::SDFExtension()
this->kp = 100000000;
this->kd = 1;
this->laserRetro = 101;
+ this->springReference = 0;
+ this->springStiffness = 0;
this->stopCfm = 0;
this->stopErp = 0.1;
this->stopKp = 100000000;
@@ -72,6 +77,7 @@ SDFExtension::SDFExtension(const SDFExtension &_ge)
{
this->material = _ge.material;
this->visual_blobs = _ge.visual_blobs;
+ this->collision_blobs = _ge.collision_blobs;
this->setStaticFlag = _ge.setStaticFlag;
this->gravity = _ge.gravity;
this->isDampingFactor = _ge.isDampingFactor;
@@ -85,6 +91,8 @@ SDFExtension::SDFExtension(const SDFExtension &_ge)
this->isKd = _ge.isKd;
this->selfCollide = _ge.selfCollide;
this->isLaserRetro = _ge.isLaserRetro;
+ this->isSpringReference = _ge.isSpringReference;
+ this->isSpringStiffness = _ge.isSpringStiffness;
this->isStopKp = _ge.isStopKp;
this->isStopKd = _ge.isStopKd;
this->isStopCfm = _ge.isStopCfm;
@@ -108,6 +116,8 @@ SDFExtension::SDFExtension(const SDFExtension &_ge)
this->kp = _ge.kp;
this->kd = _ge.kd;
this->laserRetro = _ge.laserRetro;
+ this->springReference = _ge.springReference;
+ this->springStiffness = _ge.springStiffness;
this->stopKp = _ge.stopKp;
this->stopKd = _ge.stopKd;
this->stopCfm = _ge.stopCfm;
diff --git a/src/SDF_TEST.cc b/src/SDF_TEST.cc
index 8245ec7..6b298ec 100644
--- a/src/SDF_TEST.cc
+++ b/src/SDF_TEST.cc
@@ -72,7 +72,7 @@ TEST_F(SDFUpdate, UpdateAttribute)
// Set parameter update functions to test class accessors
SDFUpdateFixture fixture;
nameParam->Get(fixture.name);
- nameParam->SetUpdateFunc(boost::bind(&SDFUpdateFixture::GetName, &fixture));
+ nameParam->SetUpdateFunc(std::bind(&SDFUpdateFixture::GetName, &fixture));
std::string nameCheck;
int i;
@@ -123,9 +123,9 @@ TEST_F(SDFUpdate, UpdateElement)
// Set parameter update functions to test class accessors
SDFUpdateFixture fixture;
staticParam->Get(fixture.flag);
- staticParam->SetUpdateFunc(boost::bind(&SDFUpdateFixture::GetFlag, &fixture));
+ staticParam->SetUpdateFunc(std::bind(&SDFUpdateFixture::GetFlag, &fixture));
poseParam->Get(fixture.pose);
- poseParam->SetUpdateFunc(boost::bind(&SDFUpdateFixture::GetPose, &fixture));
+ poseParam->SetUpdateFunc(std::bind(&SDFUpdateFixture::GetPose, &fixture));
bool flagCheck;
ignition::math::Pose3d poseCheck;
@@ -362,10 +362,10 @@ TEST_F(SDFUpdate, GetAny)
{
std::ostringstream stream;
// Test types double, bool, string, int, vector3, color, pose
- stream << "<sdf version='1.5'>"
+ stream << "<sdf version='1.6'>"
<< "<world name='test'>"
+ << " <gravity> 0 0 -7.1 </gravity>"
<< " <physics type='ode'>"
- << " <gravity> 0 0 -7.1 </gravity>"
<< " <max_contacts>8</max_contacts>"
<< " <max_step_size>0.002</max_step_size>"
<< " </physics>"
@@ -422,8 +422,8 @@ TEST_F(SDFUpdate, GetAny)
}
{
- EXPECT_TRUE(physicsElem->HasElement("gravity"));
- boost::any anyValue = physicsElem->GetElement("gravity")->GetAny();
+ EXPECT_TRUE(worldElem->HasElement("gravity"));
+ boost::any anyValue = worldElem->GetElement("gravity")->GetAny();
try
{
EXPECT_EQ(boost::any_cast<ignition::math::Vector3d>(anyValue),
@@ -462,8 +462,8 @@ TEST_F(SDFUpdate, GetAny)
}
{
- EXPECT_TRUE(physicsElem->HasElement("gravity"));
- boost::any anyValue = physicsElem->GetElement("gravity")->GetAny();
+ EXPECT_TRUE(worldElem->HasElement("gravity"));
+ boost::any anyValue = worldElem->GetElement("gravity")->GetAny();
try
{
EXPECT_EQ(boost::any_cast<ignition::math::Vector3d>(anyValue),
diff --git a/src/parser.cc b/src/parser.cc
index ef11208..52cf23b 100644
--- a/src/parser.cc
+++ b/src/parser.cc
@@ -17,7 +17,7 @@
#include <stdlib.h>
#include <stdio.h>
#include <map>
-
+#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include "sdf/Console.hh"
@@ -262,6 +262,12 @@ bool initXml(TiXmlElement *_xml, ElementPtr _sdf)
ElementPtr element(new Element);
initFile(filename, element);
+
+ // override description for include elements
+ TiXmlElement *description = child->FirstChildElement("description");
+ if (description)
+ element->SetDescription(description->GetText());
+
_sdf->AddElementDescription(element);
}
@@ -691,11 +697,17 @@ bool readXml(TiXmlElement *_xml, ElementPtr _sdf)
elemXml->FirstChildElement("name")->GetText());
}
- if (elemXml->FirstChildElement("pose"))
+ TiXmlElement *poseElemXml = elemXml->FirstChildElement("pose");
+ if (poseElemXml)
{
- includeSDF->Root()->GetElement("model")->GetElement(
- "pose")->GetValue()->SetFromString(
- elemXml->FirstChildElement("pose")->GetText());
+ sdf::ElementPtr poseElem =
+ includeSDF->Root()->GetElement("model")->GetElement("pose");
+
+ poseElem->GetValue()->SetFromString(poseElemXml->GetText());
+
+ const char *frame = poseElemXml->Attribute("frame");
+ if (frame)
+ poseElem->GetAttribute("frame")->SetFromString(frame);
}
if (elemXml->FirstChildElement("static"))
diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc
index eb24023..fe9bb9b 100644
--- a/src/parser_urdf.cc
+++ b/src/parser_urdf.cc
@@ -17,6 +17,7 @@
#include <fstream>
#include <sstream>
#include <algorithm>
+#include <memory>
#include <string>
#include <set>
#include <ignition/math.hh>
@@ -35,8 +36,8 @@ typedef boost::shared_ptr<urdf::Collision> UrdfCollisionPtr;
typedef boost::shared_ptr<urdf::Visual> UrdfVisualPtr;
typedef boost::shared_ptr<urdf::Link> UrdfLinkPtr;
typedef boost::shared_ptr<const urdf::Link> ConstUrdfLinkPtr;
-typedef boost::shared_ptr<TiXmlElement> TiXmlElementPtr;
-typedef boost::shared_ptr<SDFExtension> SDFExtensionPtr;
+typedef std::shared_ptr<TiXmlElement> TiXmlElementPtr;
+typedef std::shared_ptr<SDFExtension> SDFExtensionPtr;
typedef std::map<std::string, std::vector<SDFExtensionPtr> >
StringSDFExtensionPtrMap;
@@ -46,6 +47,7 @@ bool g_reduceFixedJoints;
bool g_enforceLimits;
std::string g_collisionExt = "_collision";
std::string g_visualExt = "_visual";
+std::string g_lumpPrefix = "_fixed_joint_lump__";
urdf::Pose g_initialRobotPose;
bool g_initialRobotPoseValid = false;
std::set<std::string> g_fixedJointsNotReduced;
@@ -310,24 +312,48 @@ std::string Vector32Str(const urdf::Vector3 _vector)
}
////////////////////////////////////////////////////////////////////////////////
-void ReduceCollisionToParent(UrdfLinkPtr _link,
-#ifndef URDF_GE_0P3
- const std::string &_groupName,
-#else
- const std::string &/*_groupName*/,
-#endif
+/// \brief Check and add collision to parent link
+/// \param[in] _parentLink destination for _collision
+/// \param[in] _name urdfdom 0.3+: urdf collision group name with lumped
+/// collision info (see ReduceCollisionsToParent).
+/// urdfdom 0.2: collision name with lumped
+/// collision info (see ReduceCollisionsToParent).
+/// \param[in] _collision move this collision to _parentLink
+////////////////////////////////////////////////////////////////
+// IMPORTANT NOTE: URDF_GE_OP3
+// IMPORTANT NOTE: on change from urdfdom_headers 0.2.x to 0.3.x
+////////////////////////////////////////////////////////////////
+// In urdfdom_headers 0.2.x, there are group names for
+// visuals and collisions in Link class:
+// std::map<std::string,
+// boost::shared_ptr<std::vector<boost::shared_ptr<Visual> > >
+// > visual_groups;
+// std::map<std::string,
+// boost::shared_ptr<std::vector<boost::shared_ptr<Collision> > >
+// > collision_groups;
+// and we have Visual::group_name and
+// Collision::group_name
+// In urdfdom_headers 0.3.x,
+// - Link::visual_groups and Link::collision_groups are removed
+// - method Link::getVisuals(group_name) has been removed
+// - method Link::getCollisions(group_name) has been removed
+// - Visual::group_name renamed to Visual::name
+// - Collision::group_name renamed to Collision::name
+////////////////////////////////////////////////////////////////
+void ReduceCollisionToParent(UrdfLinkPtr _parentLink,
+ const std::string &_name,
UrdfCollisionPtr _collision)
{
- boost::shared_ptr<std::vector<UrdfCollisionPtr> > cols;
#ifndef URDF_GE_0P3
- cols = _link->getCollisions(_groupName);
+ boost::shared_ptr<std::vector<UrdfCollisionPtr> > cols;
+ cols = _parentLink->getCollisions(_name);
if (!cols)
{
// group does not exist, create one and add to map
cols.reset(new std::vector<UrdfCollisionPtr>);
// new group name, create add vector to map and add Collision to the vector
- _link->collision_groups.insert(make_pair(_groupName, cols));
+ _parentLink->collision_groups.insert(make_pair(_name, cols));
}
// group exists, add Collision to the vector in the map
@@ -335,28 +361,70 @@ void ReduceCollisionToParent(UrdfLinkPtr _link,
find(cols->begin(), cols->end(), _collision);
if (colIt != cols->end())
sdfwarn << "attempted to add collision to link ["
- << _link->name
+ << _parentLink->name
<< "], but it already exists under group ["
- << _groupName << "]\n";
+ << _name << "]\n";
else
cols->push_back(_collision);
#else
- _link->collision_array.push_back(_collision);
+ // added a check to see if _collision already exist in
+ // _parentLink::collision_array if not, add it.
+ _collision->name = _name;
+ std::vector<UrdfCollisionPtr>::iterator collisionIt =
+ find(_parentLink->collision_array.begin(),
+ _parentLink->collision_array.end(),
+ _collision);
+ if (collisionIt != _parentLink->collision_array.end())
+ {
+ sdfwarn << "attempted to add collision [" << _collision->name
+ << "] to link ["
+ << _parentLink->name
+ << "], but it already exists in collision_array under name ["
+ << (*collisionIt)->name << "]\n";
+ }
+ else
+ {
+ _parentLink->collision_array.push_back(_collision);
+ }
#endif
}
////////////////////////////////////////////////////////////////////////////////
-void ReduceVisualToParent(UrdfLinkPtr _link,
-#ifndef URDF_GE_0P3
- const std::string &_groupName,
-#else
- const std::string &/*_groupName*/,
-#endif
+/// \brief Check and add visual to parent link
+/// \param[in] _parentLink destination for _visual
+/// \param[in] _name urdfdom 0.3+: urdf visual group name with lumped
+/// visual info (see ReduceVisualsToParent).
+/// urdfdom 0.2: visual name with lumped
+/// visual info (see ReduceVisualsToParent).
+/// \param[in] _visual move this visual to _parentLink
+////////////////////////////////////////////////////////////////
+// IMPORTANT NOTE: URDF_GE_OP3
+// IMPORTANT NOTE: on change from urdfdom_headers 0.2.x to 0.3.x
+////////////////////////////////////////////////////////////////
+// In urdfdom_headers 0.2.x, there are group names for
+// visuals and collisions in Link class:
+// std::map<std::string,
+// boost::shared_ptr<std::vector<boost::shared_ptr<Visual> > >
+// > visual_groups;
+// std::map<std::string,
+// boost::shared_ptr<std::vector<boost::shared_ptr<Collision> > >
+// > collision_groups;
+// and we have Visual::group_name and
+// Collision::group_name
+// In urdfdom_headers 0.3.x,
+// - Link::visual_groups and Link::collision_groups are removed
+// - method Link::getVisuals(group_name) has been removed
+// - method Link::getCollisions(group_name) has been removed
+// - Visual::group_name renamed to Visual::name
+// - Collision::group_name renamed to Collision::name
+////////////////////////////////////////////////////////////////
+void ReduceVisualToParent(UrdfLinkPtr _parentLink,
+ const std::string &_name,
UrdfVisualPtr _visual)
{
- boost::shared_ptr<std::vector<UrdfVisualPtr> > viss;
#ifndef URDF_GE_0P3
- viss = _link->getVisuals(_groupName);
+ boost::shared_ptr<std::vector<UrdfVisualPtr> > viss;
+ viss = _parentLink->getVisuals(_name);
if (!viss)
{
@@ -364,9 +432,9 @@ void ReduceVisualToParent(UrdfLinkPtr _link,
viss.reset(new std::vector<UrdfVisualPtr>);
// new group name, create vector, add vector to map and
// add Visual to the vector
- _link->visual_groups.insert(make_pair(_groupName, viss));
+ _parentLink->visual_groups.insert(make_pair(_name, viss));
sdfdbg << "successfully added a new visual group name ["
- << _groupName << "]\n";
+ << _name << "]\n";
}
// group exists, add Visual to the vector in the map if it's not there
@@ -374,13 +442,31 @@ void ReduceVisualToParent(UrdfLinkPtr _link,
= find(viss->begin(), viss->end(), _visual);
if (visIt != viss->end())
sdfwarn << "attempted to add visual to link ["
- << _link->name
+ << _parentLink->name
<< "], but it already exists under group ["
- << _groupName << "]\n";
+ << _name << "]\n";
else
viss->push_back(_visual);
#else
- _link->visual_array.push_back(_visual);
+ // added a check to see if _visual already exist in
+ // _parentLink::visual_array if not, add it.
+ _visual->name = _name;
+ std::vector<UrdfVisualPtr>::iterator visualIt =
+ find(_parentLink->visual_array.begin(),
+ _parentLink->visual_array.end(),
+ _visual);
+ if (visualIt != _parentLink->visual_array.end())
+ {
+ sdfwarn << "attempted to add visual [" << _visual->name
+ << "] to link ["
+ << _parentLink->name
+ << "], but it already exists in visual_array under name ["
+ << (*visualIt)->name << "]\n";
+ }
+ else
+ {
+ _parentLink->visual_array.push_back(_visual);
+ }
#endif
}
@@ -842,21 +928,29 @@ void ReduceInertialToParent(UrdfLinkPtr _link)
}
/////////////////////////////////////////////////
-/// reduce fixed joints: lump visuals to parent link
+/// \brief reduce fixed joints: lump visuals to parent link
+/// \param[in] _link take all visuals from _link and lump/move them
+/// to the parent link (_link->getParentLink()).
void ReduceVisualsToParent(UrdfLinkPtr _link)
{
- // lump visual to parent
- // lump all visual to parent, assign group name
- // "lump::"+group name+"::'+_link name
- // lump but keep the _link name in(/as) the group name,
- // so we can correlate visuals to visuals somehow.
+ // lump all visuals of _link to _link->getParent().
+ // modify visual name (urdf 0.3.x) or
+ // visual group name (urdf 0.2.x)
+ // to indicate that it was lumped (fixed joint reduced)
+ // from another descendant link connected by a fixed joint.
+ //
+ // Algorithm for generating new name (or group name) is:
+ // original name + g_lumpPrefix+original link name (urdf 0.3.x)
+ // original group name + g_lumpPrefix+original link name (urdf 0.2.x)
+ // The purpose is to track where this visual came from
+ // (original parent link name before lumping/reducing).
#ifndef URDF_GE_0P3
for (std::map<std::string,
boost::shared_ptr<std::vector<UrdfVisualPtr> > >::iterator
visualsIt = _link->visual_groups.begin();
visualsIt != _link->visual_groups.end(); ++visualsIt)
{
- if (visualsIt->first.find(std::string("lump::")) == 0)
+ if (visualsIt->first.find(g_lumpPrefix) == 0)
{
// it's a previously lumped mesh, re-lump under same _groupName
std::string lumpGroupName = visualsIt->first;
@@ -878,7 +972,7 @@ void ReduceVisualsToParent(UrdfLinkPtr _link)
else
{
// default and any other groups meshes
- std::string lumpGroupName = std::string("lump::")+_link->name;
+ std::string lumpGroupName = g_lumpPrefix+_link->name;
sdfdbg << "adding modified lump group name [" << lumpGroupName
<< "] to link [" << _link->getParent()->name << "].\n";
for (std::vector<UrdfVisualPtr>::iterator
@@ -896,38 +990,74 @@ void ReduceVisualsToParent(UrdfLinkPtr _link)
}
}
#else
- std::string lumpGroupName = std::string("lump::")+_link->name;
for (std::vector<UrdfVisualPtr>::iterator
visualIt = _link->visual_array.begin();
visualIt != _link->visual_array.end(); ++visualIt)
{
+ // 20151116: changelog for pull request #235
+ std::string newVisualName;
+ std::size_t lumpIndex = (*visualIt)->name.find(g_lumpPrefix);
+ if (lumpIndex != std::string::npos)
+ {
+ newVisualName = (*visualIt)->name;
+ sdfdbg << "re-lumping visual [" << (*visualIt)->name
+ << "] for link [" << _link->name
+ << "] to parent [" << _link->getParent()->name
+ << "] with name [" << newVisualName << "]\n";
+ }
+ else
+ {
+ if ((*visualIt)->name.empty())
+ {
+ newVisualName = _link->name;
+ }
+ else
+ {
+ newVisualName = (*visualIt)->name;
+ }
+ sdfdbg << "lumping visual [" << (*visualIt)->name
+ << "] for link [" << _link->name
+ << "] to parent [" << _link->getParent()->name
+ << "] with name [" << newVisualName << "]\n";
+ }
+
// transform visual origin from _link frame to
// parent link frame before adding to parent
- (*visualIt)->origin = TransformToParentFrame((*visualIt)->origin,
+ (*visualIt)->origin = TransformToParentFrame(
+ (*visualIt)->origin,
_link->parent_joint->parent_to_joint_origin_transform);
+
// add the modified visual to parent
- ReduceVisualToParent(_link->getParent(), lumpGroupName,
+ ReduceVisualToParent(_link->getParent(), newVisualName,
*visualIt);
}
#endif
}
/////////////////////////////////////////////////
-/// reduce fixed joints: lump collisions to parent link
+/// \brief reduce fixed joints: lump collisions to parent link
+/// \param[in] _link take all collisions from _link and lump/move them
+/// to the parent link (_link->getParentLink()).
void ReduceCollisionsToParent(UrdfLinkPtr _link)
{
- // lump collision parent
- // lump all collision to parent, assign group name
- // "lump::"+group name+"::'+_link name
- // lump but keep the _link name in(/as) the group name,
- // so we can correlate visuals to collisions somehow.
+ // lump all collisions of _link to _link->getParent().
+ // modify collision name (urdf 0.3.x) or
+ // collision group name (urdf 0.2.x)
+ // to indicate that it was lumped (fixed joint reduced)
+ // from another descendant link connected by a fixed joint.
+ //
+ // Algorithm for generating new name (or group name) is:
+ // original name + g_lumpPrefix+original link name (urdf 0.3.x)
+ // original group name + g_lumpPrefix+original link name (urdf 0.2.x)
+ // The purpose is to track where this collision came from
+ // (original parent link name before lumping/reducing).
#ifndef URDF_GE_0P3
for (std::map<std::string,
boost::shared_ptr<std::vector<UrdfCollisionPtr> > >::iterator
collisionsIt = _link->collision_groups.begin();
collisionsIt != _link->collision_groups.end(); ++collisionsIt)
{
- if (collisionsIt->first.find(std::string("lump::")) == 0)
+ if (collisionsIt->first.find(g_lumpPrefix) == 0)
{
// if it's a previously lumped mesh, relump under same _groupName
std::string lumpGroupName = collisionsIt->first;
@@ -952,7 +1082,7 @@ void ReduceCollisionsToParent(UrdfLinkPtr _link)
else
{
// default and any other group meshes
- std::string lumpGroupName = std::string("lump::")+_link->name;
+ std::string lumpGroupName = g_lumpPrefix+_link->name;
sdfdbg << "lumping collision [" << collisionsIt->first
<< "] for link [" << _link->name
<< "] to parent [" << _link->getParent()->name
@@ -975,11 +1105,36 @@ void ReduceCollisionsToParent(UrdfLinkPtr _link)
}
// this->PrintCollisionGroups(_link->getParent());
#else
- std::string lumpGroupName = std::string("lump::")+_link->name;
for (std::vector<UrdfCollisionPtr>::iterator
collisionIt = _link->collision_array.begin();
collisionIt != _link->collision_array.end(); ++collisionIt)
{
+ std::string newCollisionName;
+ std::size_t lumpIndex =
+ (*collisionIt)->name.find(g_lumpPrefix);
+ if (lumpIndex != std::string::npos)
+ {
+ newCollisionName = (*collisionIt)->name;
+ sdfdbg << "re-lumping collision [" << (*collisionIt)->name
+ << "] for link [" << _link->name
+ << "] to parent [" << _link->getParent()->name
+ << "] with name [" << newCollisionName << "]\n";
+ }
+ else
+ {
+ if ((*collisionIt)->name.empty())
+ {
+ newCollisionName = _link->name;
+ }
+ else
+ {
+ newCollisionName = (*collisionIt)->name;
+ }
+ sdfdbg << "lumping collision [" << (*collisionIt)->name
+ << "] for link [" << _link->name
+ << "] to parent [" << _link->getParent()->name
+ << "] with name [" << newCollisionName << "]\n";
+ }
// transform collision origin from _link frame to
// parent link frame before adding to parent
(*collisionIt)->origin = TransformToParentFrame(
@@ -987,7 +1142,7 @@ void ReduceCollisionsToParent(UrdfLinkPtr _link)
_link->parent_joint->parent_to_joint_origin_transform);
// add the modified collision to parent
- ReduceCollisionToParent(_link->getParent(), lumpGroupName,
+ ReduceCollisionToParent(_link->getParent(), newCollisionName,
*collisionIt);
}
#endif
@@ -1140,7 +1295,7 @@ void InsertRobotOrigin(TiXmlElement *_elem)
{
if (g_initialRobotPoseValid)
{
- /* set transform */
+ // set transform
double pose[6];
pose[0] = g_initialRobotPose.position.x;
pose[1] = g_initialRobotPose.position.y;
@@ -1201,19 +1356,27 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml)
{
sdf->material = GetKeyValueAsString(childElem);
}
- else if (childElem->ValueStr() == "visual")
+ else if (childElem->ValueStr() == "collision"
+ || childElem->ValueStr() == "visual")
{
- // anything inside of visual tags:
+ // anything inside of collision or visual tags:
// <gazebo reference="link_name">
+ // <collision>
+ // <collision_extention_stuff_here/>
+ // </collision>
// <visual>
- // <extention_stuff_here/>
+ // <visual_extention_stuff_here/>
// </visual>
// </gazebl>
// are treated as blobs that gets inserted
- // into visuals for the link
+ // into all collisions and visuals for the link
+ // <collision name="link_name[anything here]">
+ // <stuff_from_urdf_link_collisions/>
+ // <collision_extention_stuff_here/>
+ // </collision>
// <visual name="link_name[anything here]">
// <stuff_from_urdf_link_visuals/>
- // <extention_stuff_here/>
+ // <visual_extention_stuff_here/>
// </visual>
// a place to store converted doc
@@ -1224,14 +1387,19 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml)
std::ostringstream origStream;
origStream << *e;
- sdfdbg << "visual extension [" << origStream.str() << "] not " <<
- "converted from URDF, probably already in SDF format.";
xmlNewDoc.Parse(origStream.str().c_str());
// save all unknown stuff in a vector of blobs
TiXmlElementPtr blob(
new TiXmlElement(*xmlNewDoc.FirstChildElement()));
- sdf->visual_blobs.push_back(blob);
+ if (childElem->ValueStr() == "collision")
+ {
+ sdf->collision_blobs.push_back(blob);
+ }
+ else
+ {
+ sdf->visual_blobs.push_back(blob);
+ }
}
}
else if (childElem->ValueStr() == "static")
@@ -1325,6 +1493,16 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml)
sdf->laserRetro = boost::lexical_cast<double>(
GetKeyValueAsString(childElem).c_str());
}
+ else if (childElem->ValueStr() == "springReference")
+ {
+ sdf->isSpringReference = true;
+ sdf->springReference = std::stod(GetKeyValueAsString(childElem));
+ }
+ else if (childElem->ValueStr() == "springStiffness")
+ {
+ sdf->isSpringStiffness = true;
+ sdf->springStiffness = std::stod(GetKeyValueAsString(childElem));
+ }
else if (childElem->ValueStr() == "stopCfm")
{
sdf->isStopCfm = true;
@@ -1429,57 +1607,315 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml)
void InsertSDFExtensionCollision(TiXmlElement *_elem,
const std::string &_linkName)
{
+ // loop through extensions for the whole model
+ // and see which ones belong to _linkName
+ // This might be complicated since there's:
+ // - urdf collision name -> sdf collision name conversion
+ // - fixed joint reduction / lumping
for (StringSDFExtensionPtrMap::iterator
sdfIt = g_extensions.begin();
sdfIt != g_extensions.end(); ++sdfIt)
{
- for (std::vector<SDFExtensionPtr>::iterator ge = sdfIt->second.begin();
- ge != sdfIt->second.end(); ++ge)
+ if (sdfIt->first == _linkName)
{
- if (((*ge)->oldLinkName == _linkName) ||
- (_elem->Attribute("name") &&
- (std::string(_elem->Attribute("name")) ==
- _linkName + g_collisionExt + std::string("_") + (*ge)->oldLinkName)))
- {
- TiXmlElement *surface = new TiXmlElement("surface");
- TiXmlElement *friction = new TiXmlElement("friction");
- TiXmlElement *frictionOde = new TiXmlElement("ode");
- TiXmlElement *contact = new TiXmlElement("contact");
- TiXmlElement *contactOde = new TiXmlElement("ode");
-
- // insert mu1, mu2, kp, kd for collision
- if ((*ge)->isMu1)
- AddKeyValue(frictionOde, "mu",
- Values2str(1, &(*ge)->mu1));
- if ((*ge)->isMu2)
- AddKeyValue(frictionOde, "mu2",
- Values2str(1, &(*ge)->mu2));
- if (!(*ge)->fdir1.empty())
- AddKeyValue(frictionOde, "fdir1", (*ge)->fdir1);
- if ((*ge)->isKp)
- AddKeyValue(contactOde, "kp", Values2str(1, &(*ge)->kp));
- if ((*ge)->isKd)
- AddKeyValue(contactOde, "kd", Values2str(1, &(*ge)->kd));
- // max contact interpenetration correction velocity
- if ((*ge)->isMaxVel)
- AddKeyValue(contactOde, "max_vel",
- Values2str(1, &(*ge)->maxVel));
- // contact interpenetration margin tolerance
- if ((*ge)->isMinDepth)
- AddKeyValue(contactOde, "min_depth",
- Values2str(1, &(*ge)->minDepth));
- if ((*ge)->isLaserRetro)
- AddKeyValue(_elem, "laser_retro",
- Values2str(1, &(*ge)->laserRetro));
- if ((*ge)->isMaxContacts)
- AddKeyValue(_elem, "max_contacts",
- boost::lexical_cast<std::string>((*ge)->maxContacts));
-
- contact->LinkEndChild(contactOde);
- surface->LinkEndChild(contact);
- friction->LinkEndChild(frictionOde);
- surface->LinkEndChild(friction);
- _elem->LinkEndChild(surface);
+ // std::cerr << "============================\n";
+ // std::cerr << "working on g_extensions for link ["
+ // << sdfIt->first << "]\n";
+ // if _elem already has a surface element, use it
+ TiXmlNode *surface = _elem->FirstChild("surface");
+ TiXmlNode *friction = NULL;
+ TiXmlNode *frictionOde = NULL;
+ TiXmlNode *contact = NULL;
+ TiXmlNode *contactOde = NULL;
+
+ // loop through all the gazebo extensions stored in sdfIt->second
+ for (std::vector<SDFExtensionPtr>::iterator ge = sdfIt->second.begin();
+ ge != sdfIt->second.end(); ++ge)
+ {
+ // Check if this blob belongs to _elem based on
+ // - blob's reference link name (_linkName or sdfIt->first)
+ // - _elem (destination for blob, which is a collision sdf).
+
+ if (!_elem->Attribute("name"))
+ {
+ sdferr << "ERROR: collision _elem has no name,"
+ << " something is wrong" << "\n";
+ }
+
+ std::string sdfCollisionName(_elem->Attribute("name"));
+
+ // std::cerr << "----------------------------\n";
+ // std::cerr << "blob belongs to [" << _linkName
+ // << "] with old parent LinkName [" << (*ge)->oldLinkName
+ // << "]\n";
+ // std::cerr << "_elem sdf collision name [" << sdfCollisionName
+ // << "]\n";
+ // std::cerr << "----------------------------\n";
+
+ std::string lumpCollisionName = g_lumpPrefix +
+ (*ge)->oldLinkName + g_collisionExt;
+
+ bool wasReduced = (_linkName == (*ge)->oldLinkName);
+ bool collisionNameContainsLinkname =
+ sdfCollisionName.find(_linkName) != std::string::npos;
+ bool collisionNameContainsLumpedLinkname =
+ sdfCollisionName.find(lumpCollisionName) != std::string::npos;
+ bool collisionNameContainsLumpedRef =
+ sdfCollisionName.find(g_lumpPrefix) != std::string::npos;
+
+ if (!collisionNameContainsLinkname)
+ {
+ sdferr << "collision name does not contain link name,"
+ << " file an issue.\n";
+ }
+
+ // if the collision _elem was not reduced,
+ // its name should not have g_lumpPrefix in it.
+ // otherwise, its name should have
+ // "g_lumpPrefix+[original link name before reduction]".
+ if ((wasReduced && !collisionNameContainsLumpedRef) ||
+ (!wasReduced && collisionNameContainsLumpedLinkname))
+ {
+ // insert any blobs (including visual plugins)
+ // warning, if you insert a <surface> sdf here, it might
+ // duplicate what was constructed above.
+ // in the future, we should use blobs (below) in place of
+ // explicitly specified fields (above).
+ if (!(*ge)->collision_blobs.empty())
+ {
+ std::vector<TiXmlElementPtr>::iterator blob;
+ for (blob = (*ge)->collision_blobs.begin();
+ blob != (*ge)->collision_blobs.end(); ++blob)
+ {
+ // find elements and assign pointers if they exist
+ // for mu1, mu2, minDepth, maxVel, fdir1, kp, kd
+ // otherwise, they are allocated by 'new' below.
+ // std::cerr << ">>>>> working on extension blob: ["
+ // << (*blob)->Value() << "]\n";
+
+ // print for debug
+ std::ostringstream origStream;
+ std::unique_ptr<TiXmlNode> blobClone((*blob)->Clone());
+ origStream << *blobClone;
+ // std::cerr << "collision extension ["
+ // << origStream.str() << "]\n";
+
+ if (strcmp((*blob)->Value(), "surface") == 0)
+ {
+ // blob is a <surface>, tread carefully otherwise
+ // we end up with multiple copies of <surface>.
+ // Also, get pointers (contact[Ode], friction[Ode])
+ // below for backwards (non-blob) compatibility.
+ if (surface == NULL)
+ {
+ // <surface> do not exist, it simple,
+ // just add it to the current collision
+ // and it's done.
+ _elem->LinkEndChild((*blob)->Clone());
+ surface = _elem->LastChild("surface");
+ // std::cerr << " --- surface created "
+ // << (void*)surface << "\n";
+ }
+ else
+ {
+ // <surface> exist already, remove it and
+ // overwrite with the blob.
+ _elem->RemoveChild(surface);
+ _elem->LinkEndChild((*blob)->Clone());
+ surface = _elem->FirstChild("surface");
+ // std::cerr << " --- surface exists, replace with blob.\n";
+ }
+
+ // Extra code for backwards compatibility, to
+ // deal with old way of specifying collision attributes
+ // using individual elements listed below:
+ // "mu"
+ // "mu2"
+ // "fdir1"
+ // "kp"
+ // "kd"
+ // "max_vel"
+ // "min_depth"
+ // "laser_retro"
+ // "max_contacts"
+ // Get contact[Ode] and friction[Ode] node pointers
+ // if they exist.
+ contact = surface->FirstChild("contact");
+ if (contact != NULL)
+ {
+ contactOde = contact->FirstChild("ode");
+ }
+ friction = surface->FirstChild("friction");
+ if (friction != NULL)
+ {
+ frictionOde = friction->FirstChild("ode");
+ }
+ }
+ else
+ {
+ // If the blob is not a <surface>, we don't have
+ // to worry about backwards compatibility.
+ // Simply add to master element.
+ _elem->LinkEndChild((*blob)->Clone());
+ }
+ }
+ }
+
+ // Extra code for backwards compatibility, to
+ // deal with old way of specifying collision attributes
+ // using individual elements listed below:
+ // "mu"
+ // "mu2"
+ // "fdir1"
+ // "kp"
+ // "kd"
+ // "max_vel"
+ // "min_depth"
+ // "laser_retro"
+ // "max_contacts"
+ // The new way to do this is to specify everything
+ // in collision blobs by using the <collision> tag.
+ // So there's no need for custom code for each property.
+
+ // construct new elements if not in blobs
+ if (surface == NULL)
+ {
+ surface = new TiXmlElement("surface");
+ if (!surface)
+ {
+ // Memory allocation error
+ sdferr << "Memory allocation error while"
+ << " processing <surface>.\n";
+ }
+ _elem->LinkEndChild(surface);
+ }
+
+ // construct new elements if not in blobs
+ if (contact == NULL)
+ {
+ if (surface->FirstChild("contact") == NULL)
+ {
+ contact = new TiXmlElement("contact");
+ if (!contact)
+ {
+ // Memory allocation error
+ sdferr << "Memory allocation error while"
+ << " processing <contact>.\n";
+ }
+ surface->LinkEndChild(contact);
+ }
+ else
+ {
+ contact = surface->FirstChild("contact");
+ }
+ }
+
+ if (contactOde == NULL)
+ {
+
+ if (contact->FirstChild("ode") == NULL)
+ {
+ contactOde = new TiXmlElement("ode");
+ if (!contactOde)
+ {
+ // Memory allocation error
+ sdferr << "Memory allocation error while"
+ << " processing <contact><ode>.\n";
+ }
+ contact->LinkEndChild(contactOde);
+ }
+ else
+ {
+ contactOde = contact->FirstChild("ode");
+ }
+ }
+
+ if (friction == NULL)
+ {
+ if (surface->FirstChild("friction") == NULL)
+ {
+ friction = new TiXmlElement("friction");
+ if (!friction)
+ {
+ // Memory allocation error
+ sdferr << "Memory allocation error while"
+ << " processing <friction>.\n";
+ }
+ surface->LinkEndChild(friction);
+ }
+ else
+ {
+ friction = surface->FirstChild("friction");
+ }
+ }
+
+ if (frictionOde == NULL)
+ {
+ if (friction->FirstChild("ode") == NULL)
+ {
+ frictionOde = new TiXmlElement("ode");
+ if (!frictionOde)
+ {
+ // Memory allocation error
+ sdferr << "Memory allocation error while"
+ << " processing <friction><ode>.\n";
+ }
+ friction->LinkEndChild(frictionOde);
+ }
+ else
+ {
+ frictionOde = friction->FirstChild("ode");
+ }
+ }
+
+ // insert mu1, mu2, kp, kd for collision
+ if ((*ge)->isMu1)
+ {
+ AddKeyValue(frictionOde->ToElement(), "mu",
+ Values2str(1, &(*ge)->mu1));
+ }
+ if ((*ge)->isMu2)
+ {
+ AddKeyValue(frictionOde->ToElement(), "mu2",
+ Values2str(1, &(*ge)->mu2));
+ }
+ if (!(*ge)->fdir1.empty())
+ {
+ AddKeyValue(frictionOde->ToElement(), "fdir1", (*ge)->fdir1);
+ }
+ if ((*ge)->isKp)
+ {
+ AddKeyValue(contactOde->ToElement(), "kp",
+ Values2str(1, &(*ge)->kp));
+ }
+ if ((*ge)->isKd)
+ {
+ AddKeyValue(contactOde->ToElement(), "kd",
+ Values2str(1, &(*ge)->kd));
+ }
+ // max contact interpenetration correction velocity
+ if ((*ge)->isMaxVel)
+ {
+ AddKeyValue(contactOde->ToElement(), "max_vel",
+ Values2str(1, &(*ge)->maxVel));
+ }
+ // contact interpenetration margin tolerance
+ if ((*ge)->isMinDepth)
+ {
+ AddKeyValue(contactOde->ToElement(), "min_depth",
+ Values2str(1, &(*ge)->minDepth));
+ }
+ if ((*ge)->isLaserRetro)
+ {
+ AddKeyValue(_elem, "laser_retro",
+ Values2str(1, &(*ge)->laserRetro));
+ }
+ if ((*ge)->isMaxContacts)
+ {
+ AddKeyValue(_elem, "max_contacts",
+ boost::lexical_cast<std::string>((*ge)->maxContacts));
+ }
+ }
}
}
}
@@ -1489,46 +1925,188 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem,
void InsertSDFExtensionVisual(TiXmlElement *_elem,
const std::string &_linkName)
{
+ // loop through extensions for the whole model
+ // and see which ones belong to _linkName
+ // This might be complicated since there's:
+ // - urdf visual name -> sdf visual name conversion
+ // - fixed joint reduction / lumping
for (StringSDFExtensionPtrMap::iterator
sdfIt = g_extensions.begin();
sdfIt != g_extensions.end(); ++sdfIt)
{
- for (std::vector<SDFExtensionPtr>::iterator ge = sdfIt->second.begin();
- ge != sdfIt->second.end(); ++ge)
+ if (sdfIt->first == _linkName)
{
- if (_linkName.find((*ge)->oldLinkName) != std::string::npos)
+ // std::cerr << "============================\n";
+ // std::cerr << "working on g_extensions for link ["
+ // << sdfIt->first << "]\n";
+ // if _elem already has a material element, use it
+ TiXmlNode *material = _elem->FirstChild("material");
+ TiXmlElement *script = NULL;
+
+ // loop through all the gazebo extensions stored in sdfIt->second
+ for (std::vector<SDFExtensionPtr>::iterator ge = sdfIt->second.begin();
+ ge != sdfIt->second.end(); ++ge)
{
- // insert material block
- if (!(*ge)->material.empty())
+ // Check if this blob belongs to _elem based on
+ // - blob's reference link name (_linkName or sdfIt->first)
+ // - _elem (destination for blob, which is a visual sdf).
+
+ if (!_elem->Attribute("name"))
+ sdferr << "ERROR: visual _elem has no name,"
+ << " something is wrong" << "\n";
+
+ std::string sdfVisualName(_elem->Attribute("name"));
+
+ // std::cerr << "----------------------------\n";
+ // std::cerr << "blob belongs to [" << _linkName
+ // << "] with old parent LinkName [" << (*ge)->oldLinkName
+ // << "]\n";
+ // std::cerr << "_elem sdf visual name [" << sdfVisualName
+ // << "]\n";
+ // std::cerr << "----------------------------\n";
+
+ std::string lumpVisualName = g_lumpPrefix +
+ (*ge)->oldLinkName + g_visualExt;
+
+ bool wasReduced = (_linkName == (*ge)->oldLinkName);
+ bool visualNameContainsLinkname =
+ sdfVisualName.find(_linkName) != std::string::npos;
+ bool visualNameContainsLumpedLinkname =
+ sdfVisualName.find(lumpVisualName) != std::string::npos;
+ bool visualNameContainsLumpedRef =
+ sdfVisualName.find(g_lumpPrefix) != std::string::npos;
+
+ if (!visualNameContainsLinkname)
+ sdferr << "visual name does not contain link name,"
+ << " file an issue.\n";
+
+ // if the visual _elem was not reduced,
+ // its name should not have g_lumpPrefix in it.
+ // otherwise, its name should have
+ // "g_lumpPrefix+[original link name before reduction]".
+ if ((wasReduced && !visualNameContainsLumpedRef) ||
+ (!wasReduced && visualNameContainsLumpedLinkname))
{
- // new sdf needs <material><script>...</script></material>
- TiXmlElement *materialElem = new TiXmlElement("material");
- TiXmlElement *scriptElem = new TiXmlElement("script");
- if (scriptElem && materialElem)
+ // insert any blobs (including visual plugins)
+ // warning, if you insert a <material> sdf here, it might
+ // duplicate what was constructed above.
+ // in the future, we should use blobs (below) in place of
+ // explicitly specified fields (above).
+ if (!(*ge)->visual_blobs.empty())
{
- AddKeyValue(scriptElem, "name", (*ge)->material);
- materialElem->LinkEndChild(scriptElem);
- _elem->LinkEndChild(materialElem);
- // AddKeyValue(_elem, "material", (*ge)->material);
+ std::vector<TiXmlElementPtr>::iterator blob;
+ for (blob = (*ge)->visual_blobs.begin();
+ blob != (*ge)->visual_blobs.end(); ++blob)
+ {
+ // find elements and assign pointers if they exist
+ // for mu1, mu2, minDepth, maxVel, fdir1, kp, kd
+ // otherwise, they are allocated by 'new' below.
+ // std::cerr << ">>>>> working on extension blob: ["
+ // << (*blob)->Value() << "]\n";
+
+ // print for debug
+ // std::ostringstream origStream;
+ // origStream << *(*blob)->Clone();
+ // std::cerr << "visual extension ["
+ // << origStream.str() << "]\n";
+
+ if (strcmp((*blob)->Value(), "material") == 0)
+ {
+ // blob is a <material>, tread carefully otherwise
+ // we end up with multiple copies of <material>.
+ // Also, get pointers (script)
+ // below for backwards (non-blob) compatibility.
+ if (material == NULL)
+ {
+ // <material> do not exist, it simple,
+ // just add it to the current visual
+ // and it's done.
+ _elem->LinkEndChild((*blob)->Clone());
+ material = _elem->LastChild("material");
+ // std::cerr << " --- material created "
+ // << (void*)material << "\n";
+ }
+ else
+ {
+ // <material> exist already, remove it and
+ // overwrite with the blob.
+ _elem->RemoveChild(material);
+ _elem->LinkEndChild((*blob)->Clone());
+ material = _elem->FirstChild("material");
+ // std::cerr << " --- material exists, replace with blob.\n";
+ }
+
+ // Extra code for backwards compatibility, to
+ // deal with old way of specifying visual attributes
+ // using individual element:
+ // "script"
+ // Get script node pointers
+ // if they exist.
+ script = material->FirstChildElement("script");
+ }
+ else
+ {
+ // std::cerr << "***** working on extension blob: ["
+ // << (*blob)->Value() << "]\n";
+ // If the blob is not a <material>, we don't have
+ // to worry about backwards compatibility.
+ // Simply add to master element.
+ _elem->LinkEndChild((*blob)->Clone());
+ }
+ }
}
- else
+
+ // Extra code for backwards compatibility, to
+ // deal with old way of specifying visual attributes
+ // using individual element:
+ // "script"
+ // The new way to do this is to specify everything
+ // in visual blobs by using the <visual> tag.
+ // So there's no need for custom code for each property.
+
+ // construct new elements if not in blobs
+ if (material == NULL)
{
- // Memory allocation error
- sdferr << "Memory allocation error while processing <material>.\n";
+ material = new TiXmlElement("material");
+ if (!material)
+ {
+ // Memory allocation error
+ sdferr << "Memory allocation error while"
+ << " processing <material>.\n";
+ }
+ _elem->LinkEndChild(material);
}
- }
- // insert any blobs (including visual plugins)
- if (!(*ge)->visual_blobs.empty())
- {
- std::vector<TiXmlElementPtr>::iterator blob;
- for (blob = (*ge)->visual_blobs.begin();
- blob != (*ge)->visual_blobs.end(); ++blob)
+ if (script == NULL)
{
- _elem->LinkEndChild((*blob)->Clone());
+ if (material->FirstChildElement("script") == NULL)
+ {
+ script = new TiXmlElement("script");
+ if (!script)
+ {
+ // Memory allocation error
+ sdferr << "Memory allocation error while"
+ << " processing <script>.\n";
+ }
+ material->LinkEndChild(script);
+ }
+ else
+ {
+ script = material->FirstChildElement("script");
+ }
}
- }
+ // backward compatibility for old code
+ // insert material/script block for visual
+ // (*ge)->material block goes under sdf <material><script><name>.
+ if (!(*ge)->material.empty())
+ {
+ AddKeyValue(script, "name", (*ge)->material);
+ // hard code original default gazebo materials files
+ AddKeyValue(script, "uri",
+ "file://media/materials/scripts/gazebo.material");
+ }
+ }
}
}
}
@@ -1622,6 +2200,22 @@ void InsertSDFExtensionJoint(TiXmlElement *_elem,
newLimit = true;
}
+ TiXmlElement *axis = _elem->FirstChildElement("axis");
+ bool newAxis = false;
+ if (axis == NULL)
+ {
+ axis = new TiXmlElement("axis");
+ newAxis = true;
+ }
+
+ TiXmlElement *dynamics = axis->FirstChildElement("dynamics");
+ bool newDynamics = false;
+ if (dynamics == NULL)
+ {
+ dynamics = new TiXmlElement("dynamics");
+ newDynamics = true;
+ }
+
// insert stopCfm, stopErp, fudgeFactor
if ((*ge)->isStopCfm)
{
@@ -1631,11 +2225,19 @@ void InsertSDFExtensionJoint(TiXmlElement *_elem,
{
AddKeyValue(limit, "erp", Values2str(1, &(*ge)->stopErp));
}
- /* gone
- if ((*ge)->isInitialJointPosition)
- AddKeyValue(_elem, "initialJointPosition",
- Values2str(1, &(*ge)->initialJointPosition));
- */
+ if ((*ge)->isSpringReference)
+ {
+ AddKeyValue(dynamics, "spring_reference",
+ Values2str(1, &(*ge)->springReference));
+ }
+ if ((*ge)->isSpringStiffness)
+ {
+ AddKeyValue(dynamics, "spring_stiffness",
+ Values2str(1, &(*ge)->springStiffness));
+ }
+ // if ((*ge)->isInitialJointPosition)
+ // AddKeyValue(_elem, "initialJointPosition",
+ // Values2str(1, &(*ge)->initialJointPosition));
// insert provideFeedback
if ((*ge)->isProvideFeedback)
@@ -1674,6 +2276,11 @@ void InsertSDFExtensionJoint(TiXmlElement *_elem,
AddKeyValue(physicsOde, "fudge_factor",
Values2str(1, &(*ge)->fudgeFactor));
+ if (newDynamics)
+ axis->LinkEndChild(dynamics);
+ if (newAxis)
+ _elem->LinkEndChild(axis);
+
if (newLimit)
physicsOde->LinkEndChild(limit);
if (newPhysicsOde)
@@ -1783,7 +2390,7 @@ void CreateGeometry(TiXmlElement* _elem,
AddKeyValue(geometryType, "scale", Vector32Str(mesh->scale));
// do something more to meshes
{
- /* set mesh file */
+ // set mesh file
if (mesh->filename.empty())
{
sdferr << "urdf2sdf: mesh geometry with no filename given.\n";
@@ -2015,22 +2622,22 @@ void ReduceSDFExtensionToParent(UrdfLinkPtr _link)
}
// find pointer to the existing extension with the new _link reference
- std::string newLinkName = _link->getParent()->name;
- StringSDFExtensionPtrMap::iterator newExt = g_extensions.find(newLinkName);
+ std::string parentLinkName = _link->getParent()->name;
+ StringSDFExtensionPtrMap::iterator parentExt = g_extensions.find(parentLinkName);
- // if none exist, create new extension with newLinkName
- if (newExt == g_extensions.end())
+ // if none exist, create new extension with parentLinkName
+ if (parentExt == g_extensions.end())
{
std::vector<SDFExtensionPtr> ge;
g_extensions.insert(std::make_pair(
- newLinkName, ge));
- newExt = g_extensions.find(newLinkName);
+ parentLinkName, ge));
+ parentExt = g_extensions.find(parentLinkName);
}
// move sdf extensions from _link into the parent _link's extensions
for (std::vector<SDFExtensionPtr>::iterator ge = ext->second.begin();
ge != ext->second.end(); ++ge)
- newExt->second.push_back(*ge);
+ parentExt->second.push_back(*ge);
ext->second.clear();
}
@@ -2055,7 +2662,7 @@ void ReduceSDFExtensionFrameReplace(SDFExtensionPtr _ge,
UrdfLinkPtr _link)
{
std::string linkName = _link->name;
- std::string newLinkName = _link->getParent()->name;
+ std::string parentLinkName = _link->getParent()->name;
// HACK: need to do this more generally, but we also need to replace
// all instances of _link name with new link name
@@ -2064,7 +2671,7 @@ void ReduceSDFExtensionFrameReplace(SDFExtensionPtr _ge,
// and it needs to be reparented to
// <collision>base_footprint_collision</collision>
sdfdbg << " STRING REPLACE: instances of _link name ["
- << linkName << "] with [" << newLinkName << "]\n";
+ << linkName << "] with [" << parentLinkName << "]\n";
for (std::vector<TiXmlElementPtr>::iterator blobIt =
_ge->blobs.begin();
blobIt != _ge->blobs.end(); ++blobIt)
@@ -2073,7 +2680,7 @@ void ReduceSDFExtensionFrameReplace(SDFExtensionPtr _ge,
debugStreamIn << *(*blobIt);
std::string debugBlob = debugStreamIn.str();
sdfdbg << " INITIAL STRING link ["
- << linkName << "]-->[" << newLinkName << "]: ["
+ << linkName << "]-->[" << parentLinkName << "]: ["
<< debugBlob << "]\n";
ReduceSDFExtensionContactSensorFrameReplace(blobIt, _link);
@@ -2197,7 +2804,7 @@ void CreateSDF(TiXmlElement *_root,
return;
}
- /* create <body:...> block for non fixed joint attached bodies */
+ // create <body:...> block for non fixed joint attached bodies
if ((_link->getParent() && _link->getParent()->name == "world") ||
!g_reduceFixedJoints ||
(!_link->parent_joint ||
@@ -2242,13 +2849,13 @@ void CreateLink(TiXmlElement *_root,
ConstUrdfLinkPtr _link,
ignition::math::Pose3d &_currentTransform)
{
- /* create new body */
+ // create new body
TiXmlElement *elem = new TiXmlElement("link");
- /* set body name */
+ // set body name
elem->SetAttribute("name", _link->name);
- /* compute global transform */
+ // compute global transform
ignition::math::Pose3d localTransform;
// this is the transform from parent link to current _link
// this transform does not exist for the root link
@@ -2264,22 +2871,22 @@ void CreateLink(TiXmlElement *_root,
// create origin tag for this element
AddTransform(elem, _currentTransform);
- /* create new inerial block */
+ // create new inerial block
CreateInertial(elem, _link);
- /* create new collision block */
+ // create new collision block
CreateCollisions(elem, _link);
- /* create new visual block */
+ // create new visual block
CreateVisuals(elem, _link);
- /* copy sdf extensions data */
+ // copy sdf extensions data
InsertSDFExtensionLink(elem, _link->name);
- /* add body to document */
+ // add body to document
_root->LinkEndChild(elem);
- /* make a <joint:...> block */
+ // make a <joint:...> block
CreateJoint(_root, _link, _currentTransform);
}
@@ -2287,7 +2894,11 @@ void CreateLink(TiXmlElement *_root,
void CreateCollisions(TiXmlElement* _elem,
ConstUrdfLinkPtr _link)
{
- // loop through all collision groups. as well as additional collision from
+ // loop through all collisions in
+ // collision_array (urdf 0.3.x)
+ // collision_groups (urdf 0.2.x)
+ // and create collision sdf blocks.
+ // Note, well as additional collision from
// lumped meshes (fixed joint reduction)
#ifndef URDF_GE_0P3
for (std::map<std::string,
@@ -2325,9 +2936,9 @@ void CreateCollisions(TiXmlElement* _elem,
// only 1 default mesh
++defaultMeshCount;
}
- else if (collisionsIt->first.find(std::string("lump::")) == 0)
+ else if (collisionsIt->first.find(g_lumpPrefix) == 0)
{
- // if collision name starts with "lump::", pass through
+ // if collision name starts with g_lumpPrefix, pass through
// original parent link name
sdfdbg << "creating lump collision [" << collisionsIt->first
<< "] for link [" << _link->name << "].\n";
@@ -2367,30 +2978,36 @@ void CreateCollisions(TiXmlElement* _elem,
}
}
#else
- unsigned int defaultMeshCount = 0;
+ unsigned int collisionCount = 0;
for (std::vector<UrdfCollisionPtr>::const_iterator
collision = _link->collision_array.begin();
collision != _link->collision_array.end();
++collision)
{
- sdfdbg << "creating default collision for link [" << _link->name
- << "]";
+ sdfdbg << "creating collision for link [" << _link->name
+ << "] collision [" << (*collision)->name << "]\n";
+
+ // collision sdf has a name if it was lumped/reduced
+ // otherwise, use the link name
+ std::string collisionName = (*collision)->name;
+ if (collisionName.empty())
+ collisionName = _link->name;
- std::string collisionPrefix = _link->name;
+ // add _collision extension
+ collisionName = collisionName + g_collisionExt;
- if (defaultMeshCount > 0)
+ if (collisionCount > 0)
{
- // append _[meshCount] to link name for additional collisions
std::ostringstream collisionNameStream;
- collisionNameStream << collisionPrefix << "_" << defaultMeshCount;
- collisionPrefix = collisionNameStream.str();
+ collisionNameStream << collisionName
+ << "_" << collisionCount;
+ collisionName = collisionNameStream.str();
}
- /* make a <collision> block */
- CreateCollision(_elem, _link, *collision, collisionPrefix);
+ // make a <collision> block
+ CreateCollision(_elem, _link, *collision, collisionName);
- // only 1 default mesh
- ++defaultMeshCount;
+ ++collisionCount;
}
#endif
}
@@ -2399,7 +3016,11 @@ void CreateCollisions(TiXmlElement* _elem,
void CreateVisuals(TiXmlElement* _elem,
ConstUrdfLinkPtr _link)
{
- // loop through all visual groups. as well as additional visuals from
+ // loop through all visuals in
+ // visual_array (urdf 0.3.x)
+ // visual_groups (urdf 0.2.x)
+ // and create visual sdf blocks.
+ // Note, well as additional visual from
// lumped meshes (fixed joint reduction)
#ifndef URDF_GE_0P3
for (std::map<std::string,
@@ -2437,9 +3058,9 @@ void CreateVisuals(TiXmlElement* _elem,
// only 1 default mesh
++defaultMeshCount;
}
- else if (visualsIt->first.find(std::string("lump::")) == 0)
+ else if (visualsIt->first.find(g_lumpPrefix) == 0)
{
- // if visual name starts with "lump::", pass through
+ // if visual name starts with g_lumpPrefix, pass through
// original parent link name
sdfdbg << "creating lump visual [" << visualsIt->first
<< "] for link [" << _link->name << "].\n";
@@ -2479,30 +3100,36 @@ void CreateVisuals(TiXmlElement* _elem,
}
}
#else
- unsigned int defaultMeshCount = 0;
+ unsigned int visualCount = 0;
for (std::vector<UrdfVisualPtr>::const_iterator
visual = _link->visual_array.begin();
visual != _link->visual_array.end();
++visual)
{
- sdfdbg << "creating default visual for link [" << _link->name
- << "]";
+ sdfdbg << "creating visual for link [" << _link->name
+ << "] visual [" << (*visual)->name << "]\n";
- std::string visualPrefix = _link->name;
+ // visual sdf has a name if it was lumped/reduced
+ // otherwise, use the link name
+ std::string visualName = (*visual)->name;
+ if (visualName.empty())
+ visualName = _link->name;
- if (defaultMeshCount > 0)
+ // add _visual extension
+ visualName = visualName + g_visualExt;
+
+ if (visualCount > 0)
{
- // append _[meshCount] to _link name for additional visuals
std::ostringstream visualNameStream;
- visualNameStream << visualPrefix << "_" << defaultMeshCount;
- visualPrefix = visualNameStream.str();
+ visualNameStream << visualName
+ << "_" << visualCount;
+ visualName = visualNameStream.str();
}
- // create a <visual> block
- CreateVisual(_elem, _link, *visual, visualPrefix);
+ // make a <visual> block
+ CreateVisual(_elem, _link, *visual, visualName);
- // only 1 default mesh
- ++defaultMeshCount;
+ ++visualCount;
}
#endif
}
@@ -2513,7 +3140,7 @@ void CreateInertial(TiXmlElement *_elem,
{
TiXmlElement *inertial = new TiXmlElement("inertial");
- /* set mass properties */
+ // set mass properties
// check and print a warning message
double roll, pitch, yaw;
_link->inertial->origin.rotation.getRPY(roll, pitch, yaw);
@@ -2550,7 +3177,7 @@ void CreateJoint(TiXmlElement *_root,
ConstUrdfLinkPtr _link,
ignition::math::Pose3d &_currentTransform)
{
- /* compute the joint tag */
+ // compute the joint tag
std::string jtype;
jtype.clear();
if (_link->parent_joint != NULL)
@@ -2668,10 +3295,10 @@ void CreateJoint(TiXmlElement *_root,
jointAxis->LinkEndChild(jointAxisDynamics);
joint->LinkEndChild(jointAxis);
- /* copy sdf extensions data */
+ // copy sdf extensions data
InsertSDFExtensionJoint(joint, _link->parent_joint->name);
- /* add joint to document */
+ // add joint to document
_root->LinkEndChild(joint);
}
}
@@ -2680,17 +3307,24 @@ void CreateJoint(TiXmlElement *_root,
void CreateCollision(TiXmlElement* _elem, ConstUrdfLinkPtr _link,
UrdfCollisionPtr _collision, const std::string &_oldLinkName)
{
- /* begin create geometry node, skip if no collision specified */
+ // begin create geometry node, skip if no collision specified
TiXmlElement *sdfCollision = new TiXmlElement("collision");
- /* set its name, if lumped, add original link name */
- if (_oldLinkName == _link->name)
- sdfCollision->SetAttribute("name", _link->name + g_collisionExt);
+ // std::cerr << "CreateCollision link [" << _link->name
+ // << "] old [" << _oldLinkName
+ // << "]\n";
+ // set its name, if lumped, add original link name
+ // for meshes in an original mesh, it's likely
+ // _link->name + mesh count
+ if (_oldLinkName.find(_link->name) == 0 || _oldLinkName.empty())
+ sdfCollision->SetAttribute("name", _oldLinkName);
else
- sdfCollision->SetAttribute("name", _link->name + g_collisionExt
- + std::string("_") + _oldLinkName);
+ sdfCollision->SetAttribute("name", _link->name
+ + g_lumpPrefix + _oldLinkName);
+
+ // std::cerr << "collision [" << sdfCollision->Attribute("name") << "]\n";
- /* set transform */
+ // set transform
double pose[6];
pose[0] = _collision->origin.position.x;
pose[1] = _collision->origin.position.y;
@@ -2698,8 +3332,7 @@ void CreateCollision(TiXmlElement* _elem, ConstUrdfLinkPtr _link,
_collision->origin.rotation.getRPY(pose[3], pose[4], pose[5]);
AddKeyValue(sdfCollision, "pose", Values2str(6, pose));
-
- /* add geometry block */
+ // add geometry block
if (!_collision || !_collision->geometry)
{
sdfdbg << "urdf2sdf: collision of link [" << _link->name
@@ -2710,10 +3343,10 @@ void CreateCollision(TiXmlElement* _elem, ConstUrdfLinkPtr _link,
CreateGeometry(sdfCollision, _collision->geometry);
}
- /* set additional data from extensions */
+ // set additional data from extensions
InsertSDFExtensionCollision(sdfCollision, _link->name);
- /* add geometry to body */
+ // add geometry to body
_elem->LinkEndChild(sdfCollision);
}
@@ -2721,19 +3354,17 @@ void CreateCollision(TiXmlElement* _elem, ConstUrdfLinkPtr _link,
void CreateVisual(TiXmlElement *_elem, ConstUrdfLinkPtr _link,
UrdfVisualPtr _visual, const std::string &_oldLinkName)
{
- /* begin create sdf visual node */
+ // begin create sdf visual node
TiXmlElement *sdfVisual = new TiXmlElement("visual");
- /* set its name */
- sdfdbg << "original link name [" << _oldLinkName
- << "] new link name [" << _link->name << "]\n";
- if (_oldLinkName == _link->name)
- sdfVisual->SetAttribute("name", _link->name + g_visualExt);
+ // set its name
+ if (_oldLinkName.find(_link->name) == 0 || _oldLinkName.empty())
+ sdfVisual->SetAttribute("name", _oldLinkName);
else
- sdfVisual->SetAttribute("name", _link->name + g_visualExt
- + std::string("_") + _oldLinkName);
+ sdfVisual->SetAttribute("name", _link->name
+ + g_lumpPrefix + _oldLinkName);
- /* add the visualisation transfrom */
+ // add the visualisation transfrom
double pose[6];
pose[0] = _visual->origin.position.x;
pose[1] = _visual->origin.position.y;
@@ -2741,19 +3372,21 @@ void CreateVisual(TiXmlElement *_elem, ConstUrdfLinkPtr _link,
_visual->origin.rotation.getRPY(pose[3], pose[4], pose[5]);
AddKeyValue(sdfVisual, "pose", Values2str(6, pose));
- /* insert geometry */
+ // insert geometry
if (!_visual || !_visual->geometry)
{
sdfdbg << "urdf2sdf: visual of link [" << _link->name
<< "] has no <geometry>.\n";
}
else
+ {
CreateGeometry(sdfVisual, _visual->geometry);
+ }
- /* set additional data from extensions */
- InsertSDFExtensionVisual(sdfVisual, _oldLinkName);
+ // set additional data from extensions
+ InsertSDFExtensionVisual(sdfVisual, _link->name);
- /* end create _visual node */
+ // end create _visual node
_elem->LinkEndChild(sdfVisual);
}
@@ -2763,7 +3396,7 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr,
{
g_enforceLimits = _enforceLimits;
- /* Create a RobotModel from string */
+ // Create a RobotModel from string
boost::shared_ptr<urdf::ModelInterface> robotModel =
urdf::parseURDF(_urdfStr.c_str());
@@ -2776,17 +3409,17 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr,
return sdfXmlOut;
}
- /* create root element and define needed namespaces */
+ // create root element and define needed namespaces
TiXmlElement *robot = new TiXmlElement("model");
// set model name to urdf robot name if not specified
robot->SetAttribute("name", robotModel->getName());
- /* initialize transform for the model, urdf is recursive,
- while sdf defines all links relative to model frame */
+ // initialize transform for the model, urdf is recursive,
+ // while sdf defines all links relative to model frame
ignition::math::Pose3d transform;
- /* parse sdf extension */
+ // parse sdf extension
TiXmlDocument urdfXml;
urdfXml.Parse(_urdfStr.c_str());
g_extensions.clear();
@@ -2797,20 +3430,20 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr,
ConstUrdfLinkPtr rootLink = robotModel->getRoot();
- /* Fixed Joint Reduction */
- /* if link connects to parent via fixed joint, lump down and remove link */
- /* set reduceFixedJoints to false will replace fixed joints with
- zero limit revolute joints, otherwise, we reduce it down to its
- parent link recursively */
- /* using the disabledFixedJointLumping option is possible to disable
- fixed joint lumping only for selected joints */
+ // Fixed Joint Reduction
+ // if link connects to parent via fixed joint, lump down and remove link
+ // set reduceFixedJoints to false will replace fixed joints with
+ // zero limit revolute joints, otherwise, we reduce it down to its
+ // parent link recursively
+ // using the disabledFixedJointLumping option is possible to disable
+ // fixed joint lumping only for selected joints
if (g_reduceFixedJoints)
ReduceFixedJoints(robot,
(boost::const_pointer_cast< urdf::Link >(rootLink)));
if (rootLink->name == "world")
{
- /* convert all children link */
+ // convert all children link
for (std::vector<UrdfLinkPtr>::const_iterator
child = rootLink->child_links.begin();
child != rootLink->child_links.end(); ++child)
@@ -2818,11 +3451,11 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr,
}
else
{
- /* convert, starting from root link */
+ // convert, starting from root link
CreateSDF(robot, rootLink, transform);
}
- /* insert the extensions without reference into <robot> root level */
+ // insert the extensions without reference into <robot> root level
InsertSDFExtensionRobot(robot);
InsertRobotOrigin(robot);
@@ -2938,19 +3571,18 @@ void ReduceSDFExtensionProjectorTransformReduction(
// overwrite <pose> (xyz/rpy) if it exists
if ((*_blobIt)->ValueStr() == "projector")
{
- /*
// parse it and add/replace the reduction transform
// find first instance of xyz and rpy, replace with reduction transform
- for (TiXmlNode* elIt = (*_blobIt)->FirstChild();
- elIt; elIt = elIt->NextSibling())
- {
- std::ostringstream streamIn;
- streamIn << *elIt;
- sdfdbg << " " << streamIn << "\n";
- }
- */
+ //
+ // for (TiXmlNode* elIt = (*_blobIt)->FirstChild();
+ // elIt; elIt = elIt->NextSibling())
+ // {
+ // std::ostringstream streamIn;
+ // streamIn << *elIt;
+ // sdfdbg << " " << streamIn << "\n";
+ // }
- /* should read <pose>...</pose> and agregate reductionTransform */
+ // should read <pose>...</pose> and agregate reductionTransform
TiXmlNode* poseKey = (*_blobIt)->FirstChild("pose");
// read pose and save it
@@ -2989,7 +3621,7 @@ void ReduceSDFExtensionContactSensorFrameReplace(
UrdfLinkPtr _link)
{
std::string linkName = _link->name;
- std::string newLinkName = _link->getParent()->name;
+ std::string parentLinkName = _link->getParent()->name;
if ((*_blobIt)->ValueStr() == "sensor")
{
// parse it and add/replace the reduction transform
@@ -3006,7 +3638,7 @@ void ReduceSDFExtensionContactSensorFrameReplace(
contact->RemoveChild(collision);
TiXmlElement* collisionNameKey = new TiXmlElement("collision");
std::ostringstream collisionNameStream;
- collisionNameStream << newLinkName << g_collisionExt
+ collisionNameStream << parentLinkName << g_collisionExt
<< "_" << linkName;
TiXmlText* collisionNameTxt = new TiXmlText(
collisionNameStream.str());
@@ -3029,7 +3661,7 @@ void ReduceSDFExtensionPluginFrameReplace(
ignition::math::Pose3d _reductionTransform)
{
std::string linkName = _link->name;
- std::string newLinkName = _link->getParent()->name;
+ std::string parentLinkName = _link->getParent()->name;
if ((*_blobIt)->ValueStr() == _pluginName)
{
// replace element containing _link names to parent link names
@@ -3042,7 +3674,7 @@ void ReduceSDFExtensionPluginFrameReplace(
(*_blobIt)->RemoveChild(elementNode);
TiXmlElement* bodyNameKey = new TiXmlElement(_elementName);
std::ostringstream bodyNameStream;
- bodyNameStream << newLinkName;
+ bodyNameStream << parentLinkName;
TiXmlText* bodyNameTxt = new TiXmlText(bodyNameStream.str());
bodyNameKey->LinkEndChild(bodyNameTxt);
(*_blobIt)->LinkEndChild(bodyNameKey);
@@ -3111,7 +3743,7 @@ void ReduceSDFExtensionProjectorFrameReplace(
UrdfLinkPtr _link)
{
std::string linkName = _link->name;
- std::string newLinkName = _link->getParent()->name;
+ std::string parentLinkName = _link->getParent()->name;
// updates _link reference for <projector> inside of
// projector plugins
@@ -3137,7 +3769,7 @@ void ReduceSDFExtensionProjectorFrameReplace(
if (projectorLinkName == linkName)
{
// do the replacement
- projectorName = newLinkName + "/" +
+ projectorName = parentLinkName + "/" +
projectorName.substr(pos+1, projectorName.size());
(*_blobIt)->RemoveChild(projectorElem);
@@ -3159,7 +3791,7 @@ void ReduceSDFExtensionGripperFrameReplace(
UrdfLinkPtr _link)
{
std::string linkName = _link->name;
- std::string newLinkName = _link->getParent()->name;
+ std::string parentLinkName = _link->getParent()->name;
if ((*_blobIt)->ValueStr() == "gripper")
{
@@ -3171,7 +3803,7 @@ void ReduceSDFExtensionGripperFrameReplace(
(*_blobIt)->RemoveChild(gripperLink);
TiXmlElement* bodyNameKey = new TiXmlElement("gripper_link");
std::ostringstream bodyNameStream;
- bodyNameStream << newLinkName;
+ bodyNameStream << parentLinkName;
TiXmlText* bodyNameTxt = new TiXmlText(bodyNameStream.str());
bodyNameKey->LinkEndChild(bodyNameTxt);
(*_blobIt)->LinkEndChild(bodyNameKey);
@@ -3185,7 +3817,7 @@ void ReduceSDFExtensionGripperFrameReplace(
(*_blobIt)->RemoveChild(palmLink);
TiXmlElement* bodyNameKey = new TiXmlElement("palm_link");
std::ostringstream bodyNameStream;
- bodyNameStream << newLinkName;
+ bodyNameStream << parentLinkName;
TiXmlText* bodyNameTxt = new TiXmlText(bodyNameStream.str());
bodyNameKey->LinkEndChild(bodyNameTxt);
(*_blobIt)->LinkEndChild(bodyNameKey);
@@ -3200,7 +3832,7 @@ void ReduceSDFExtensionJointFrameReplace(
UrdfLinkPtr _link)
{
std::string linkName = _link->name;
- std::string newLinkName = _link->getParent()->name;
+ std::string parentLinkName = _link->getParent()->name;
if ((*_blobIt)->ValueStr() == "joint")
{
@@ -3214,7 +3846,7 @@ void ReduceSDFExtensionJointFrameReplace(
(*_blobIt)->RemoveChild(parent);
TiXmlElement* parentNameKey = new TiXmlElement("parent");
std::ostringstream parentNameStream;
- parentNameStream << newLinkName;
+ parentNameStream << parentLinkName;
TiXmlText* parentNameTxt = new TiXmlText(parentNameStream.str());
parentNameKey->LinkEndChild(parentNameTxt);
(*_blobIt)->LinkEndChild(parentNameKey);
@@ -3228,7 +3860,7 @@ void ReduceSDFExtensionJointFrameReplace(
(*_blobIt)->RemoveChild(child);
TiXmlElement* childNameKey = new TiXmlElement("child");
std::ostringstream childNameStream;
- childNameStream << newLinkName;
+ childNameStream << parentLinkName;
TiXmlText* childNameTxt = new TiXmlText(childNameStream.str());
childNameKey->LinkEndChild(childNameTxt);
(*_blobIt)->LinkEndChild(childNameKey);
diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
index 85fdcaf..5537bfb 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -3,9 +3,12 @@ set(TEST_TYPE "INTEGRATION")
set(tests
audio.cc
cfm_damping_implicit_spring_damper.cc
+ converter.cc
disable_fixed_joint_reduction.cc
fixed_joint_reduction.cc
force_torque_sensor.cc
+ frame.cc
+ include.cc
joint_axis_frame.cc
locale_fix.cc
nested_model.cc
@@ -13,6 +16,7 @@ set(tests
plugin_attribute.cc
plugin_bool.cc
provide_feedback.cc
+ urdf_gazebo_extensions.cc
urdf_joint_parameters.cc
)
diff --git a/test/integration/converter.cc b/test/integration/converter.cc
new file mode 100644
index 0000000..8396ad0
--- /dev/null
+++ b/test/integration/converter.cc
@@ -0,0 +1,181 @@
+/*
+ * Copyright 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <gtest/gtest.h>
+#include "sdf/sdf.hh"
+
+#include "test_config.h"
+
+const std::string CONVERT_DOC =
+ std::string(PROJECT_SOURCE_PATH) + "/sdf/1.6/1_5.convert";
+
+/////////////////////////////////////////////////
+/// Test conversion of imu in 1.5 to 1.6
+TEST(ConverterIntegration, IMU_15_to_16)
+{
+ // The imu noise in 1.5 format
+ std::string xmlString = R"(
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <model name="box_old_imu_noise">
+ <link name="link">
+ <sensor name='imu_sensor' type='imu'>
+ <imu>
+ <noise>
+ <type>gaussian</type>
+ <rate>
+ <mean>0</mean>
+ <stddev>0.0002</stddev>
+ <bias_mean>7.5e-06</bias_mean>
+ <bias_stddev>8e-07</bias_stddev>
+ </rate>
+ <accel>
+ <mean>0</mean>
+ <stddev>0.017</stddev>
+ <bias_mean>0.1</bias_mean>
+ <bias_stddev>0.001</bias_stddev>
+ </accel>
+ </noise>
+ </imu>
+ </sensor>
+ </link>
+ </model>
+ </world>
+</sdf>)";
+
+ TiXmlDocument xmlDoc;
+ xmlDoc.Parse(xmlString.c_str());
+
+ // Convert
+ TiXmlDocument convertXmlDoc;
+ convertXmlDoc.LoadFile(CONVERT_DOC);
+ sdf::Converter::Convert(&xmlDoc, &convertXmlDoc);
+
+ // Check some basic elements
+ TiXmlElement *convertedElem = xmlDoc.FirstChildElement();
+ EXPECT_EQ(convertedElem->ValueStr(), "sdf");
+ convertedElem = convertedElem->FirstChildElement();
+ EXPECT_EQ(convertedElem->ValueStr(), "world");
+ convertedElem = convertedElem->FirstChildElement();
+ EXPECT_EQ(convertedElem->ValueStr(), "model");
+ convertedElem = convertedElem->FirstChildElement();
+ EXPECT_EQ(convertedElem->ValueStr(), "link");
+ convertedElem = convertedElem->FirstChildElement();
+ EXPECT_EQ(convertedElem->ValueStr(), "sensor");
+
+ // Get the imu
+ TiXmlElement *imuElem = convertedElem->FirstChildElement();
+ EXPECT_EQ(imuElem->ValueStr(), "imu");
+
+ // Get the angular_velocity
+ TiXmlElement *angVelElem = imuElem->FirstChildElement();
+ EXPECT_EQ(angVelElem->ValueStr(), "angular_velocity");
+
+ // Get the linear_acceleration
+ TiXmlElement *linAccElem = angVelElem->NextSiblingElement();
+ EXPECT_EQ(linAccElem->ValueStr(), "linear_acceleration");
+
+ std::array<char, 3> axis = {'x', 'y', 'z'};
+
+ TiXmlElement *angVelAxisElem = angVelElem->FirstChildElement();
+ TiXmlElement *linAccAxisElem = linAccElem->FirstChildElement();
+
+ // Iterate over <x>, <y>, and <z> elements under <angular_velocity> and
+ // <linear_acceleration>
+ for (auto const &a : axis)
+ {
+ EXPECT_EQ(angVelAxisElem->Value()[0], a);
+ EXPECT_EQ(linAccAxisElem->Value()[0], a);
+
+ TiXmlElement *angVelAxisNoiseElem = angVelAxisElem->FirstChildElement();
+ TiXmlElement *linAccAxisNoiseElem = linAccAxisElem->FirstChildElement();
+
+ EXPECT_EQ(angVelAxisNoiseElem->ValueStr(), "noise");
+ EXPECT_EQ(linAccAxisNoiseElem->ValueStr(), "noise");
+
+ EXPECT_STREQ(angVelAxisNoiseElem->Attribute("type"), "gaussian");
+ EXPECT_STREQ(linAccAxisNoiseElem->Attribute("type"), "gaussian");
+
+ EXPECT_STREQ(angVelAxisNoiseElem->FirstChildElement("mean")->GetText(),
+ "0");
+ EXPECT_STREQ(linAccAxisNoiseElem->FirstChildElement("mean")->GetText(),
+ "0");
+
+ EXPECT_STREQ(angVelAxisNoiseElem->FirstChildElement("stddev")->GetText(),
+ "0.0002");
+ EXPECT_STREQ(linAccAxisNoiseElem->FirstChildElement("stddev")->GetText(),
+ "0.017");
+
+ EXPECT_STREQ(angVelAxisNoiseElem->FirstChildElement("bias_mean")->GetText(),
+ "7.5e-06");
+ EXPECT_STREQ(linAccAxisNoiseElem->FirstChildElement("bias_mean")->GetText(),
+ "0.1");
+
+ EXPECT_STREQ(angVelAxisNoiseElem->FirstChildElement(
+ "bias_stddev")->GetText(), "8e-07");
+ EXPECT_STREQ(linAccAxisNoiseElem->FirstChildElement(
+ "bias_stddev")->GetText(), "0.001");
+
+ angVelAxisElem = angVelAxisElem->NextSiblingElement();
+ linAccAxisElem = linAccAxisElem->NextSiblingElement();
+ }
+}
+
+/////////////////////////////////////////////////
+/// Test conversion of gravity, magnetic_field in 1.5 to 1.6
+TEST(ConverterIntegration, World_15_to_16)
+{
+ // The gravity and magnetic_field in 1.5 format
+ std::string xmlString = R"(
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <physics type="ode">
+ <gravity>0 0 -9.8</gravity>
+ <magnetic_field>1 2 3</magnetic_field>
+ </physics>
+ </world>
+</sdf>)";
+
+ TiXmlDocument xmlDoc;
+ xmlDoc.Parse(xmlString.c_str());
+
+ // Convert
+ TiXmlDocument convertXmlDoc;
+ convertXmlDoc.LoadFile(CONVERT_DOC);
+ sdf::Converter::Convert(&xmlDoc, &convertXmlDoc);
+
+ // Check some basic elements
+ TiXmlElement *convertedElem = xmlDoc.FirstChildElement();
+ EXPECT_EQ(convertedElem->ValueStr(), "sdf");
+ convertedElem = convertedElem->FirstChildElement();
+ EXPECT_EQ(convertedElem->ValueStr(), "world");
+ convertedElem = convertedElem->FirstChildElement();
+ EXPECT_EQ(convertedElem->ValueStr(), "physics");
+
+ // Get the gravity
+ TiXmlElement *gravityElem = convertedElem->NextSiblingElement("gravity");
+ ASSERT_TRUE(gravityElem != NULL);
+ EXPECT_STREQ(gravityElem->GetText(), "0 0 -9.8");
+
+ // Get the magnetic_field
+ TiXmlElement *magneticFieldElem =
+ convertedElem->NextSiblingElement("magnetic_field");
+ ASSERT_TRUE(magneticFieldElem != NULL);
+ EXPECT_STREQ(magneticFieldElem->GetText(), "1 2 3");
+}
diff --git a/test/integration/fixed_joint_reduction.cc b/test/integration/fixed_joint_reduction.cc
index 9d5ee77..5b2d2d3 100644
--- a/test/integration/fixed_joint_reduction.cc
+++ b/test/integration/fixed_joint_reduction.cc
@@ -32,10 +32,28 @@ const std::string SDF_TEST_FILE_SIMPLE =
const std::string SDF_TEST_FILE_VISUAL =
std::string(PROJECT_SOURCE_PATH)
+ "/test/integration/fixed_joint_reduction_visual.urdf";
+const std::string SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION =
+ std::string(PROJECT_SOURCE_PATH)
+ + "/test/integration/fixed_joint_reduction_collision_visual_extension.urdf";
+const std::string SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_SDF =
+ std::string(PROJECT_SOURCE_PATH)
+ + "/test/integration/fixed_joint_reduction_collision_visual_extension.sdf";
+const std::string SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT =
+ std::string(PROJECT_SOURCE_PATH)
+ + "/test/integration/"
+ + "fixed_joint_reduction_collision_visual_empty_root.urdf";
+const std::string SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF =
+ std::string(PROJECT_SOURCE_PATH)
+ + "/test/integration/"
+ + "fixed_joint_reduction_collision_visual_empty_root.sdf";
const double gc_tolerance = 1e-6;
void FixedJointReductionEquivalence(const std::string &_file);
+void FixedJointReductionCollisionVisualExtension(
+ const std::string &_urdfFile, const std::string &_sdfFile);
+void FixedJointReductionCollisionVisualExtensionEmptyRoot(
+ const std::string &_urdfFile, const std::string &_sdfFile);
/////////////////////////////////////////////////
TEST(SDFParser, FixedJointReductionEquivalenceTest)
@@ -61,6 +79,422 @@ TEST(SDFParser, FixedJointReductionCollisionTest)
FixedJointReductionEquivalence(SDF_TEST_FILE_COLLISION);
}
+#if URDF_GE_0P3
+// this updated test will not work for old parser
+
+/////////////////////////////////////////////////
+// This test uses a urdf that has two levels
+// of fixed joint reduction.
+// Compare parsing results with a pre-built verified sdf
+// as well as with hardcoded expected values.
+TEST(SDFParser, FixedJointReductionCollisionVisualExtensionTest)
+{
+ FixedJointReductionCollisionVisualExtension(
+ SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION,
+ SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_SDF);
+}
+
+/////////////////////////////////////////////////
+// This test uses a urdf that has a fixed joint whose parent link
+// (base_link) has no collisions.
+// Compare parsing results with a pre-built verified sdf
+// as well as with hardcoded expected values.
+TEST(SDFParser, FixedJointReductionCollisionVisualExtensionEmptyRootTest)
+{
+ FixedJointReductionCollisionVisualExtensionEmptyRoot(
+ SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT,
+ SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF);
+}
+#endif
+
+/////////////////////////////////////////////////
+void FixedJointReductionCollisionVisualExtension(
+ const std::string &_urdfFile, const std::string &_sdfFile)
+{
+ // load urdf file, load sdf file.
+ // check to see that urdf load results are consistent with sdf load.
+
+ // load urdf
+ sdf::SDFPtr urdfRobot(new sdf::SDF());
+ sdf::init(urdfRobot);
+ ASSERT_TRUE(sdf::readFile(_urdfFile, urdfRobot));
+
+ // load sdf
+ sdf::SDFPtr sdfRobot(new sdf::SDF());
+ sdf::init(sdfRobot);
+ ASSERT_TRUE(sdf::readFile(_sdfFile, sdfRobot));
+
+ // check two loaded files, make sure they are the same
+ sdf::ElementPtr urdfModel = urdfRobot->Root()->GetElement("model");
+ sdf::ElementPtr urdf_child_link_1_col;
+ sdf::ElementPtr urdf_child_link_1a_col;
+ sdf::ElementPtr urdf_child_link_2_col;
+ sdf::ElementPtr urdf_child_link_1_vis;
+ sdf::ElementPtr urdf_child_link_1a_vis;
+ sdf::ElementPtr urdf_child_link_2_vis;
+ for (sdf::ElementPtr link = urdfModel->GetElement("link"); link;
+ link = link->GetNextElement("link"))
+ {
+ for (sdf::ElementPtr col = link->GetElement("collision"); col;
+ col = col->GetNextElement("collision"))
+ {
+ std::string colName = col->Get<std::string>("name");
+ if (colName == "base_link_fixed_joint_lump__child_link_1_collision_1")
+ {
+ urdf_child_link_1_col = col;
+ }
+ else if (colName ==
+ "base_link_fixed_joint_lump__child_link_1a_collision_3")
+ {
+ urdf_child_link_1a_col = col;
+ }
+ else if (colName ==
+ "base_link_fixed_joint_lump__child_link_2_collision_2")
+ {
+ urdf_child_link_2_col = col;
+ }
+ sdfmsg << "col: " << colName << "\n";
+ }
+ for (sdf::ElementPtr vis = link->GetElement("visual"); vis;
+ vis = vis->GetNextElement("visual"))
+ {
+ std::string visName = vis->Get<std::string>("name");
+ if (visName == "base_link_fixed_joint_lump__child_link_1_visual_1")
+ {
+ urdf_child_link_1_vis = vis;
+ }
+ else if (visName == "base_link_fixed_joint_lump__child_link_1a_visual_3")
+ {
+ urdf_child_link_1a_vis = vis;
+ }
+ else if (visName == "base_link_fixed_joint_lump__child_link_2_visual_2")
+ {
+ urdf_child_link_2_vis = vis;
+ }
+ sdfmsg << "vis: " << visName << "\n";
+ }
+ }
+ sdf::ElementPtr sdfModel = sdfRobot->Root()->GetElement("model");
+ sdf::ElementPtr sdf_child_link_1_col;
+ sdf::ElementPtr sdf_child_link_1a_col;
+ sdf::ElementPtr sdf_child_link_2_col;
+ sdf::ElementPtr sdf_child_link_1_vis;
+ sdf::ElementPtr sdf_child_link_1a_vis;
+ sdf::ElementPtr sdf_child_link_2_vis;
+ for (sdf::ElementPtr link = sdfModel->GetElement("link"); link;
+ link = link->GetNextElement("link"))
+ {
+ for (sdf::ElementPtr col = link->GetElement("collision"); col;
+ col = col->GetNextElement("collision"))
+ {
+ std::string colName = col->Get<std::string>("name");
+ if (colName == "base_link_fixed_joint_lump__child_link_1_collision_1")
+ {
+ sdf_child_link_1_col = col;
+ }
+ else if (colName ==
+ "base_link_fixed_joint_lump__child_link_1a_collision_3")
+ {
+ sdf_child_link_1a_col = col;
+ }
+ else if (colName ==
+ "base_link_fixed_joint_lump__child_link_2_collision_2")
+ {
+ sdf_child_link_2_col = col;
+ }
+ sdfmsg << "col: " << colName << "\n";
+ }
+ for (sdf::ElementPtr vis = link->GetElement("visual"); vis;
+ vis = vis->GetNextElement("visual"))
+ {
+ std::string visName = vis->Get<std::string>("name");
+ if (visName == "base_link_fixed_joint_lump__child_link_1_visual_1")
+ {
+ sdf_child_link_1_vis = vis;
+ }
+ else if (visName == "base_link_fixed_joint_lump__child_link_1a_visual_3")
+ {
+ sdf_child_link_1a_vis = vis;
+ }
+ else if (visName == "base_link_fixed_joint_lump__child_link_2_visual_2")
+ {
+ sdf_child_link_2_vis = vis;
+ }
+ sdfmsg << "vis: " << visName << "\n";
+ }
+ }
+ // child_link_1
+ // <collision name='base_link_fixed_joint_lump__child_link_1_collision_1'>
+ // <minDepth>0.007</minDepth>
+ // <mu1>0.7</mu1>
+ // <mu2>0.71</mu2>
+ // <visual name='base_link_fixed_joint_lump__child_link_1_visual_1'>
+ // <ambient>0 1 0 1</ambient>
+ // <script><name>script_uri_71_name</name></script>
+ EXPECT_EQ(urdf_child_link_1_col->Get<int>("max_contacts"), 177);
+ EXPECT_EQ(urdf_child_link_1_col->Get<int>("max_contacts"),
+ sdf_child_link_1_col->Get<int>("max_contacts"));
+
+ double urdf_mu1 = urdf_child_link_1_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu");
+ double sdf_mu1 = sdf_child_link_1_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu");
+ sdfmsg << "urdf mu1: " << urdf_mu1 << "\n";
+ EXPECT_FLOAT_EQ(urdf_mu1, 0.7);
+ EXPECT_FLOAT_EQ(urdf_mu1, sdf_mu1);
+
+ double urdf_mu2 = urdf_child_link_1_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu2");
+ double sdf_mu2 = sdf_child_link_1_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu2");
+ sdfmsg << "urdf mu2: " << urdf_mu2 << "\n";
+ EXPECT_FLOAT_EQ(urdf_mu2, 0.71);
+ EXPECT_FLOAT_EQ(urdf_mu2, sdf_mu2);
+
+ EXPECT_EQ(urdf_child_link_1_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("name"),
+ "script_uri_71_name");
+ EXPECT_EQ(urdf_child_link_1_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("name"),
+ sdf_child_link_1_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("name"));
+
+ EXPECT_EQ(urdf_child_link_1_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("uri"),
+ "file://media/materials/scripts/gazebo.material");
+
+ EXPECT_EQ(urdf_child_link_1_vis->GetElement("material")->
+ Get<sdf::Color>("ambient"), sdf::Color(0, 1, 0, 1));
+ EXPECT_EQ(urdf_child_link_1_vis->GetElement("material")->
+ Get<sdf::Color>("ambient"),
+ sdf_child_link_1_vis->GetElement("material")->
+ Get<sdf::Color>("ambient"));
+
+ // child_link_1a
+ // <collision name='base_link_fixed_joint_lump__child_link_1a_collision_3'>
+ // <maxContacts>166</maxContacts>
+ // <mu1>0.6</mu1>
+ // <mu2>0.61</mu2>
+ // <visual name='base_link_fixed_joint_lump__child_link_1a_visual_3'>
+ // <material>
+ // <script>
+ // <uri>__default__</uri>
+ // <name>__default__</name>
+ // </script>
+ // </material>
+ EXPECT_EQ(urdf_child_link_1a_col->Get<int>("max_contacts"), 166);
+ EXPECT_EQ(urdf_child_link_1a_col->Get<int>("max_contacts"),
+ sdf_child_link_1a_col->Get<int>("max_contacts"));
+
+ EXPECT_FLOAT_EQ(urdf_child_link_1a_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu"), 0.6);
+ EXPECT_FLOAT_EQ(urdf_child_link_1a_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu"),
+ sdf_child_link_1a_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu"));
+
+ EXPECT_FLOAT_EQ(urdf_child_link_1a_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu2"), 0.61);
+ EXPECT_FLOAT_EQ(urdf_child_link_1a_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu2"),
+ sdf_child_link_1a_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu2"));
+
+ EXPECT_EQ(urdf_child_link_1a_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("name"),
+ "__default__");
+ EXPECT_EQ(urdf_child_link_1a_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("name"),
+ sdf_child_link_1a_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("name"));
+
+ EXPECT_EQ(urdf_child_link_1a_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("uri"),
+ "__default__");
+
+ // ambient unassigned should be 0, 0, 0, 1
+ EXPECT_EQ(urdf_child_link_1a_vis->GetElement("material")->
+ Get<sdf::Color>("ambient"), sdf::Color(0, 0, 0, 1));
+ EXPECT_EQ(urdf_child_link_1a_vis->GetElement("material")->
+ Get<sdf::Color>("ambient"),
+ sdf_child_link_1a_vis->GetElement("material")->
+ Get<sdf::Color>("ambient"));
+
+ // child_link_2
+ // <collision name='base_link_fixed_joint_lump__child_link_2_collision_2'>
+ // <mu1>0.5</mu1>
+ // <mu2>0.51</mu2>
+ // <visual name='base_link_fixed_joint_lump__child_link_2_visual_2'>
+ // <uri>script_uri_51</uri>
+ // <name>script_name_51</name>
+ // unassigne max_contacts defaut is 10
+ EXPECT_EQ(urdf_child_link_2_col->Get<int>("max_contacts"), 10);
+ EXPECT_EQ(urdf_child_link_2_col->Get<int>("max_contacts"),
+ sdf_child_link_2_col->Get<int>("max_contacts"));
+
+ EXPECT_FLOAT_EQ(urdf_child_link_2_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu"), 0.5);
+ EXPECT_FLOAT_EQ(urdf_child_link_2_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu"),
+ sdf_child_link_2_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu"));
+
+ EXPECT_FLOAT_EQ(urdf_child_link_2_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu2"), 0.51);
+ EXPECT_FLOAT_EQ(urdf_child_link_2_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu2"),
+ sdf_child_link_2_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu2"));
+
+ EXPECT_EQ(urdf_child_link_2_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("name"),
+ "script_name_51");
+ EXPECT_EQ(urdf_child_link_2_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("name"),
+ sdf_child_link_2_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("name"));
+
+ EXPECT_EQ(urdf_child_link_2_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("uri"),
+ "script_uri_51");
+
+ // ambient unassigned should be 0, 0, 0, 1
+ EXPECT_EQ(urdf_child_link_2_vis->GetElement("material")->
+ Get<sdf::Color>("ambient"), sdf::Color(0, 0, 0, 1));
+ EXPECT_EQ(urdf_child_link_2_vis->GetElement("material")->
+ Get<sdf::Color>("ambient"),
+ sdf_child_link_2_vis->GetElement("material")->
+ Get<sdf::Color>("ambient"));
+}
+
+/////////////////////////////////////////////////
+void FixedJointReductionCollisionVisualExtensionEmptyRoot(
+ const std::string &_urdfFile, const std::string &_sdfFile)
+{
+ // load urdf file, load sdf file.
+ // check to see that urdf load results are consistent with sdf load.
+
+ // load urdf
+ sdf::SDFPtr urdfRobot(new sdf::SDF());
+ sdf::init(urdfRobot);
+ ASSERT_TRUE(sdf::readFile(_urdfFile, urdfRobot));
+
+ // load sdf
+ sdf::SDFPtr sdfRobot(new sdf::SDF());
+ sdf::init(sdfRobot);
+ ASSERT_TRUE(sdf::readFile(_sdfFile, sdfRobot));
+
+ // check two loaded files, make sure they are the same
+ sdf::ElementPtr urdfModel = urdfRobot->Root()->GetElement("model");
+ sdf::ElementPtr urdf_child_link_1_col;
+ sdf::ElementPtr urdf_child_link_1_vis;
+ for (sdf::ElementPtr link = urdfModel->GetElement("link"); link;
+ link = link->GetNextElement("link"))
+ {
+ for (sdf::ElementPtr col = link->GetElement("collision"); col;
+ col = col->GetNextElement("collision"))
+ {
+ std::string colName = col->Get<std::string>("name");
+ // note that if there is only one child visual, no _1 is appended
+ // compare this with FixedJointReductionCollisionVisualExtension test
+ if (colName == "base_link_fixed_joint_lump__child_link_1_collision")
+ {
+ urdf_child_link_1_col = col;
+ }
+ sdfmsg << "col: " << colName << "\n";
+ }
+ for (sdf::ElementPtr vis = link->GetElement("visual"); vis;
+ vis = vis->GetNextElement("visual"))
+ {
+ std::string visName = vis->Get<std::string>("name");
+ // note that if there is only one child visual, no _1 is appended
+ // compare this with FixedJointReductionCollisionVisualExtension test
+ if (visName == "base_link_fixed_joint_lump__child_link_1_visual")
+ {
+ urdf_child_link_1_vis = vis;
+ }
+ sdfmsg << "vis: " << visName << "\n";
+ }
+ }
+ sdf::ElementPtr sdfModel = sdfRobot->Root()->GetElement("model");
+ sdf::ElementPtr sdf_child_link_1_col;
+ sdf::ElementPtr sdf_child_link_1_vis;
+ for (sdf::ElementPtr link = sdfModel->GetElement("link"); link;
+ link = link->GetNextElement("link"))
+ {
+ for (sdf::ElementPtr col = link->GetElement("collision"); col;
+ col = col->GetNextElement("collision"))
+ {
+ std::string colName = col->Get<std::string>("name");
+ // note that if there is only one child visual, no _1 is appended
+ // compare this with FixedJointReductionCollisionVisualExtension test
+ if (colName == "base_link_fixed_joint_lump__child_link_1_collision")
+ {
+ sdf_child_link_1_col = col;
+ }
+ sdfmsg << "col: " << colName << "\n";
+ }
+ for (sdf::ElementPtr vis = link->GetElement("visual"); vis;
+ vis = vis->GetNextElement("visual"))
+ {
+ std::string visName = vis->Get<std::string>("name");
+ // note that if there is only one child visual, no _1 is appended
+ // compare this with FixedJointReductionCollisionVisualExtension test
+ if (visName == "base_link_fixed_joint_lump__child_link_1_visual")
+ {
+ sdf_child_link_1_vis = vis;
+ }
+ sdfmsg << "vis: " << visName << "\n";
+ }
+ }
+ // child_link_1
+ // <collision name='base_link_fixed_joint_lump__child_link_1_collision_1'>
+ // <minDepth>0.007</minDepth>
+ // <mu1>0.7</mu1>
+ // <mu2>0.71</mu2>
+ // <visual name='base_link_fixed_joint_lump__child_link_1_visual_1'>
+ // <ambient>0 1 0 1</ambient>
+ // <script><name>script_uri_71_name</name></script>
+ EXPECT_EQ(urdf_child_link_1_col->Get<int>("max_contacts"), 177);
+ EXPECT_EQ(urdf_child_link_1_col->Get<int>("max_contacts"),
+ sdf_child_link_1_col->Get<int>("max_contacts"));
+
+ double urdf_mu1 = urdf_child_link_1_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu");
+ double sdf_mu1 = sdf_child_link_1_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu");
+ sdfmsg << "urdf mu1: " << urdf_mu1 << "\n";
+ EXPECT_FLOAT_EQ(urdf_mu1, 0.7);
+ EXPECT_FLOAT_EQ(urdf_mu1, sdf_mu1);
+
+ double urdf_mu2 = urdf_child_link_1_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu2");
+ double sdf_mu2 = sdf_child_link_1_col->GetElement("surface")
+ ->GetElement("friction")->GetElement("ode")->Get<double>("mu2");
+ sdfmsg << "urdf mu2: " << urdf_mu2 << "\n";
+ EXPECT_FLOAT_EQ(urdf_mu2, 0.71);
+ EXPECT_FLOAT_EQ(urdf_mu2, sdf_mu2);
+
+ EXPECT_EQ(urdf_child_link_1_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("name"),
+ "script_uri_71_name");
+ EXPECT_EQ(urdf_child_link_1_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("name"),
+ sdf_child_link_1_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("name"));
+
+ EXPECT_EQ(urdf_child_link_1_vis->GetElement("material")->
+ GetElement("script")->Get<std::string>("uri"),
+ "file://media/materials/scripts/gazebo.material");
+
+ EXPECT_EQ(urdf_child_link_1_vis->GetElement("material")->
+ Get<sdf::Color>("ambient"), sdf::Color(0, 1, 0, 1));
+ EXPECT_EQ(urdf_child_link_1_vis->GetElement("material")->
+ Get<sdf::Color>("ambient"),
+ sdf_child_link_1_vis->GetElement("material")->
+ Get<sdf::Color>("ambient"));
+}
+
/////////////////////////////////////////////////
void FixedJointReductionEquivalence(const std::string &_file)
{
diff --git a/test/integration/fixed_joint_reduction_collision_visual_empty_root.sdf b/test/integration/fixed_joint_reduction_collision_visual_empty_root.sdf
new file mode 100644
index 0000000..1921346
--- /dev/null
+++ b/test/integration/fixed_joint_reduction_collision_visual_empty_root.sdf
@@ -0,0 +1,59 @@
+<sdf version='1.5'>
+ <model name='lumping_test'>
+ <link name='base_link'>
+ <pose>0 0 0 0 -0 0</pose>
+ <inertial>
+ <pose>0 0 0 0 -0 0</pose>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+ <collision name='base_link_fixed_joint_lump__child_link_1_collision'>
+ <pose>0 0 0 0 -0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>0.03</radius>
+ </sphere>
+ </geometry>
+ <max_contacts>177</max_contacts>
+ <surface>
+ <contact>
+ <ode>
+ <min_depth>0.007</min_depth>
+ </ode>
+ </contact>
+ <friction>
+ <ode>
+ <mu2>0.71</mu2>
+ <mu>0.7</mu>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+ <visual name='base_link_fixed_joint_lump__child_link_1_visual'>
+ <pose>0 0 0 0 -0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>0.03</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <ambient>0 1 0 1</ambient>
+ <script>
+ <name>script_uri_71_name</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ <gravity>1</gravity>
+ <velocity_decay/>
+ <self_collide>0</self_collide>
+ </link>
+ </model>
+</sdf>
diff --git a/test/integration/fixed_joint_reduction_collision_visual_empty_root.urdf b/test/integration/fixed_joint_reduction_collision_visual_empty_root.urdf
new file mode 100644
index 0000000..8358b47
--- /dev/null
+++ b/test/integration/fixed_joint_reduction_collision_visual_empty_root.urdf
@@ -0,0 +1,57 @@
+<robot name="lumping_test">
+ <!-- test if base_link is empty, lumping still works well -->
+ <link name="base_link"/>
+ <link name="child_link_1">
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="1.0" />
+ <inertia ixx="0.001" ixy="0.000" ixz="0.000" iyy="0.001" iyz="0.000" izz="0.001" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="0.03" />
+ </geometry>
+ <material name="">
+ <color rgba="0.1 0.1 0.1 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="0.03" />
+ </geometry>
+ </collision>
+ </link>
+ <joint name="fixed_joint_1" type="fixed">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <parent link="base_link" />
+ <child link="child_link_1" />
+ </joint>
+ <gazebo reference="child_link_1">
+ <minDepth>0.007</minDepth>
+ <mu1>0.7</mu1>
+ <collision>
+ <max_contacts>177</max_contacts>
+ <surface>
+ <contact>
+ <ode/>
+ </contact>
+ <friction>
+ <ode>
+ <mu2>0.71</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+ <visual>
+ <material>
+ <ambient>0 1 0 1</ambient>
+ </material>
+ </visual>
+ <material>
+ script_uri_71_name
+ </material>
+ </gazebo>
+</robot>
+
diff --git a/test/integration/fixed_joint_reduction_collision_visual_extension.sdf b/test/integration/fixed_joint_reduction_collision_visual_extension.sdf
new file mode 100644
index 0000000..02172fb
--- /dev/null
+++ b/test/integration/fixed_joint_reduction_collision_visual_extension.sdf
@@ -0,0 +1,144 @@
+<sdf version='1.5'>
+ <model name='lumping_test'>
+ <link name='base_link'>
+ <pose>0 0 0 0 -0 0</pose>
+ <inertial>
+ <pose>0 0 0 0 -0 0</pose>
+ <mass>4</mass>
+ <inertia>
+ <ixx>0.004</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.004</iyy>
+ <iyz>0</iyz>
+ <izz>0.004</izz>
+ </inertia>
+ </inertial>
+ <collision name='base_link_collision'>
+ <pose>0 0 0 0 -0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>0.02</radius>
+ </sphere>
+ </geometry>
+ </collision>
+ <collision name='base_link_fixed_joint_lump__child_link_1_collision_1'>
+ <pose>0 0 0 0 -0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>0.03</radius>
+ </sphere>
+ </geometry>
+ <max_contacts>177</max_contacts>
+ <surface>
+ <contact>
+ <ode>
+ <min_depth>0.007</min_depth>
+ </ode>
+ </contact>
+ <friction>
+ <ode>
+ <mu2>0.71</mu2>
+ <mu>0.7</mu>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+ <collision name='base_link_fixed_joint_lump__child_link_2_collision_2'>
+ <pose>0 0 0 0 -0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>0.05</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <contact>
+ <ode/>
+ </contact>
+ <friction>
+ <ode>
+ <mu2>0.51</mu2>
+ <mu>0.5</mu>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+ <collision name='base_link_fixed_joint_lump__child_link_1a_collision_3'>
+ <pose>0 0 0 0 -0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>0.03</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <contact>
+ <ode/>
+ </contact>
+ <friction>
+ <ode>
+ <mu2>0.61</mu2>
+ <mu>0.6</mu>
+ </ode>
+ </friction>
+ </surface>
+ <max_contacts>166</max_contacts>
+ </collision>
+ <visual name='base_link_visual'>
+ <pose>0 0 0 0 -0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>0.02</radius>
+ </sphere>
+ </geometry>
+ </visual>
+ <visual name='base_link_fixed_joint_lump__child_link_1_visual_1'>
+ <pose>0 0 0 0 -0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>0.03</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <ambient>0 1 0 1</ambient>
+ <script>
+ <name>script_uri_71_name</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ <visual name='base_link_fixed_joint_lump__child_link_2_visual_2'>
+ <pose>0 0 0 0 -0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>0.05</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <uri>script_uri_51</uri>
+ <name>script_name_51</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='base_link_fixed_joint_lump__child_link_1a_visual_3'>
+ <pose>0 0 0 0 -0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>0.03</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <uri>__default__</uri>
+ <name>__default__</name>
+ </script>
+ </material>
+ </visual>
+ <velocity_decay/>
+ <velocity_decay/>
+ <gravity>1</gravity>
+ <velocity_decay/>
+ <self_collide>0</self_collide>
+ </link>
+ </model>
+</sdf>
diff --git a/test/integration/fixed_joint_reduction_collision_visual_extension.urdf b/test/integration/fixed_joint_reduction_collision_visual_extension.urdf
new file mode 100644
index 0000000..edcbe96
--- /dev/null
+++ b/test/integration/fixed_joint_reduction_collision_visual_extension.urdf
@@ -0,0 +1,170 @@
+<robot name="lumping_test">
+ <link name="base_link">
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="1.0" />
+ <inertia ixx="0.001" ixy="0.000" ixz="0.000" iyy="0.001" iyz="0.000" izz="0.001" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="0.02" />
+ </geometry>
+ <material name="">
+ <color rgba="0.1 0.1 0.1 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="0.02" />
+ </geometry>
+ </collision>
+ </link>
+ <link name="child_link_1">
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="1.0" />
+ <inertia ixx="0.001" ixy="0.000" ixz="0.000" iyy="0.001" iyz="0.000" izz="0.001" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="0.03" />
+ </geometry>
+ <material name="">
+ <color rgba="0.1 0.1 0.1 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="0.03" />
+ </geometry>
+ </collision>
+ </link>
+ <joint name="fixed_joint_1" type="fixed">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <parent link="base_link" />
+ <child link="child_link_1" />
+ </joint>
+ <link name="child_link_1a">
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="1.0" />
+ <inertia ixx="0.001" ixy="0.000" ixz="0.000" iyy="0.001" iyz="0.000" izz="0.001" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="0.03" />
+ </geometry>
+ <material name="">
+ <color rgba="0.1 0.1 0.1 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="0.03" />
+ </geometry>
+ </collision>
+ </link>
+ <joint name="fixed_joint_1a" type="fixed">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <parent link="base_link" />
+ <child link="child_link_1a" />
+ </joint>
+ <link name="child_link_2">
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="1.0" />
+ <inertia ixx="0.001" ixy="0.000" ixz="0.000" iyy="0.001" iyz="0.000" izz="0.001" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="0.05" />
+ </geometry>
+ <material name="">
+ <color rgba="0.1 0.1 0.1 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <sphere radius="0.05" />
+ </geometry>
+ </collision>
+ </link>
+ <joint name="fixed_joint_2" type="fixed">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <parent link="child_link_1" />
+ <child link="child_link_2" />
+ </joint>
+ <gazebo reference="child_link_1">
+ <minDepth>0.007</minDepth>
+ <mu1>0.7</mu1>
+ <collision>
+ <max_contacts>177</max_contacts>
+ <surface>
+ <contact>
+ <ode/>
+ </contact>
+ <friction>
+ <ode>
+ <mu2>0.71</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+ <visual>
+ <material>
+ <ambient>0 1 0 1</ambient>
+ </material>
+ </visual>
+ <material>
+ script_uri_71_name
+ </material>
+ </gazebo>
+ <gazebo reference="child_link_1a">
+ <mu1>0.6</mu1>
+ <maxContacts>166</maxContacts>
+ <collision>
+ <surface>
+ <contact>
+ <ode/>
+ </contact>
+ <friction>
+ <ode>
+ <mu2>0.61</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+ </gazebo>
+ <gazebo reference="child_link_2">
+ <mu1>0.5</mu1>
+ <collision>
+ <surface>
+ <contact>
+ <ode/>
+ </contact>
+ <friction>
+ <ode>
+ <mu2>0.51</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+ <visual>
+ <material>
+ <script>
+ <uri>script_uri_51</uri>
+ <name>script_name_51</name>
+ </script>
+ </material>
+ </visual>
+ </gazebo>
+</robot>
+
diff --git a/test/integration/frame.cc b/test/integration/frame.cc
new file mode 100644
index 0000000..0d8f4d3
--- /dev/null
+++ b/test/integration/frame.cc
@@ -0,0 +1,1251 @@
+/*
+ * Copyright 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <gtest/gtest.h>
+#include <string>
+#include "sdf/sdf.hh"
+
+#include "test_config.h"
+
+////////////////////////////////////////
+// Test parsing a model element that has a frame element
+TEST(Frame, ModelFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<model name='my_model'>"
+ << " <frame name='mframe'>"
+ << " <pose frame='/world'>1 1 0 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='mframe'>1 0 0 0 0 0</pose>"
+ << " <link name='link'/>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // model
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("model"));
+ sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model");
+ EXPECT_TRUE(modelElem->HasAttribute("name"));
+ EXPECT_EQ(modelElem->Get<std::string>("name"), "my_model");
+
+ // model frame
+ EXPECT_TRUE(modelElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = modelElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "mframe");
+
+ // model frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "/world");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(1, 1, 0, 0, 0, 0));
+
+ // model pose
+ EXPECT_TRUE(modelElem->HasElement("pose"));
+ sdf::ElementPtr modelPoseElem = modelElem->GetElement("pose");
+ EXPECT_TRUE(modelPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(modelPoseElem->Get<std::string>("frame"), "mframe");
+ EXPECT_EQ(modelPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(1, 0, 0, 0, 0, 0));
+
+ // link
+ EXPECT_TRUE(modelElem->HasElement("link"));
+ sdf::ElementPtr linkElem = modelElem->GetElement("link");
+ EXPECT_TRUE(linkElem->HasAttribute("name"));
+ EXPECT_EQ(linkElem->Get<std::string>("name"), "link");
+}
+
+////////////////////////////////////////
+// Test parsing a model element with an empty frame element
+TEST(Frame, FrameDefaultPose)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<model name='my_model'>"
+ << " <frame name='mframe'/>"
+ << " <link name='link'/>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // model
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("model"));
+ sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model");
+ EXPECT_TRUE(modelElem->HasAttribute("name"));
+ EXPECT_EQ(modelElem->Get<std::string>("name"), "my_model");
+
+ // model frame
+ EXPECT_TRUE(modelElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = modelElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "mframe");
+
+ // model frame pose
+ EXPECT_TRUE(!frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
+
+ // link
+ EXPECT_TRUE(modelElem->HasElement("link"));
+ sdf::ElementPtr linkElem = modelElem->GetElement("link");
+ EXPECT_TRUE(linkElem->HasAttribute("name"));
+ EXPECT_EQ(linkElem->Get<std::string>("name"), "link");
+}
+
+////////////////////////////////////////
+// Test parsing a model element with no frames - for backward compatibility
+TEST(Frame, NoFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<model name='my_model'>"
+ << " <link name='link'/>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // model
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("model"));
+ sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model");
+ EXPECT_TRUE(modelElem->HasAttribute("name"));
+ EXPECT_EQ(modelElem->Get<std::string>("name"), "my_model");
+
+ {
+ // model frame
+ EXPECT_TRUE(!modelElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = modelElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "");
+
+ // model frame pose
+ EXPECT_TRUE(!frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
+ }
+
+ // link
+ EXPECT_TRUE(modelElem->HasElement("link"));
+ sdf::ElementPtr linkElem = modelElem->GetElement("link");
+ EXPECT_TRUE(linkElem->HasAttribute("name"));
+ EXPECT_EQ(linkElem->Get<std::string>("name"), "link");
+
+ {
+ // link frame
+ EXPECT_TRUE(!linkElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = linkElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "");
+
+ // link frame pose
+ EXPECT_TRUE(!frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
+ }
+}
+
+////////////////////////////////////////
+// Test parsing a link element that has a frame element
+TEST(Frame, LinkFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<model name='my_model'>"
+ << " <link name='link'>"
+ << " <frame name='lframe'>"
+ << " <pose frame='model'>1 0 2 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='lframe'>0 5 0 0 0 0</pose>"
+ << " </link>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // model
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("model"));
+ sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model");
+ EXPECT_TRUE(modelElem->HasAttribute("name"));
+ EXPECT_EQ(modelElem->Get<std::string>("name"), "my_model");
+
+ // link
+ EXPECT_TRUE(modelElem->HasElement("link"));
+ sdf::ElementPtr linkElem = modelElem->GetElement("link");
+ EXPECT_TRUE(linkElem->HasAttribute("name"));
+ EXPECT_EQ(linkElem->Get<std::string>("name"), "link");
+
+ // link frame
+ EXPECT_TRUE(linkElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = linkElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "lframe");
+
+ // link frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "model");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(1, 0, 2, 0, 0, 0));
+
+ // link pose
+ EXPECT_TRUE(linkElem->HasElement("pose"));
+ sdf::ElementPtr linkPoseElem = linkElem->GetElement("pose");
+ EXPECT_TRUE(linkPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(linkPoseElem->Get<std::string>("frame"), "lframe");
+ EXPECT_EQ(linkPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(0, 5, 0, 0, 0, 0));
+}
+
+////////////////////////////////////////
+// Test parsing a joint element that has a frame element
+TEST(Frame, JointFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<model name='my_model'>"
+ << " <link name='parent'/>"
+ << " <link name='child'/>"
+ << " <joint name='revjoint' type='revolute'>"
+ << " <parent>parent</parent>"
+ << " <child>child</child>"
+ << " <axis>"
+ << " <xyz>1 0 0</xyz>"
+ << " </axis>"
+ << " <frame name='jframe'>"
+ << " <pose frame='child'>0 0 1 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='jframe'>0 2 1 0 0 0</pose>"
+ << " </joint>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // model
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("model"));
+ sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model");
+ EXPECT_TRUE(modelElem->HasAttribute("name"));
+ EXPECT_EQ(modelElem->Get<std::string>("name"), "my_model");
+
+ // link
+ EXPECT_TRUE(modelElem->HasElement("link"));
+ sdf::ElementPtr linkElem = modelElem->GetElement("link");
+ EXPECT_TRUE(linkElem->HasAttribute("name"));
+ EXPECT_EQ(linkElem->Get<std::string>("name"), "parent");
+ linkElem = linkElem->GetNextElement("link");
+ EXPECT_TRUE(linkElem != NULL);
+ EXPECT_TRUE(linkElem->HasAttribute("name"));
+ EXPECT_EQ(linkElem->Get<std::string>("name"), "child");
+
+ // joint
+ EXPECT_TRUE(modelElem->HasElement("joint"));
+ sdf::ElementPtr jointElem = modelElem->GetElement("joint");
+ EXPECT_TRUE(jointElem->HasAttribute("name"));
+ EXPECT_EQ(jointElem->Get<std::string>("name"), "revjoint");
+ EXPECT_TRUE(jointElem->HasAttribute("type"));
+ EXPECT_EQ(jointElem->Get<std::string>("type"), "revolute");
+
+ // joint links
+ EXPECT_TRUE(jointElem->HasElement("parent"));
+ EXPECT_EQ(jointElem->Get<std::string>("parent"), "parent");
+ EXPECT_TRUE(jointElem->HasElement("child"));
+ EXPECT_EQ(jointElem->Get<std::string>("child"), "child");
+
+ // joint axis
+ EXPECT_TRUE(jointElem->HasElement("axis"));
+ sdf::ElementPtr axisElem = jointElem->GetElement("axis");
+
+ EXPECT_TRUE(axisElem->HasElement("xyz"));
+ EXPECT_EQ(axisElem->Get<ignition::math::Vector3d>("xyz"),
+ ignition::math::Vector3d(1, 0, 0));
+
+ // joint frame
+ EXPECT_TRUE(jointElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = jointElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "jframe");
+
+ // joint frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "child");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(0, 0, 1, 0, 0, 0));
+
+ // joint pose
+ EXPECT_TRUE(jointElem->HasElement("pose"));
+ sdf::ElementPtr jointPoseElem = jointElem->GetElement("pose");
+ EXPECT_TRUE(jointPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(jointPoseElem->Get<std::string>("frame"), "jframe");
+ EXPECT_EQ(jointPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(0, 2, 1, 0, 0, 0));
+}
+
+////////////////////////////////////////
+// Test parsing a collision element that has a frame element
+TEST(Frame, CollisionFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<model name='my_model'>"
+ << " <link name='link'>"
+ << " <collision name='collision'>"
+ << " <frame name='cframe'>"
+ << " <pose frame='link'>1 3 1 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='cframe'>0 2 0 0 0 0</pose>"
+ << " </collision>"
+ << " </link>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // model
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("model"));
+ sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model");
+ EXPECT_TRUE(modelElem->HasAttribute("name"));
+ EXPECT_EQ(modelElem->Get<std::string>("name"), "my_model");
+
+ // link
+ EXPECT_TRUE(modelElem->HasElement("link"));
+ sdf::ElementPtr linkElem = modelElem->GetElement("link");
+ EXPECT_TRUE(linkElem->HasAttribute("name"));
+ EXPECT_EQ(linkElem->Get<std::string>("name"), "link");
+
+ // collision
+ EXPECT_TRUE(linkElem->HasElement("collision"));
+ sdf::ElementPtr collisionElem = linkElem->GetElement("collision");
+ EXPECT_TRUE(collisionElem->HasAttribute("name"));
+ EXPECT_EQ(collisionElem->Get<std::string>("name"), "collision");
+
+ // collision frame
+ EXPECT_TRUE(collisionElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = collisionElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "cframe");
+
+ // collision frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "link");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(1, 3, 1, 0, 0, 0));
+
+ // collision pose
+ EXPECT_TRUE(collisionElem->HasElement("pose"));
+ sdf::ElementPtr collisionPoseElem = collisionElem->GetElement("pose");
+ EXPECT_TRUE(collisionPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(collisionPoseElem->Get<std::string>("frame"), "cframe");
+ EXPECT_EQ(collisionPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(0, 2, 0, 0, 0, 0));
+}
+
+////////////////////////////////////////
+// Test parsing a visual element that has a frame element
+TEST(Frame, VisualFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<model name='my_model'>"
+ << " <link name='link'>"
+ << " <visual name='visual'>"
+ << " <frame name='vframe'>"
+ << " <pose frame='link'>1 1 1 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='vframe'>2 2 2 0 0 0</pose>"
+ << " </visual>"
+ << " </link>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // model
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("model"));
+ sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model");
+ EXPECT_TRUE(modelElem->HasAttribute("name"));
+ EXPECT_EQ(modelElem->Get<std::string>("name"), "my_model");
+
+ // link
+ EXPECT_TRUE(modelElem->HasElement("link"));
+ sdf::ElementPtr linkElem = modelElem->GetElement("link");
+ EXPECT_TRUE(linkElem->HasAttribute("name"));
+ EXPECT_EQ(linkElem->Get<std::string>("name"), "link");
+
+ // visual
+ EXPECT_TRUE(linkElem->HasElement("visual"));
+ sdf::ElementPtr visualElem = linkElem->GetElement("visual");
+ EXPECT_TRUE(visualElem->HasAttribute("name"));
+ EXPECT_EQ(visualElem->Get<std::string>("name"), "visual");
+
+ // visual frame
+ EXPECT_TRUE(visualElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = visualElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "vframe");
+
+ // visual frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "link");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(1, 1, 1, 0, 0, 0));
+
+ // visual pose
+ EXPECT_TRUE(visualElem->HasElement("pose"));
+ sdf::ElementPtr visualPoseElem = visualElem->GetElement("pose");
+ EXPECT_TRUE(visualPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(visualPoseElem->Get<std::string>("frame"), "vframe");
+ EXPECT_EQ(visualPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(2, 2, 2, 0, 0, 0));
+}
+
+////////////////////////////////////////
+// Test parsing an inertial element that has a frame element
+TEST(Frame, InertialFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<model name='my_model'>"
+ << " <link name='link'>"
+ << " <inertial>"
+ << " <frame name='iframe'>"
+ << " <pose frame='link'>1 2 3 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='iframe'>3 2 1 0 0 0</pose>"
+ << " </inertial>"
+ << " </link>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // model
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("model"));
+ sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model");
+ EXPECT_TRUE(modelElem->HasAttribute("name"));
+ EXPECT_EQ(modelElem->Get<std::string>("name"), "my_model");
+
+ // link
+ EXPECT_TRUE(modelElem->HasElement("link"));
+ sdf::ElementPtr linkElem = modelElem->GetElement("link");
+ EXPECT_TRUE(linkElem->HasAttribute("name"));
+ EXPECT_EQ(linkElem->Get<std::string>("name"), "link");
+
+ // inertial
+ EXPECT_TRUE(linkElem->HasElement("inertial"));
+ sdf::ElementPtr inertialElem = linkElem->GetElement("inertial");
+
+ // inertial frame
+ EXPECT_TRUE(inertialElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = inertialElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "iframe");
+
+ // inertial frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "link");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(1, 2, 3, 0, 0, 0));
+
+ // inertial pose
+ EXPECT_TRUE(inertialElem->HasElement("pose"));
+ sdf::ElementPtr inertialPoseElem = inertialElem->GetElement("pose");
+ EXPECT_TRUE(inertialPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(inertialPoseElem->Get<std::string>("frame"), "iframe");
+ EXPECT_EQ(inertialPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(3, 2, 1, 0, 0, 0));
+}
+
+////////////////////////////////////////
+// Test parsing a light element that has a frame element
+TEST(Frame, LightFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<light type= 'directional' name='my_light'>"
+ << " <frame name='lframe'>"
+ << " <pose frame='/world'>0.1 10 0 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='lframe'>0.1 0 0 0 0 0</pose>"
+ << " <diffuse>0.2 0.3 0.4 1</diffuse>"
+ << " <specular>0.3 0.4 0.5 1</specular>"
+ << "</light>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // light
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("light"));
+ sdf::ElementPtr lightElem = sdfParsed->Root()->GetElement("light");
+ EXPECT_TRUE(lightElem->HasAttribute("name"));
+ EXPECT_EQ(lightElem->Get<std::string>("name"), "my_light");
+ EXPECT_TRUE(lightElem->HasAttribute("type"));
+ EXPECT_EQ(lightElem->Get<std::string>("type"), "directional");
+
+ // light frame
+ EXPECT_TRUE(lightElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = lightElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "lframe");
+
+ // light frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "/world");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(0.1, 10, 0, 0, 0, 0));
+
+ // light pose
+ EXPECT_TRUE(lightElem->HasElement("pose"));
+ sdf::ElementPtr lightPoseElem = lightElem->GetElement("pose");
+ EXPECT_TRUE(lightPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(lightPoseElem->Get<std::string>("frame"), "lframe");
+ EXPECT_EQ(lightPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(0.1, 0, 0, 0, 0, 0));
+
+ // diffuse
+ EXPECT_TRUE(lightElem->HasElement("diffuse"));
+ sdf::ElementPtr diffuseElem = lightElem->GetElement("diffuse");
+ EXPECT_EQ(diffuseElem->Get<sdf::Color>(), sdf::Color(0.2, 0.3, 0.4, 1));
+
+ // specular
+ EXPECT_TRUE(lightElem->HasElement("specular"));
+ sdf::ElementPtr specularElem = lightElem->GetElement("specular");
+ EXPECT_EQ(specularElem->Get<sdf::Color>(), sdf::Color(0.3, 0.4, 0.5, 1));
+}
+
+////////////////////////////////////////
+// Test parsing an actor element that has a frame element
+TEST(Frame, ActorFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<actor name='my_actor'>"
+ << " <frame name='aframe'>"
+ << " <pose frame='/world'>1 5 0 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='aframe'>0.1 3 0 0 0 0</pose>"
+ << " <skin>"
+ << " <filename>moonwalk.dae</filename>"
+ << " </skin>"
+ << " <animation name='walking'>"
+ << " <filename>walk.dae</filename>"
+ << " </animation>"
+ << " <script/>"
+ << "</actor>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // actor
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("actor"));
+ sdf::ElementPtr actorElem = sdfParsed->Root()->GetElement("actor");
+ EXPECT_TRUE(actorElem->HasAttribute("name"));
+ EXPECT_EQ(actorElem->Get<std::string>("name"), "my_actor");
+
+ // actor frame
+ EXPECT_TRUE(actorElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = actorElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "aframe");
+
+ // actor frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "/world");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(1, 5, 0, 0, 0, 0));
+
+ // actor pose
+ EXPECT_TRUE(actorElem->HasElement("pose"));
+ sdf::ElementPtr actorPoseElem = actorElem->GetElement("pose");
+ EXPECT_TRUE(actorPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(actorPoseElem->Get<std::string>("frame"), "aframe");
+ EXPECT_EQ(actorPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(0.1, 3, 0, 0, 0, 0));
+
+ // skin
+ EXPECT_TRUE(actorElem->HasElement("skin"));
+ sdf::ElementPtr skinElem = actorElem->GetElement("skin");
+ EXPECT_TRUE(skinElem->HasElement("filename"));
+ sdf::ElementPtr filenameElem = skinElem->GetElement("filename");
+ EXPECT_EQ(filenameElem->Get<std::string>(), "moonwalk.dae");
+
+ // animation
+ EXPECT_TRUE(actorElem->HasElement("animation"));
+ sdf::ElementPtr animationElem = actorElem->GetElement("animation");
+ EXPECT_TRUE(animationElem->HasAttribute("name"));
+ EXPECT_EQ(animationElem->Get<std::string>("name"), "walking");
+ EXPECT_TRUE(animationElem->HasElement("filename"));
+ filenameElem = animationElem->GetElement("filename");
+ EXPECT_EQ(filenameElem->Get<std::string>(), "walk.dae");
+}
+
+
+////////////////////////////////////////
+// Test parsing nested model states
+TEST(NestedModel, StateFrame)
+{
+ std::ostringstream sdfStr;
+ sdfStr << "<sdf version ='" << SDF_VERSION << "'>"
+ << "<world name='default'>"
+ << "<state world_name='default'>"
+ << "<model name='my_model'>"
+ << " <frame name='mframe'>"
+ << " <pose frame='/world'>1 0 2 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='mframe'>3 3 9 0 0 0</pose>"
+ << " <link name='my_link'>"
+ << " <frame name='lframe'>"
+ << " <pose frame='mframe'>8 5 2 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='lframe'>111 3 0 0 0 0</pose>"
+ << " </link>"
+ << "</model>"
+ << "<light name='my_light'>"
+ << " <frame name='lframe'>"
+ << " <pose frame='/world'>1 0 1 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='lframe'>99 0 22 0 0 0</pose>"
+ << "</light>"
+ << "</state>"
+ << "</world>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(sdfStr.str(), sdfParsed));
+
+ // load the state sdf
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("world"));
+ sdf::ElementPtr worldElem = sdfParsed->Root()->GetElement("world");
+ EXPECT_TRUE(worldElem->HasElement("state"));
+ sdf::ElementPtr stateElem = worldElem->GetElement("state");
+
+ EXPECT_TRUE(stateElem->HasElement("model"));
+ sdf::ElementPtr modelStateElem = stateElem->GetElement("model");
+
+ // model
+ EXPECT_TRUE(modelStateElem->HasAttribute("name"));
+ EXPECT_EQ(modelStateElem->Get<std::string>("name"), "my_model");
+
+ {
+ // model frame
+ EXPECT_TRUE(modelStateElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = modelStateElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "mframe");
+
+ // model frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "/world");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(1, 0, 2, 0, 0, 0));
+
+ // model pose
+ EXPECT_TRUE(modelStateElem->HasElement("pose"));
+ sdf::ElementPtr modelPoseElem = modelStateElem->GetElement("pose");
+ EXPECT_TRUE(modelPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(modelPoseElem->Get<std::string>("frame"), "mframe");
+ EXPECT_EQ(modelPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(3, 3, 9, 0, 0, 0));
+ }
+
+ // link
+ EXPECT_TRUE(modelStateElem->HasElement("link"));
+ sdf::ElementPtr linkStateElem = modelStateElem->GetElement("link");
+ EXPECT_TRUE(linkStateElem->HasAttribute("name"));
+ EXPECT_EQ(linkStateElem->Get<std::string>("name"), "my_link");
+
+ {
+ // link frame
+ EXPECT_TRUE(linkStateElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = linkStateElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "lframe");
+
+ // link frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "mframe");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(8, 5, 2, 0, 0, 0));
+
+ // link pose
+ EXPECT_TRUE(linkStateElem->HasElement("pose"));
+ sdf::ElementPtr linkPoseElem = linkStateElem->GetElement("pose");
+ EXPECT_TRUE(linkPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(linkPoseElem->Get<std::string>("frame"), "lframe");
+ EXPECT_EQ(linkPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(111, 3, 0, 0, 0, 0));
+ }
+
+ EXPECT_TRUE(stateElem->HasElement("light"));
+ sdf::ElementPtr lightStateElem = stateElem->GetElement("light");
+
+ // light
+ EXPECT_TRUE(lightStateElem->HasAttribute("name"));
+ EXPECT_EQ(lightStateElem->Get<std::string>("name"), "my_light");
+
+ {
+ // light frame
+ EXPECT_TRUE(lightStateElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = lightStateElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "lframe");
+
+ // light frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "/world");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(1, 0, 1, 0, 0, 0));
+
+ // light pose
+ EXPECT_TRUE(lightStateElem->HasElement("pose"));
+ sdf::ElementPtr lightPoseElem = lightStateElem->GetElement("pose");
+ EXPECT_TRUE(lightPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(lightPoseElem->Get<std::string>("frame"), "lframe");
+ EXPECT_EQ(lightPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(99, 0, 22, 0, 0, 0));
+ }
+}
+
+////////////////////////////////////////
+// Test parsing a projector element that has a frame element
+TEST(Frame, ProjectorFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<model name='my_model'>"
+ << " <link name='my_link'>"
+ << " <projector name='my_projector'>"
+ << " <frame name='pframe'>"
+ << " <pose frame='link'>1 -1 0 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='pframe'>-1 0 0 0 0 0</pose>"
+ << " </projector>"
+ << " </link>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // model
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("model"));
+ sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model");
+ EXPECT_TRUE(modelElem->HasAttribute("name"));
+ EXPECT_EQ(modelElem->Get<std::string>("name"), "my_model");
+
+ // link
+ EXPECT_TRUE(modelElem->HasElement("link"));
+ sdf::ElementPtr linkElem = modelElem->GetElement("link");
+ EXPECT_TRUE(linkElem->HasAttribute("name"));
+ EXPECT_EQ(linkElem->Get<std::string>("name"), "my_link");
+
+ // projector
+ EXPECT_TRUE(linkElem->HasElement("projector"));
+ sdf::ElementPtr projectorElem = linkElem->GetElement("projector");
+ EXPECT_TRUE(projectorElem->HasAttribute("name"));
+ EXPECT_EQ(projectorElem->Get<std::string>("name"), "my_projector");
+
+ // projector frame
+ EXPECT_TRUE(projectorElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = projectorElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "pframe");
+
+ // projector frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "link");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(1, -1, 0, 0, 0, 0));
+
+ // projector pose
+ EXPECT_TRUE(projectorElem->HasElement("pose"));
+ sdf::ElementPtr projectorPoseElem = projectorElem->GetElement("pose");
+ EXPECT_TRUE(projectorPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(projectorPoseElem->Get<std::string>("frame"), "pframe");
+ EXPECT_EQ(projectorPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(-1, 0, 0, 0, 0, 0));
+}
+
+////////////////////////////////////////
+// Test parsing a sensor element that has a frame element
+TEST(Frame, SensorFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<model name='my_model'>"
+ << " <link name='my_link'>"
+ << " <sensor name='my_sensor' type='ray'>"
+ << " <frame name='sframe'>"
+ << " <pose frame='link'>1 -1 1 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='sframe'>1 2 2 0 0 0</pose>"
+ << " </sensor>"
+ << " </link>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // model
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("model"));
+ sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model");
+ EXPECT_TRUE(modelElem->HasAttribute("name"));
+ EXPECT_EQ(modelElem->Get<std::string>("name"), "my_model");
+
+ // link
+ EXPECT_TRUE(modelElem->HasElement("link"));
+ sdf::ElementPtr linkElem = modelElem->GetElement("link");
+ EXPECT_TRUE(linkElem->HasAttribute("name"));
+ EXPECT_EQ(linkElem->Get<std::string>("name"), "my_link");
+
+ // sensor
+ EXPECT_TRUE(linkElem->HasElement("sensor"));
+ sdf::ElementPtr sensorElem = linkElem->GetElement("sensor");
+ EXPECT_TRUE(sensorElem->HasAttribute("name"));
+ EXPECT_EQ(sensorElem->Get<std::string>("name"), "my_sensor");
+ EXPECT_TRUE(sensorElem->HasAttribute("type"));
+ EXPECT_EQ(sensorElem->Get<std::string>("type"), "ray");
+
+ // sensor frame
+ EXPECT_TRUE(sensorElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = sensorElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "sframe");
+
+ // sensor frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "link");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(1, -1, 1, 0, 0, 0));
+
+ // sensor pose
+ EXPECT_TRUE(sensorElem->HasElement("pose"));
+ sdf::ElementPtr sensorPoseElem = sensorElem->GetElement("pose");
+ EXPECT_TRUE(sensorPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(sensorPoseElem->Get<std::string>("frame"), "sframe");
+ EXPECT_EQ(sensorPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(1, 2, 2, 0, 0, 0));
+}
+
+////////////////////////////////////////
+// Test parsing a camera sensor element that has a frame element
+TEST(Frame, CameraFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<model name='my_model'>"
+ << " <link name='my_link'>"
+ << " <sensor name='my_sensor' type='camera'>"
+ << " <camera>"
+ << " <frame name='cframe'>"
+ << " <pose frame='link'>-1.3 0 1 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='cframe'>4 2 2 0 0 0</pose>"
+ << " </camera>"
+ << " </sensor>"
+ << " </link>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // model
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("model"));
+ sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model");
+ EXPECT_TRUE(modelElem->HasAttribute("name"));
+ EXPECT_EQ(modelElem->Get<std::string>("name"), "my_model");
+
+ // link
+ EXPECT_TRUE(modelElem->HasElement("link"));
+ sdf::ElementPtr linkElem = modelElem->GetElement("link");
+ EXPECT_TRUE(linkElem->HasAttribute("name"));
+ EXPECT_EQ(linkElem->Get<std::string>("name"), "my_link");
+
+ // sensor
+ EXPECT_TRUE(linkElem->HasElement("sensor"));
+ sdf::ElementPtr sensorElem = linkElem->GetElement("sensor");
+ EXPECT_TRUE(sensorElem->HasAttribute("name"));
+ EXPECT_EQ(sensorElem->Get<std::string>("name"), "my_sensor");
+ EXPECT_TRUE(sensorElem->HasAttribute("type"));
+ EXPECT_EQ(sensorElem->Get<std::string>("type"), "camera");
+
+ // sensor
+ EXPECT_TRUE(sensorElem->HasElement("camera"));
+ sdf::ElementPtr cameraElem = sensorElem->GetElement("camera");
+
+ // camera frame
+ EXPECT_TRUE(cameraElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = cameraElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "cframe");
+
+ // camera frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "link");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(-1.3, 0, 1, 0, 0, 0));
+
+ // camera pose
+ EXPECT_TRUE(cameraElem->HasElement("pose"));
+ sdf::ElementPtr cameraPoseElem = cameraElem->GetElement("pose");
+ EXPECT_TRUE(cameraPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(cameraPoseElem->Get<std::string>("frame"), "cframe");
+ EXPECT_EQ(cameraPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(4, 2, 2, 0, 0, 0));
+}
+
+////////////////////////////////////////
+// Test parsing a audio source element that has a frame element
+TEST(Frame, AudioSourceFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<model name='my_model'>"
+ << " <link name='my_link'>"
+ << " <audio_source>"
+ << " <frame name='asframe'>"
+ << " <pose frame='link'>0 -1 0 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='asframe'>0 0 2 0 0 0</pose>"
+ << " </audio_source>"
+ << " </link>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // model
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("model"));
+ sdf::ElementPtr modelElem = sdfParsed->Root()->GetElement("model");
+ EXPECT_TRUE(modelElem->HasAttribute("name"));
+ EXPECT_EQ(modelElem->Get<std::string>("name"), "my_model");
+
+ // link
+ EXPECT_TRUE(modelElem->HasElement("link"));
+ sdf::ElementPtr linkElem = modelElem->GetElement("link");
+ EXPECT_TRUE(linkElem->HasAttribute("name"));
+ EXPECT_EQ(linkElem->Get<std::string>("name"), "my_link");
+
+ // audio source
+ EXPECT_TRUE(linkElem->HasElement("audio_source"));
+ sdf::ElementPtr audioSourceElem = linkElem->GetElement("audio_source");
+
+ // audio source frame
+ EXPECT_TRUE(audioSourceElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = audioSourceElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "asframe");
+
+ // audio source frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "link");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(0, -1, 0, 0, 0, 0));
+
+ // audio source pose
+ EXPECT_TRUE(audioSourceElem->HasElement("pose"));
+ sdf::ElementPtr audioSourcePoseElem = audioSourceElem->GetElement("pose");
+ EXPECT_TRUE(audioSourcePoseElem->HasAttribute("frame"));
+ EXPECT_EQ(audioSourcePoseElem->Get<std::string>("frame"), "asframe");
+ EXPECT_EQ(audioSourcePoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(0, 0, 2, 0, 0, 0));
+}
+
+////////////////////////////////////////
+// Test parsing a population element that has a frame element
+TEST(Frame, PopulationFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<world name='default'>"
+ << "<population name='my_population'>"
+ << " <frame name='pframe'>"
+ << " <pose frame='/world'>0 1 0 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='pframe'>0 0 0.2 0 0 0</pose>"
+ << "</population>"
+ << "</world>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // population
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("world"));
+ sdf::ElementPtr worldElem = sdfParsed->Root()->GetElement("world");
+ EXPECT_TRUE(worldElem->HasElement("population"));
+ sdf::ElementPtr populationElem = worldElem->GetElement("population");
+ EXPECT_TRUE(populationElem->HasAttribute("name"));
+ EXPECT_EQ(populationElem->Get<std::string>("name"), "my_population");
+
+ // population frame
+ EXPECT_TRUE(populationElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = populationElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "pframe");
+
+ // population frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "/world");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(0, 1, 0, 0, 0, 0));
+
+ // population pose
+ EXPECT_TRUE(populationElem->HasElement("pose"));
+ sdf::ElementPtr populationPoseElem = populationElem->GetElement("pose");
+ EXPECT_TRUE(populationPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(populationPoseElem->Get<std::string>("frame"), "pframe");
+ EXPECT_EQ(populationPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(0, 0, 0.2, 0, 0, 0));
+}
+
+////////////////////////////////////////
+// Test parsing a gui camera element that has a frame element
+TEST(Frame, GuiCameraFrame)
+{
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<world name='default'>"
+ << "<gui>"
+ << " <camera name='my_camera'>"
+ << " <frame name='cframe'>"
+ << " <pose frame='/world'>1.2 1 0 0 0 0</pose>"
+ << " </frame>"
+ << " <pose frame='cframe'>3.1 22 0 0 0 0</pose>"
+ << " </camera>"
+ << "</gui>"
+ << "</world>"
+ << "</sdf>";
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // gui
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("world"));
+ sdf::ElementPtr worldElem = sdfParsed->Root()->GetElement("world");
+ EXPECT_TRUE(worldElem->HasElement("gui"));
+ sdf::ElementPtr guiElem = worldElem->GetElement("gui");
+
+ // camera
+ EXPECT_TRUE(guiElem->HasElement("camera"));
+ sdf::ElementPtr cameraElem = guiElem->GetElement("camera");
+ EXPECT_TRUE(cameraElem->HasAttribute("name"));
+ EXPECT_EQ(cameraElem->Get<std::string>("name"), "my_camera");
+
+ // camera frame
+ EXPECT_TRUE(cameraElem->HasElement("frame"));
+ sdf::ElementPtr frameElem = cameraElem->GetElement("frame");
+ EXPECT_TRUE(frameElem->HasAttribute("name"));
+ EXPECT_EQ(frameElem->Get<std::string>("name"), "cframe");
+
+ // camera frame pose
+ EXPECT_TRUE(frameElem->HasElement("pose"));
+ sdf::ElementPtr poseElem = frameElem->GetElement("pose");
+ EXPECT_TRUE(poseElem->HasAttribute("frame"));
+ EXPECT_EQ(poseElem->Get<std::string>("frame"), "/world");
+ EXPECT_EQ(poseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(1.2, 1, 0, 0, 0, 0));
+
+ // camera pose
+ EXPECT_TRUE(cameraElem->HasElement("pose"));
+ sdf::ElementPtr cameraPoseElem = cameraElem->GetElement("pose");
+ EXPECT_TRUE(cameraPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(cameraPoseElem->Get<std::string>("frame"), "cframe");
+ EXPECT_EQ(cameraPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(3.1, 22, 0, 0, 0, 0));
+}
+
+////////////////////////////////////////
+// Test parsing a include element that has a pose element
+TEST(Frame, IncludeFrame)
+{
+ const std::string MODEL_PATH = std::string(PROJECT_SOURCE_PATH)
+ + "/test/integration/model/box";
+
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << "<world name='default'>"
+ << "<include>"
+ << " <name>my_model</name>"
+ << " <pose frame='/world'>5 -2 1 0 0 0</pose>"
+ << " <uri>" + MODEL_PATH +"</uri>"
+ << "</include>"
+ << "</world>"
+ << "</sdf>";
+
+
+ sdf::SDFPtr sdfParsed(new sdf::SDF());
+ sdf::init(sdfParsed);
+ ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+ // Verify correct parsing
+
+ // model
+ EXPECT_TRUE(sdfParsed->Root()->HasElement("world"));
+ sdf::ElementPtr worldElem = sdfParsed->Root()->GetElement("world");
+
+ EXPECT_TRUE(worldElem->HasElement("model"));
+ sdf::ElementPtr modelElem = worldElem->GetElement("model");
+ EXPECT_TRUE(modelElem->HasAttribute("name"));
+ EXPECT_EQ(modelElem->Get<std::string>("name"), "my_model");
+
+ // model pose
+ EXPECT_TRUE(modelElem->HasElement("pose"));
+ sdf::ElementPtr modelPoseElem = modelElem->GetElement("pose");
+ EXPECT_TRUE(modelPoseElem->HasAttribute("frame"));
+ EXPECT_EQ(modelPoseElem->Get<std::string>("frame"), "/world");
+ EXPECT_EQ(modelPoseElem->Get<ignition::math::Pose3d>(),
+ ignition::math::Pose3d(5, -2, 1, 0, 0, 0));
+}
diff --git a/test/integration/include.cc b/test/integration/include.cc
new file mode 100644
index 0000000..b656a61
--- /dev/null
+++ b/test/integration/include.cc
@@ -0,0 +1,54 @@
+/*
+ * Copyright 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <gtest/gtest.h>
+#include "test_config.h"
+#include "sdf/sdf.hh"
+
+////////////////////////////////////////////////////
+/// Ensure that include sdf descriptions can be overriden
+TEST(Include, IncludeDescription)
+{
+ const std::string SDF_DESCRITPTION_PATH = std::string(PROJECT_SOURCE_PATH)
+ + "/test/integration/include_description.sdf";
+
+ std::ostringstream stream;
+ std::string version = SDF_VERSION;
+ stream
+ << "<sdf version='" << version << "'>"
+ << " <pose>0 0 0 0 0 0</pose>"
+ << "</sdf>";
+
+ std::string filename = SDF_DESCRITPTION_PATH;
+
+ sdf::SDFPtr sdf(new sdf::SDF());
+ EXPECT_TRUE(sdf::initFile(filename, sdf));
+
+ ASSERT_TRUE(sdf::readString(stream.str(), sdf));
+
+ EXPECT_TRUE(sdf->Root()->HasElement("pose"));
+ sdf::ElementPtr poseElem = sdf->Root()->GetElement("pose");
+ EXPECT_EQ(poseElem->GetDescription(), "override");
+}
+
+/////////////////////////////////////////////////
+/// Main
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/integration/include_description.sdf b/test/integration/include_description.sdf
new file mode 100644
index 0000000..82cc662
--- /dev/null
+++ b/test/integration/include_description.sdf
@@ -0,0 +1,12 @@
+<element name="sdf" required="1">
+ <description>This sdf file contains one pose include element whose description will be overriden.</description>
+
+ <attribute name="version" type="string" default="1.5" required="1">
+ <description>Version number of the SDF format.</description>
+ </attribute>
+
+ <include filename="pose.sdf" required="*">
+ <description>override</description>
+ </include>
+
+</element> <!-- End SDF -->
diff --git a/test/integration/model/box/model.config b/test/integration/model/box/model.config
new file mode 100644
index 0000000..089c029
--- /dev/null
+++ b/test/integration/model/box/model.config
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+ <name>Box</name>
+ <version>1.0</version>
+ <sdf version="1.5">model.sdf</sdf>
+
+ <author>
+ <name>Ian Chen</name>
+ <email>ichen at osrfoundation.org</email>
+ </author>
+
+ <description>
+ A box.
+ </description>
+</model>
diff --git a/test/integration/model/box/model.sdf b/test/integration/model/box/model.sdf
new file mode 100644
index 0000000..32d16f7
--- /dev/null
+++ b/test/integration/model/box/model.sdf
@@ -0,0 +1,22 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <model name="box">
+ <pose>0 0 0.5 0 0 0</pose>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+</sdf>
diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc
new file mode 100644
index 0000000..0cc2c15
--- /dev/null
+++ b/test/integration/urdf_gazebo_extensions.cc
@@ -0,0 +1,100 @@
+/*
+ * Copyright 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <gtest/gtest.h>
+#include <map>
+#include "sdf/sdf.hh"
+
+#include "test_config.h"
+
+const std::string URDF_TEST_FILE = std::string(PROJECT_SOURCE_PATH)
+ + "/test/integration/urdf_gazebo_extensions.urdf";
+
+/////////////////////////////////////////////////
+TEST(SDFParser, UrdfGazeboExtensionURDFTest)
+{
+ sdf::SDFPtr robot(new sdf::SDF());
+ sdf::init(robot);
+ ASSERT_TRUE(sdf::readFile(URDF_TEST_FILE, robot));
+
+ sdf::ElementPtr model = robot->Root()->GetElement("model");
+ for (sdf::ElementPtr joint = model->GetElement("joint"); joint;
+ joint = joint->GetNextElement("joint"))
+ {
+ std::string jointName = joint->Get<std::string>("name");
+ if (jointName == "jointw0")
+ {
+ // No cfm_damping tag was specified
+ EXPECT_FALSE(joint->HasElement("physics"));
+ }
+ else if (jointName == "joint01")
+ {
+ // cfmDamping = true
+ ASSERT_TRUE(joint->HasElement("physics"));
+ sdf::ElementPtr physics = joint->GetElement("physics");
+ ASSERT_TRUE(physics->HasElement("ode"));
+ ASSERT_TRUE(physics->HasElement("provide_feedback"));
+ EXPECT_TRUE(physics->Get<bool>("provide_feedback"));
+
+ ASSERT_TRUE(physics->HasElement("ode"));
+ sdf::ElementPtr ode = physics->GetElement("ode");
+ ASSERT_TRUE(ode->HasElement("provide_feedback"));
+ EXPECT_TRUE(ode->Get<bool>("provide_feedback"));
+ ASSERT_TRUE(ode->HasElement("implicit_spring_damper"));
+ EXPECT_TRUE(!ode->Get<bool>("implicit_spring_damper"));
+ ASSERT_TRUE(ode->HasElement("cfm_damping"));
+ EXPECT_TRUE(!ode->Get<bool>("cfm_damping"));
+ ASSERT_TRUE(ode->HasElement("fudge_factor"));
+ EXPECT_FLOAT_EQ(ode->Get<double>("fudge_factor"), 0.56789);
+
+ ASSERT_TRUE(ode->HasElement("limit"));
+ sdf::ElementPtr limit = ode->GetElement("limit");
+ ASSERT_TRUE(limit->HasElement("cfm"));
+ EXPECT_FLOAT_EQ(limit->Get<double>("cfm"), 123);
+ ASSERT_TRUE(limit->HasElement("erp"));
+ EXPECT_FLOAT_EQ(limit->Get<double>("erp"), 0.987);
+
+ ASSERT_TRUE(joint->HasElement("axis"));
+ sdf::ElementPtr axis = joint->GetElement("axis");
+ ASSERT_TRUE(axis->HasElement("dynamics"));
+ sdf::ElementPtr dynamics = axis->GetElement("dynamics");
+ ASSERT_TRUE(dynamics->HasElement("damping"));
+ EXPECT_FLOAT_EQ(dynamics->Get<double>("damping"), 1.1111);
+ ASSERT_TRUE(dynamics->HasElement("friction"));
+ EXPECT_FLOAT_EQ(dynamics->Get<double>("friction"), 2.2222);
+ ASSERT_TRUE(dynamics->HasElement("spring_reference"));
+ EXPECT_FLOAT_EQ(dynamics->Get<double>("spring_reference"), 0.234);
+ ASSERT_TRUE(dynamics->HasElement("spring_stiffness"));
+ EXPECT_FLOAT_EQ(dynamics->Get<double>("spring_stiffness"), 0.567);
+ }
+ else if (jointName == "joint12")
+ {
+ // cfmDamping not provided
+ ASSERT_TRUE(joint->HasElement("physics"));
+ sdf::ElementPtr physics = joint->GetElement("physics");
+ EXPECT_FALSE(physics->HasElement("implicit_spring_damper"));
+ }
+ else if (jointName == "joint13")
+ {
+ // implicitSpringDamper = 1
+ ASSERT_TRUE(joint->HasElement("physics"));
+ sdf::ElementPtr physics = joint->GetElement("physics");
+ ASSERT_TRUE(physics->HasElement("implicit_spring_damper"));
+ EXPECT_TRUE(physics->Get<bool>("implicit_spring_damper"));
+ }
+ }
+}
diff --git a/test/integration/urdf_gazebo_extensions.urdf b/test/integration/urdf_gazebo_extensions.urdf
new file mode 100644
index 0000000..4703907
--- /dev/null
+++ b/test/integration/urdf_gazebo_extensions.urdf
@@ -0,0 +1,140 @@
+<?xml version="1.0" ?>
+<robot name="urdf_gazebo_extension_test">
+
+ <link name="world"/>
+
+ <joint name="jointw0" type="continuous">
+ <origin rpy="0 0 1.57079632679" xyz="0 0 1.0"/>
+ <axis xyz="1 0 0"/> <!-- in child (link1) frame -->
+ <parent link="world"/>
+ <child link="link0"/>
+ <dynamics damping="0.0" friction="0.0" />
+ </joint>
+
+ <link name="link0">
+ <inertial>
+ <mass value="1000"/>
+ <origin rpy="0 0 0" xyz="0 0 -0.5"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <box size="0.1 0.1 1.0"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 -0.5"/>
+ </visual>
+ </link>
+
+ <joint name="joint01" type="continuous">
+ <origin rpy="0 0 1.57079632679" xyz="0 0 -1"/>
+ <axis xyz="1 0 0"/> <!-- in child (link1) frame -->
+ <parent link="link0"/>
+ <child link="link1"/>
+ <dynamics damping="1.1111" friction="2.2222" />
+ </joint>
+ <gazebo reference="joint01">
+ <cfmDamping>true</cfmDamping>
+ <stopCfm>123</stopCfm>
+ <stopErp>0.987</stopErp>
+ <springReference>0.234</springReference>
+ <springStiffness>0.567</springStiffness>
+ <provideFeedback>1</provideFeedback>
+ <implicitSpringDamper>false</implicitSpringDamper>
+ <fudgeFactor>0.56789</fudgeFactor>
+ </gazebo>
+
+ <link name="link1">
+ <inertial>
+ <mass value="100"/>
+ <origin rpy="1 3 4" xyz="0 -1.5 0"/>
+ <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <box size="0.2 3.0 0.2"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 -1.5 0"/>
+ </visual>
+ </link>
+
+ <joint name="joint12" type="revolute">
+ <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 -3.0 0.0"/>
+ <axis xyz="0 1 0"/> <!-- in child (link1) frame -->
+ <limit lower="0" upper="0" velocity="1000" effort="10000"/>
+ <parent link="link1"/>
+ <child link="link2"/>
+ </joint>
+ <gazebo reference="joint12">
+ <implicitSpringDamper>0</implicitSpringDamper>
+ </gazebo>
+
+ <link name="link2">
+ <inertial>
+ <mass value="200"/>
+ <origin rpy="2 -3 -1.57079632679" xyz="0.2 0.4 1.0"/>
+ <inertia ixx="5" ixy="0" ixz="0" iyy="6" iyz="0" izz="7"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <box size="0.3 0.3 2.0"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 1.0"/>
+ </visual>
+ </link>
+
+ <joint name="joint13" type="revolute">
+ <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 -3.0 0.0"/>
+ <axis xyz="0 1 0"/> <!-- in child (link1) frame -->
+ <limit lower="0" upper="0" velocity="1000" effort="10000"/>
+ <parent link="link1"/>
+ <child link="link3"/>
+ </joint>
+
+ <joint name="joint23" type="revolute">
+ <origin rpy="-1.57079632679 0 -1.57079632679" xyz="-0.5 0.5 2.5"/>
+ <axis xyz="0 1 0"/> <!-- in child (link1) frame -->
+ <limit lower="0" upper="0" velocity="1000" effort="10000"/>
+ <parent link="link2"/>
+ <child link="link3"/>
+ </joint>
+
+ <link name="link3">
+ <inertial>
+ <mass value="400"/>
+ <origin rpy="2 3 4" xyz="0.1 0.2 0.3"/>
+ <inertia ixx="8" ixy="0" ixz="0" iyy="9" iyz="0" izz="10"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <box size="0.3 0.4 0.5"/>
+ </geometry>
+ <origin rpy="2 3 4" xyz="0.1 0.2 0.3"/>
+ </visual>
+ </link>
+
+ <gazebo reference="joint13">
+ <implicitSpringDamper>true</implicitSpringDamper>
+ </gazebo>
+
+ <joint name="joint01a" type="continuous">
+ <origin rpy="0 0 1.57079632679" xyz="0 0 -1"/>
+ <axis xyz="1 0 0"/> <!-- in child (link1) frame -->
+ <parent link="link0"/>
+ <child link="link1a"/>
+ </joint>
+
+ <link name="link1a">
+ <inertial>
+ <mass value="100"/>
+ <origin rpy="1 3 4" xyz="0 -1.5 0"/>
+ <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <box size="0.2 3.0 0.2"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 -1.5 0"/>
+ </visual>
+ </link>
+
+</robot>
diff --git a/tools/code_check.sh b/tools/code_check.sh
index 5c9b1be..3c3929a 100755
--- a/tools/code_check.sh
+++ b/tools/code_check.sh
@@ -21,9 +21,9 @@ echo "*:src/Assert.cc" >> $SUPPRESS
echo "*:src/Console.cc" >> $SUPPRESS
echo "*:src/parser.cc" >> $SUPPRESS
echo "*:src/parser_urdf.cc" >> $SUPPRESS
-echo "*:src/Element.cc:463" >> $SUPPRESS
+echo "*:src/Element.cc:464" >> $SUPPRESS
echo "*:src/Element.cc:39" >> $SUPPRESS
-echo "*:src/Element.cc:751" >> $SUPPRESS
+echo "*:src/Element.cc:752" >> $SUPPRESS
CHECK_FILE_DIRS="./src ./include ./test/performance ./test/integration"
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/sdformat.git
More information about the debian-science-commits
mailing list