Initial urdf parser plugin that parses sdformat

README.md

@@ -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

sdformat_test_files/CMakeLists.txt

@@ -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}")

sdformat_test_files/README.md

@@ -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.
+

sdformat_test_files/_scripts/README.md.in

@@ -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

sdformat_test_files/_scripts/make_readme.py

@@ -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))

sdformat_test_files/models/geometry_box/geometry_box.sdf

@@ -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>