Skip to content

Commit

Permalink
Initial urdf parser plugin that parses sdformat (#1)
Browse files Browse the repository at this point in the history
* Initial urdf parser plugin that parses sdformat

Lots of TODOs

Signed-off-by: Shane Loretz<sloretz@openrobotics.org>
Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Visual and Collision with Box geometry

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Sphere geometry

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Cylinder geometry

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Mesh support

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* remove TODO comment about resolving poses

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Let downstream worry about resolving mesh URIs

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Reword assumption about kinematic root link

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Make fall through comments easier to see

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Create urdf_mesh variable

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Handle inertia

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Mesh scale

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Visibility control header

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Remove resource_retriever dependency

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Support joint axis expressed in other frames

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Include ignition/math/Pose3.hh

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Use final on plugin class

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Use TinyXML2 to filter non-sdf XML documents

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Apache 2.0 License

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Satisfy uncrustify

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Satisfy cpplint

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add linter tests

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add test for pose_link case

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Actually add test files

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add  SDF link pose to URDF link members

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Pose link test actually checks pose

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add SDF visual pose test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add SDF collision pose test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add SDF inertial pose test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add test for model with pose, plus refactor test tools

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Replace todo with explanation

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add pose_link_all test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Assert model name is correct

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add pose_link_in_frame test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add pose_collision_in_frame test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add pose_visual_in_frame test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add skipped pose_inertial_in_frame test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Depend on sdformat

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Fix link poses of child links

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add test for model with joint pose

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Remove unused variable

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add test for model with joint pose in frame

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Rename variables for consistent ordering

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Fix fame names in calculating joint pose

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* URDF link frames are coincident with joint frames

And the root link pose has no effect on the URDF output except that it
changes the relative locations of other links with the root link.
The SDF link pose cannot be expressed in URDF C++ structures without
adding dummy links, but this does not do that.

Signed-off-by: Shane Loretz<sloretz@openrobotics.org>
Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add pose_chain test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Clearer description of pose_collision expectations

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Clearer description of pose_collision_in_frame expectations

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Clearer pose_inertial* expectations

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Remove unnecessary ASSERT

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Initializer lists

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Remove unnecessary ASSERT

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Initializer lists

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Fix pose_link* test expectations

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Clearer pose_visual* test expecations

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Shorter file name

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Shorter file name

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add graph_loop test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add graph_four_bar test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Alphabetize

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Refactor tree traversal code

Fixes bug were tree's with non-canonical roots were not caught during
conversion
Shrink list of joints to check after checking them
Use visited_links only to catch multiple roots and algorithm errors
Get rid of trailing_

Signed-off-by: Shane Loretz<sloretz@openrobotics.org>
Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add graph_tree_non_canonical_root test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Transfrom -> transform

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Different error when XML file has no model

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* urdf child link keeps weak reference to parent link

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add graph_tree test and EXPECT_NAMES util

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add graph_chain test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add graph_chain_non_canonical_root test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add geometry_plane test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add geometry_box test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add geometry_cylinder test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add geometry_sphere test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add geometry_heightmap test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add geometry_collada test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add geometry_obj test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add geometry_stl test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add geometry_mesh_scaled test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add tests for zero and two models

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add joint_ball test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Get rid of num_axes variable

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add damping and friction info to joints

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add joint_continuous test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add joint_fixed test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add joint limits info

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add joint_prismatic test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add axis expectations

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add joint_revolute test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add joint_revolute2 test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add joint_screw test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add joint_universal test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add link_inertia test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add joint_revolute_axis test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Warn on non-default values of unsupported <joint> tags

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add link_multiple_collisions test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add link_multiple_visuals test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add joint_revolute_axis_in_frame test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add joint_revolute_default_limits test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Detect kinematic loops involving redundant joints

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add joint_revolute_two_joints_two_links test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Warn if <sensor> or <light> are used on <link>

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add link_sensor_imu test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add link_light_point test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Sensors -> lights

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Move package into folder in repo

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add sdformat_test_files package to repo

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* lilnk -> link

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Whitespace

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Save a line

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Move PlaneShape check lower

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Remove impossible condition after make_shared

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Make names shorter

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Remove accidentally comitted file

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add material_blinn_phong test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Warn if unsupported <material><lighting> feature is used

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add skipped material_dynamic_lights test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Remove unfinished material models and add TODO for them

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Expect no Algorithm errors

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add joint_gearbox model and test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Update descriptions of all models

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add sdformat_test_files README

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add README for sdformat_urdf

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Misc tiny edits

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Eliminate elements

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add repo level readme

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Remove .

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Trick to use sdformat9 from source

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Remove condition hack

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add colcon.pkg for building sdformat from source

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* check errors instead of valid pointer

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Make urdf a <depend>

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Remove std::cout dbg print

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* the the -> the

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Compare WorldCount against 0u

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Concatenate link and visual names to make material name

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Compatibility with CMake earlier than 3.14

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Use target_link_libraries()

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* One install rule per target

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* voilated -> violated

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Use axis2 for second axis

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Universal joint perpendicular axis

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Use Pose3d::operator*

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* structures -> DOM objects

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Document EXPECT_NAMES()

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* Compare normalized joint axes

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* Check casted pointer before using them

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* Check geometry pointer isn't null before using it

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* Assert joint isn't null before using it

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* Assert link isn't null before using it

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* Assert link isn't null before using it

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* Color = 0.4 * ambient + 0.8 * diffuse

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* Fix application of link pose on inertial/visual/collision

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* Add pose_joint_all test

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>
  • Loading branch information
sloretz authored Nov 2, 2020
1 parent 0b47f5f commit efc3913
Show file tree
Hide file tree
Showing 137 changed files with 9,508 additions and 11 deletions.
11 changes: 0 additions & 11 deletions LICENSE

This file was deleted.

13 changes: 13 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
# SDFormat XML Robot Descriptions

This repo enables using SDFormat XML as a robot description format instead of URDF XML.
It does this by providing a `urdf_parser_plugin` for SDFormat that reads SDFormat and outputs URDF C++ DOM structures.
To use it, install `sdformat_urdf` and use a valid SDFormat XML file (with some limitations) for your robot description.
See the [README in the `sdformat_urdf` package](./sdformat_urdf/README.md) for more info on the limitations.

## Packages

* [`sdformat_urdf`](./sdformat_urdf/README.md)
* provides a library and a `urdf_parser_plugin` using that library to convert SDFormat XML to URDF C++ DOM structures
* [`sdformat_test_files`](./sdformat_test_files/README.md)
* provides SDFormat models using different parts of the SDFormat XML specification for testing
103 changes: 103 additions & 0 deletions sdformat_test_files/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,103 @@
cmake_minimum_required(VERSION 3.5)
project(sdformat_test_files)

set(sdformat_test_files_VERSION 0.1.0)

# Decide where to install stuff
include(GNUInstallDirs)

set(INSTALL_SHARE_DIR "${CMAKE_INSTALL_DATAROOTDIR}/sdformat_test_files" CACHE PATH
"Installation directory for arch independent data files")

set(INSTALL_CMAKE_DIR "${CMAKE_INSTALL_DATAROOTDIR}/sdformat_test_files/cmake" CACHE PATH
"Installation directory for CMake files")

# Install models
set(model_names
"geometry_box"
"geometry_cylinder"
"geometry_heightmap"
"geometry_mesh_collada"
"geometry_mesh_obj"
"geometry_mesh_scaled"
"geometry_mesh_stl"
"geometry_plane"
"geometry_sphere"
"graph_chain"
"graph_chain_non_canonical_root"
"graph_four_bar"
"graph_loop"
"graph_tree"
"graph_tree_non_canonical_root"
"joint_ball"
"joint_continuous"
"joint_fixed"
"joint_gearbox"
"joint_prismatic"
"joint_revolute"
"joint_revolute2"
"joint_revolute_axis"
"joint_revolute_axis_in_frame"
"joint_revolute_default_limits"
"joint_revolute_two_joints_two_links"
"joint_screw"
"joint_universal"
"link_inertia"
"link_light_point"
"link_multiple_collisions"
"link_multiple_visuals"
"link_sensor_imu"
"material_blinn_phong"
"material_dynamic_lights"
"model_two_models"
"model_zero_models"
"pose_chain"
"pose_collision"
"pose_collision_in_frame"
"pose_inertial"
"pose_inertial_in_frame"
"pose_joint"
"pose_joint_all"
"pose_joint_in_frame"
"pose_link"
"pose_link_all"
"pose_link_in_frame"
"pose_model"
"pose_visual"
"pose_visual_in_frame"
)

foreach(model ${model_names})
# Install models to share/project-name/model-name
install(DIRECTORY "models/${model}" DESTINATION "${INSTALL_SHARE_DIR}/models")
endforeach()

set(_arch_independent "")
if(${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.14.0")
set(_arch_independent "ARCH_INDEPENDENT")
endif()

# Create projectConfigVersion.cmake
include(CMakePackageConfigHelpers)
write_basic_package_version_file(
"${CMAKE_CURRENT_BINARY_DIR}/sdformat_test_files/sdformat_test_filesConfigVersion.cmake"
VERSION "${sdformat_test_files_VERSION}"
COMPATIBILITY SameMajorVersion
${_arch_independent})

# Create projectConfig.cmake
configure_package_config_file("sdformat_test_filesConfig.cmake.in"
"${CMAKE_CURRENT_BINARY_DIR}/sdformat_test_files/sdformat_test_filesConfig.cmake"
INSTALL_DESTINATION "${INSTALL_CMAKE_DIR}"
PATH_VARS "INSTALL_CMAKE_DIR" "INSTALL_SHARE_DIR")

# Create file with CMake functions
configure_file("sdformat_test_files_functions.cmake.in"
"${CMAKE_CURRENT_BINARY_DIR}/sdformat_test_files/sdformat_test_files_functions.cmake" @ONLY)

# Install cmake files
install(FILES
"${CMAKE_CURRENT_BINARY_DIR}/sdformat_test_files/sdformat_test_filesConfig.cmake"
"${CMAKE_CURRENT_BINARY_DIR}/sdformat_test_files/sdformat_test_filesConfigVersion.cmake"
"${CMAKE_CURRENT_BINARY_DIR}/sdformat_test_files/sdformat_test_files_functions.cmake"
DESTINATION "${INSTALL_CMAKE_DIR}")
129 changes: 129 additions & 0 deletions sdformat_test_files/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,129 @@
# SDFormat Test files

This package contains a list of SDFormat files for testing tools that work with SDFormat XML.

## Models

### Geometry

* `geometry_box`
* A single-link model using the box geometry type for both the visual and collision.
* `geometry_cylinder`
* A single-link model using the cylinder geometry type for both the visual and collision.
* `geometry_heightmap`
* A single-link model using heightmap geometry for both the visual and collision.
* `geometry_mesh_collada`
* A single link using mesh geometry with a COLLADA mesh.
* `geometry_mesh_obj`
* A single link using a Wavefront OBJ mesh.
* `geometry_mesh_scaled`
* A single-link model using a mesh scaled differently on x, y, and z axes for both the visual and collision.
* `geometry_mesh_stl`
* A single-link model using an STL mesh.
* `geometry_plane`
* A single-link model using the plane geometry type for both the visual and collision.
* `geometry_sphere`
* A single-link model using the sphere geometry type for both the visual and collision.

### Materials

* `material_blinn_phong`
* A single link with a material that uses ambient/diffuse/specular/emissive components to color it.
* `material_dynamic_lights`
* A link with two visuals: one with dynamic lights on, and the other with dynamic lights off.

### Joints

* `joint_ball`
* A model with two links connected by a ball joint.
* `joint_continuous`
* A model with two links connected by a continuous joint.
* `joint_fixed`
* A model with two links connected by a fixed joint.
* `joint_gearbox`
* A model with 3 links total. Two links are connected to a reference link with revolute joints and a gearbox joint enforces that one revolute joint rotates faster than the other.
* `joint_prismatic`
* A model with two links connected by a prismatic joint.
* `joint_revolute`
* A model with two links connected by a revolute joint.
* `joint_revolute2`
* A model with two links connected by a revolute2 joint.
* `joint_revolute_axis`
* A model with two links connected by a revolute joint with an axis having different values for x, y, and z.
* `joint_revolute_axis_in_frame`
* A model with two links connected by a revolute joint with an axis having different values for x, y, and z, and specified in a frame on the model.
* `joint_revolute_default_limits`
* A model with two links connected by a revolute joint, having no joint limits specified on its axis.
* `joint_revolute_two_joints_two_links`
* A model with two links connected by two revolute joints, effectively rigidly connecting the two.
* `joint_screw`
* A model with two links connected by a screw joint.
* `joint_universal`
* A model with two links connected by a universal joint.

### Links

* `link_inertia`
* A link having an inerta with a different value for each of it's 6 components.
* `link_light_point`
* A model with a single link having a point light attached to it.
* `link_multiple_collisions`
* A model with a single link having multiple collision elements on it.
* `link_multiple_visuals`
* A model with a single link having multile visual elements on it.
* `link_sensor_imu`
* A model with a single link having an IMU sensor attached to it.

### Kinematic structures

* `graph_chain`
* A model having a chain of 3 links connected in series with revolute joints.
* `graph_chain_non_canonical_root`
* A model having a chain of 3 links connected in series with revolute joints, but the canonical link is not the root of the chain.
* `graph_four_bar`
* A four-bar linkage made with four links connected by 4 revolute joints.
* `graph_loop`
* A model having three links connected by 3 joints to form a triangle.
* `graph_tree`
* A model with multiple links connected by joints forming a tree.
* `graph_tree_non_canonical_root`
* A model with multiple links connected by joints forming a tree, but the canonical link is not the root of the tree.

### Poses and Frames

* `pose_chain`
* A chain of links connected by joints, where every link and joint in the chain has a non-zero pose.
* `pose_collision`
* A single-link model where only the collision has a non-zero pose.
* `pose_collision_in_frame`
* A single-link model where only the collision has a non-zero pose, and that pose is given in a frame on the model.
* `pose_inertial`
* A single-link model where only the inertial has a non-zero pose.
* `pose_inertial_in_frame`
* A single-link model where only the inertial has a non-zero pose, and that pose is given in a frame on the model.
* `pose_joint`
* A model having two links and a revolute joint, where only the joint has a non-zero pose.
* `pose_joint_all`
* A model having two links and a revolute joint, with non-zero poses on all including the inertials, visuals, and collisions.
* `pose_joint_in_frame`
* A model having two links and a revolute joint, where only the joint has a non-zero pose, and that pose is given in a frame on the model.
* `pose_link`
* A single-link model where only the link has a non-zero pose.
* `pose_link_all`
* A single-link model where the link, visual, collision, and inertial elements all have poses.
* `pose_link_in_frame`
* A single-link model where only the link has a non-zero pose, and that pose is given in a frame on the model.
* `pose_model`
* A single-link model where the model itself has a non-zero pose.
* `pose_visual`
* A single-link model where only the visual has a non-zero pose.
* `pose_visual_in_frame`
* A single-link model where only the visual has a non-zero pose, and that pose is given in a frame on the model.

### Models

* `model_two_models`
* An SDFormat XML file having two models in it.
* `model_zero_models`
* An SDFormat XML file that does not have a model in it.

33 changes: 33 additions & 0 deletions sdformat_test_files/_scripts/README.md.in
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
# SDFormat Test files

This package contains a list of SDFormat files for testing tools that work with SDFormat XML.

## Models

### Geometry

$geometry

### Materials

$materials

### Joints

$joints

### Links

$links

### Kinematic structures

$graphs

### Poses and Frames

$poses

### Models

$models
60 changes: 60 additions & 0 deletions sdformat_test_files/_scripts/make_readme.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,60 @@
import string
import os
import xml.etree.ElementTree as ET


def get_model_name_and_description(path_to_model_dir):
tree = ET.parse(os.path.join(path_to_model_dir, 'model.config'))
root = tree.getroot()
name = root.findall('name')[0].text.strip()
description = root.findall('description')[0].text.strip()
return (name, description)


def models_in_dir(path_to_dir):
for path in os.listdir(path_to_dir):
abspath = os.path.abspath(os.path.join(path_to_dir, path))
if os.path.isdir(abspath) and os.path.isfile(os.path.join(abspath, 'model.config')):
yield abspath


if __name__ == '__main__':
this_dir = os.path.abspath(os.path.dirname(__file__))
models_dir = os.path.join(this_dir, '..', 'models')

template_mapping = {
'geometry': [],
'materials': [],
'joints': [],
'links': [],
'graphs': [],
'poses': [],
'models': []
}

for model_path in sorted(models_in_dir(models_dir)):
name, desc = get_model_name_and_description(model_path)
entry = f'* `{name}`\n * {desc}'
if name.startswith('geometry'):
template_mapping['geometry'].append(entry)
elif name.startswith('material'):
template_mapping['materials'].append(entry)
elif name.startswith('joint'):
template_mapping['joints'].append(entry)
elif name.startswith('link'):
template_mapping['links'].append(entry)
elif name.startswith('graph'):
template_mapping['graphs'].append(entry)
elif name.startswith('pose'):
template_mapping['poses'].append(entry)
elif name.startswith('model'):
template_mapping['models'].append(entry)
else:
raise RuntimeError(f'Unknown model type {name} at {model_path}')

for key in template_mapping.keys():
template_mapping[key] = '\n'.join(template_mapping[key])

with open(os.path.join(this_dir, 'README.md.in'), 'r') as fin:
template = string.Template(fin.read())
print(template.substitute(template_mapping))
32 changes: 32 additions & 0 deletions sdformat_test_files/models/geometry_box/geometry_box.sdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
<?xml version='1.0' encoding='utf-8'?>
<sdf version="1.7">
<model name="geometry_box">
<link name="link">
<visual name="link_visual">
<geometry>
<box>
<size>0.1 0.2 0.4</size>
</box>
</geometry>
</visual>
<collision name="link_collision">
<geometry>
<box>
<size>0.1 0.2 0.4</size>
</box>
</geometry>
</collision>
<inertial>
<mass>1.23</mass>
<inertia>
<ixx>0.0205</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.017425</iyy>
<iyz>0</iyz>
<izz>0.005125</izz>
</inertia>
</inertial>
</link>
</model>
</sdf>
Loading

0 comments on commit efc3913

Please sign in to comment.