diff --git a/.github/ci/before_cmake.sh b/.github/ci/before_cmake.sh index 123af216d..8172bf198 100644 --- a/.github/ci/before_cmake.sh +++ b/.github/ci/before_cmake.sh @@ -7,12 +7,17 @@ BUILD_DIR=`pwd` cd /tmp # check that we can compile USD from sources (only Focal) +# see https://github.com/ignitionrobotics/sdformat/issues/869 +return_code=0 +if [ "$(lsb_release -r -s)" != "20.04" ]; then + return_code=$(($return_code + 1)) +fi + mkdir cmake_test cd cmake_test echo "cmake_minimum_required(VERSION 3.12)" > CMakeLists.txt -return_code=0 cmake . || return_code=$(($return_code + $?)) if [ $return_code -eq 0 ] then diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index 4fd618661..8430b7ca9 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -1,11 +1,15 @@ -libignition-cmake2-dev +libignition-cmake3-dev libignition-common5-dev libignition-math7-dev -libignition-tools-dev -libignition-utils1-dev -libignition-utils1-cli-dev +libignition-tools2-dev +libignition-utils2-dev +libignition-utils2-cli-dev libtinyxml2-dev liburdfdom-dev libxml2-utils +python3-dev +python3-distutils +python3-ignition-math7 python3-psutil +python3-pybind11 ruby-dev diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 26bd9f069..164a62cd8 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -21,3 +21,12 @@ jobs: codecov-enabled: true cppcheck-enabled: true cpplint-enabled: true + jammy-ci: + runs-on: ubuntu-latest + name: Ubuntu Jammy CI + steps: + - name: Checkout + uses: actions/checkout@v2 + - name: Compile and test + id: ci + uses: ignition-tooling/action-ignition-ci@jammy diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml index 7df08f33a..a8dac0422 100644 --- a/.github/workflows/macos.yml +++ b/.github/workflows/macos.yml @@ -39,7 +39,9 @@ jobs: - run: mkdir build - name: cmake working-directory: build - run: cmake .. -DCMAKE_INSTALL_PREFIX=/usr/local/Cellar/${PACKAGE}/HEAD + run: | + export PYTHONPATH=$PYTHONPATH:/usr/local/lib/python + cmake .. -DCMAKE_INSTALL_PREFIX=/usr/local/Cellar/${PACKAGE}/HEAD - run: make working-directory: build - run: make test diff --git a/CMakeLists.txt b/CMakeLists.txt index 51e204042..1404d4cce 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) +cmake_minimum_required(VERSION 3.12 FATAL_ERROR) if(COMMAND CMAKE_POLICY) CMAKE_POLICY(SET CMP0003 NEW) @@ -18,8 +18,18 @@ set (BUILD_SDF ON CACHE INTERNAL "Build SDF" FORCE) ################################################# # Find ign-cmake -find_package(ignition-cmake2 2.10 REQUIRED) -set(IGN_CMAKE_VER ${ignition-cmake2_VERSION_MAJOR}) +find_package(ignition-cmake3 REQUIRED) +set(IGN_CMAKE_VER ${ignition-cmake3_VERSION_MAJOR}) + +######################################## +# Python interfaces vars +option(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION + "Install python modules in standard system paths in the system" + OFF) + +option(USE_DIST_PACKAGES_FOR_PYTHON + "Use dist-packages instead of site-package to install python modules" + OFF) if (BUILD_SDF) ign_configure_project( @@ -70,7 +80,7 @@ if (BUILD_SDF) # A module's location is usually a directory, but for # binary modules # it's a .so file. - execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c" "import re, ${module}; print(re.compile('/__init__.py.*').sub('',${module}.__file__))" + execute_process(COMMAND "${Python3_EXECUTABLE}" "-c" "import re, ${module}; print(re.compile('/__init__.py.*').sub('',${module}.__file__))" RESULT_VARIABLE _${module}_status OUTPUT_VARIABLE _${module}_location ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE) @@ -106,14 +116,34 @@ if (BUILD_SDF) ######################################## # Find ignition utils - ign_find_package(ignition-utils1 VERSION REQUIRED) - set(IGN_UTILS_VER ${ignition-utils1_VERSION_MAJOR}) + ign_find_package(ignition-utils2 REQUIRED COMPONENTS cli) + set(IGN_UTILS_VER ${ignition-utils2_VERSION_MAJOR}) ######################################## # Find ignition common ign_find_package(ignition-common5 COMPONENTS graphics REQUIRED_BY usd) set(IGN_COMMON_VER ${ignition-common5_VERSION_MAJOR}) + ######################################## + # Python interfaces + if (NOT PYTHON3_FOUND) + IGN_BUILD_WARNING("Python is missing: Python interfaces are disabled.") + message (STATUS "Searching for Python - not found.") + else() + message (STATUS "Searching for Python - found version ${Python3_VERSION}.") + + set(PYBIND11_PYTHON_VERSION 3) + find_package(pybind11 2.4 QUIET) + + if (${pybind11_FOUND}) + find_package(Python3 ${IGN_PYTHON_VERSION} REQUIRED COMPONENTS Development) + message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") + else() + IGN_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.") + message (STATUS "Searching for pybind11 - not found.") + endif() + endif() + ######################################## # Find PXR ign_find_package(pxr QUIET REQUIRED_BY usd PKGCONFIG pxr) @@ -126,6 +156,9 @@ if (BUILD_SDF) add_subdirectory(sdf) add_subdirectory(conf) add_subdirectory(doc) + if (${pybind11_FOUND}) + add_subdirectory(python) + endif() endif(BUILD_SDF) ######################################## diff --git a/Changelog.md b/Changelog.md index 5d204f626..b34dfe0c1 100644 --- a/Changelog.md +++ b/Changelog.md @@ -5,6 +5,156 @@ ## libsdformat 12.X +### libsdformat 12.X.X (2022-03-xx) + +### libsdformat 12.4.0 (2022-03-29) + +1. Use ParserConfig more in parser.cc + * [Pull request #883](https://github.com/ignitionrobotics/sdformat/pull/883) + * [Pull request #885](https://github.com/ignitionrobotics/sdformat/pull/885) + * [Pull request #910](https://github.com/ignitionrobotics/sdformat/pull/910) + +1. Added option to visualize light in GUI + * [Pull request #877](https://github.com/ignitionrobotics/sdformat/pull/877) + +1. Make `computeMergedModelProxyFrameName` public + * [Pull request #868](https://github.com/ignitionrobotics/sdformat/pull/868) + +1. SDFormat to USD conversion + * [Pull request #818](https://github.com/ignitionrobotics/sdformat/pull/818) + * [Pull request #827](https://github.com/ignitionrobotics/sdformat/pull/827) + * [Pull request #828](https://github.com/ignitionrobotics/sdformat/pull/828) + * [Pull request #829](https://github.com/ignitionrobotics/sdformat/pull/829) + * [Pull request #830](https://github.com/ignitionrobotics/sdformat/pull/830) + * [Pull request #831](https://github.com/ignitionrobotics/sdformat/pull/831) + * [Pull request #837](https://github.com/ignitionrobotics/sdformat/pull/837) + * [Pull request #862](https://github.com/ignitionrobotics/sdformat/pull/862) + * [Pull request #863](https://github.com/ignitionrobotics/sdformat/pull/863) + * [Pull request #870](https://github.com/ignitionrobotics/sdformat/pull/870) + * [Pull request #888](https://github.com/ignitionrobotics/sdformat/pull/888) + * [Pull request #889](https://github.com/ignitionrobotics/sdformat/pull/889) + * [Pull request #895](https://github.com/ignitionrobotics/sdformat/pull/895) + * [Pull request #896](https://github.com/ignitionrobotics/sdformat/pull/896) + * [Pull request #901](https://github.com/ignitionrobotics/sdformat/pull/901) + * [Pull request #906](https://github.com/ignitionrobotics/sdformat/pull/906) + * [Pull request #908](https://github.com/ignitionrobotics/sdformat/pull/908) + * [Pull request #913](https://github.com/ignitionrobotics/sdformat/pull/913) + * [Pull request #915](https://github.com/ignitionrobotics/sdformat/pull/915) + +1. Add ToElement conversions for various classes + * [Pull request #771](https://github.com/ignitionrobotics/sdformat/pull/771) + * [Pull request #772](https://github.com/ignitionrobotics/sdformat/pull/772) + * [Pull request #775](https://github.com/ignitionrobotics/sdformat/pull/775) + * [Pull request #776](https://github.com/ignitionrobotics/sdformat/pull/776) + * [Pull request #777](https://github.com/ignitionrobotics/sdformat/pull/777) + * [Pull request #781](https://github.com/ignitionrobotics/sdformat/pull/781) + * [Pull request #782](https://github.com/ignitionrobotics/sdformat/pull/782) + * [Pull request #783](https://github.com/ignitionrobotics/sdformat/pull/783) + * [Pull request #842](https://github.com/ignitionrobotics/sdformat/pull/842) + * [Pull request #887](https://github.com/ignitionrobotics/sdformat/pull/887) + * [Pull request #918](https://github.com/ignitionrobotics/sdformat/pull/918) + +1. Fix compiler warnings + * [Pull request #808](https://github.com/ignitionrobotics/sdformat/pull/808) + * [Pull request #810](https://github.com/ignitionrobotics/sdformat/pull/810) + +1. Infrastructure and Documentation + * [Pull request #861](https://github.com/ignitionrobotics/sdformat/pull/861) + * [Pull request #800](https://github.com/ignitionrobotics/sdformat/pull/800) + * [Pull request #686](https://github.com/ignitionrobotics/sdformat/pull/686) + * [Pull request #713](https://github.com/ignitionrobotics/sdformat/pull/713) + * [Pull request #864](https://github.com/ignitionrobotics/sdformat/pull/864) + * [Pull request #880](https://github.com/ignitionrobotics/sdformat/pull/880) + * [Pull request #890](https://github.com/ignitionrobotics/sdformat/pull/890) + * [Pull request #891](https://github.com/ignitionrobotics/sdformat/pull/891) + +1. Use the Plugin DOM in other DOM objects + * [Pull request #858](https://github.com/ignitionrobotics/sdformat/pull/858) + +1. Add SDFormat tags for Triggered Cameras + * [Pull request #846](https://github.com/ignitionrobotics/sdformat/pull/846) + +1. Fix bug where //include/pose was ignored when using the Interface API + * [Pull request #853](https://github.com/ignitionrobotics/sdformat/pull/853) + +1. Fix joint parent/child frame existence checks to include interface elements + * [Pull request #855](https://github.com/ignitionrobotics/sdformat/pull/855) + +1. Remove USD visibility macro from internal APIs + * [Pull request #857](https://github.com/ignitionrobotics/sdformat/pull/857) + +1. Added non-const mutable accessors for world child objects + * [Pull request #840](https://github.com/ignitionrobotics/sdformat/pull/840) + +1. Added non-const accessors for Model child objects + * [Pull request #839](https://github.com/ignitionrobotics/sdformat/pull/839) + +1. Added to light if the light is on or off + * [Pull request #851](https://github.com/ignitionrobotics/sdformat/pull/851) + +1. Added Root mutable accessors, and Root::Clone function + * [Pull request #841](https://github.com/ignitionrobotics/sdformat/pull/841) + +1. Hide USDUtils.hh file from public API + * [Pull request #850](https://github.com/ignitionrobotics/sdformat/pull/850) + +1. Added non-const accessors for Link child objects + * [Pull request #838](https://github.com/ignitionrobotics/sdformat/pull/838) + +1. Add USDError class + * [Pull request #836](https://github.com/ignitionrobotics/sdformat/pull/836) + +1. Use USD component visibility macro + * [Pull request #849](https://github.com/ignitionrobotics/sdformat/pull/849) + +1. Add support for merge-include in the Interface API + * [Pull request #768](https://github.com/ignitionrobotics/sdformat/pull/768) + +1. Handle `__model__` in joint parent or child when using merge-include + * [Pull request #835](https://github.com/ignitionrobotics/sdformat/pull/835) + +1. Allow model frames (__model__) to be used as joint parent or child + * [Pull request #833](https://github.com/ignitionrobotics/sdformat/pull/833) + +1. Fix bug where a sdf::ParserConfig object was not passed to all sdf::readFile calls + * [Pull request #824](https://github.com/ignitionrobotics/sdformat/pull/824) + +1. Make SDF to USD a separate component of sdformat + * [Pull request #817](https://github.com/ignitionrobotics/sdformat/pull/817) + +1. Add ParserConfig flag for preserveFixedJoint + * [Pull request #815](https://github.com/ignitionrobotics/sdformat/pull/815) + +1. Fix parsing 'type' attibutes in plugins + * [Pull request #809](https://github.com/ignitionrobotics/sdformat/pull/809) + +1. sdf_custom: fix nested model expectations + * [Pull request #807](https://github.com/ignitionrobotics/sdformat/pull/807) + +1. Replace custom cmake code with ign-cmake2 + * [Pull request #780](https://github.com/ignitionrobotics/sdformat/pull/780) + +1. Support printing sdf poses in degrees and allow snapping to commonly used angles + * [Pull request #689](https://github.com/ignitionrobotics/sdformat/pull/689) + +1. Refactor FrameSemantics.cc + * [Pull request #764](https://github.com/ignitionrobotics/sdformat/pull/764) + +1. Fix loading nested include with custom attributes + * [Pull request #789](https://github.com/ignitionrobotics/sdformat/pull/789) + +1. Added plugin to SDF DOM + * [Pull request #788](https://github.com/ignitionrobotics/sdformat/pull/788) + +1. Support URI in the Model DOM + * [Pull request #786](https://github.com/ignitionrobotics/sdformat/pull/786) + +1. Support adding and clearing sensors from a joint + * [Pull request #785](https://github.com/ignitionrobotics/sdformat/pull/785) + +1. PrintConfig option to preserve includes when converting to string + * [Pull request #749](https://github.com/ignitionrobotics/sdformat/pull/749) + ### libsdformat 12.3.0 (2021-12-01) 1. Fix empty pose parsing fail for rotation_format='quat_xyzw' @@ -172,6 +322,100 @@ ## libsdformat 11.X +### libsdformat 11.4.1 (2022-03-21) + +1. Install sdf/1.8 to versioned path + * [Pull request #898](https://github.com/ignitionrobotics/sdformat/pull/898) + +### libsdformat 11.4.0 (2022-03-14) + +1. Added option to visualize light on the GUI + * [Pull request #877](https://github.com/ignitionrobotics/sdformat/pull/877) + +1. Fix joint parent/child frame existence checks to include interface elements + * [Pull request #855](https://github.com/ignitionrobotics/sdformat/pull/855) + +1. Added to light whether it is on or off + * [Pull request #851](https://github.com/ignitionrobotics/sdformat/pull/851) + +1. Allow model frames (__model__) to be used as joint parent or child + * [Pull request #833](https://github.com/ignitionrobotics/sdformat/pull/833) + +1. Add ParserConfig flag for preserveFixedJoint + * [Pull request #815](https://github.com/ignitionrobotics/sdformat/pull/815) + +1. Fix compiler warnings + * [Pull request #808](https://github.com/ignitionrobotics/sdformat/pull/808) + +1. `sdf_custom`: fix nested model expectations + * [Pull request #807](https://github.com/ignitionrobotics/sdformat/pull/807) + +1. Fix test compilation with `USE_INTERNAL_URDF` + * [Pull request #800](https://github.com/ignitionrobotics/sdformat/pull/800) + +1. Replace custom CMake code with `ign-cmake2` + * [Pull request #780](https://github.com/ignitionrobotics/sdformat/pull/780) + +1. Fix loading nested include with custom attributes + * [Pull request #789](https://github.com/ignitionrobotics/sdformat/pull/789) + +1. Documentation + 1. Clarify behavior of `//model/model/static` + * [Pull request #713](https://github.com/ignitionrobotics/sdformat/pull/713) + 1. Only allow one `canonical_link` attribute for model + * [Pull request #716](https://github.com/ignitionrobotics/sdformat/pull/716) + 1. Don't mention elements that can't be included + * [Pull request #715](https://github.com/ignitionrobotics/sdformat/pull/715) + 1. Clarify documentation on `//pose/@relative_to` in the spec + * [Pull request #666](https://github.com/ignitionrobotics/sdformat/pull/666) + 1. Remove duplicate link documentation + * [Pull request #702](https://github.com/ignitionrobotics/sdformat/pull/702) + +1. Fix URDF fixed joint reduction of plugins + * [Pull request #745](https://github.com/ignitionrobotics/sdformat/pull/745) + +1. Add `enable_orientation` to 1.6 spec + * [Pull request #686](https://github.com/ignitionrobotics/sdformat/pull/686) + +1. Remove outdated deprecation note from `parser_urdf.hh` + * [Pull request #740](https://github.com/ignitionrobotics/sdformat/pull/740) + +1. Add Joint DOM API to access joint sensors + * [Pull request #517](https://github.com/ignitionrobotics/sdformat/pull/517) + +1. Add force torque sensor + * [Pull request #393](https://github.com/ignitionrobotics/sdformat/pull/393) + * [Pull request #669](https://github.com/ignitionrobotics/sdformat/pull/669) + +1. Check joint parent link names in `Model::Load` + * [Pull request #726](https://github.com/ignitionrobotics/sdformat/pull/726) + +1. Check joint parent/child names in `Root::Load` + * [Pull request #727](https://github.com/ignitionrobotics/sdformat/pull/727) + +1. Remove empty `//inertial/pose/@relative_to` during 1_7->1.8 conversion + * [Pull request #720](https://github.com/ignitionrobotics/sdformat/pull/720) + +1. Fix `xyz` and `rpy` offsets in fixed joint reduction + * [Pull request #500](https://github.com/ignitionrobotics/sdformat/pull/500) + +1. Infrastructure updates + * [Pull request #674](https://github.com/ignitionrobotics/sdformat/pull/674) + * [Pull request #650](https://github.com/ignitionrobotics/sdformat/pull/650) + * [Pull request #626](https://github.com/ignitionrobotics/sdformat/pull/626) + * [Pull request #258](https://github.com/ignitionrobotics/sdformat/pull/258) + * [Pull request #237](https://github.com/ignitionrobotics/sdformat/pull/237) + * [Pull request #730](https://github.com/ignitionrobotics/sdformat/pull/730) + +1. Translate poses of nested models inside other nested models + * [Pull request #596](https://github.com/ignitionrobotics/sdformat/pull/596) + +1. Fix flattening logic for nested model names + * [Pull request #597](https://github.com/ignitionrobotics/sdformat/pull/597) + +1. Parse `rpyOffset` as radians + * [Pull request #497](https://github.com/ignitionrobotics/sdformat/pull/497) + ### libsdformat 11.3.0 (2021-09-10) 1. Fix world-complete.sdf and add particle_scatter_ratio to v1.8 diff --git a/examples/python/README.md b/examples/python/README.md new file mode 100644 index 000000000..e4e768ac7 --- /dev/null +++ b/examples/python/README.md @@ -0,0 +1,19 @@ +# sdformat Python API + +This example shows how to use sdformat's Python API. + +In the +[examples/python](https://github.com/ignitionrobotics/sdformat/blob/main/examples/python) +folder there is a Python script that shows how to make use of this API. + +> If you compiled sdformat from source you should modify your `PYTHONPATH`: +> +> ```bash +> export PYTHONPATH=$PYTHONPATH:/install/lib/python +> ``` + +Now you can run the example: + +```bash +$ python3 python_sdformat.py +``` diff --git a/examples/python/python_sdformat.py b/examples/python/python_sdformat.py new file mode 100644 index 000000000..7fe2a7acc --- /dev/null +++ b/examples/python/python_sdformat.py @@ -0,0 +1,15 @@ +# Copyright (C) 2022 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. + +import sdformat diff --git a/examples/usdConverter/README.md b/examples/usdConverter/README.md index 323b06306..669a07eb5 100644 --- a/examples/usdConverter/README.md +++ b/examples/usdConverter/README.md @@ -7,7 +7,7 @@ This example shows how a world in a SDF file can be converted to [USD](https://g You will need all of the dependencies for sdformat, along with the following additional dependencies: * USD: [installation instructions](https://github.com/PixarAnimationStudios/USD/blob/release/README.md#getting-and-building-the-code) * [ignition-common5](https://github.com/ignitionrobotics/ign-common) -* [ignition-utils1 (including the CLI component)](https://github.com/ignitionrobotics/ign-utils) +* [ignition-utils2 (including the CLI component)](https://github.com/ignitionrobotics/ign-utils) ## Setup diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index 993baf783..44c5874e6 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -152,6 +152,14 @@ namespace sdf /// \param[in] _fmt The pixel format string. public: void SetPixelFormatStr(const std::string &_fmt); + /// \brief Get the anti-aliasing value. + /// \return The anti-aliasing value. + public: uint32_t AntiAliasingValue() const; + + /// \brief Set the anti-aliasing value. + /// \param[in] _antiAliasingValue The anti-aliasing value. + public: void SetAntiAliasingValue(uint32_t _antiAliasingValue); + /// \brief Get the near clip distance for the depth camera. /// \return The near clip depth distance. public: double DepthNearClip() const; diff --git a/include/sdf/Light.hh b/include/sdf/Light.hh index 4de83ce3a..5e62b28c7 100644 --- a/include/sdf/Light.hh +++ b/include/sdf/Light.hh @@ -134,6 +134,14 @@ namespace sdf /// \param[in] _cast True to indicate that the light is on, False otherwise. public: void SetLightOn(const bool _isLightOn); + /// \brief Whether light visualization in the GUI is enabled. + /// \return True if visualization is enabled. + public: bool Visualize() const; + + /// \brief Set whether light visualization in the GUI is enabled. + /// \param[in] _visualize True to view the light on the GUI. + public: void SetVisualize(const bool _visualize); + /// \brief Get the light intensity /// \return The light intensity public: double Intensity() const; diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index c24749a73..b0a702dc9 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -24,6 +24,8 @@ #include #include #include "sdf/Element.hh" +#include "sdf/OutputConfig.hh" +#include "sdf/ParserConfig.hh" #include "sdf/Plugin.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" @@ -420,7 +422,8 @@ namespace sdf /// model. /// Note that parameter passing functionality is not captured with this /// function. - /// \param[in] _useIncludeTag When true, the model's URI is used to create + /// \param[in] _config Output configuration. When the ToElementUseIncludeTag + /// policy is true, the model's URI is used to create /// an SDF `` rather than a ``. The model's URI must be /// first set using the `Model::SetUri` function. If the model's URI is /// empty, then a `` element will be generated. The default is true @@ -429,7 +432,8 @@ namespace sdf /// is loaded from an `` tag since the parser will /// automatically expand an `` element to a `` element. /// \return SDF element pointer with updated model values. - public: sdf::ElementPtr ToElement(bool _useIncludeTag = true) const; + public: sdf::ElementPtr ToElement( + const OutputConfig &_config = OutputConfig::GlobalConfig()) const; /// \brief Check if a given name exists in the FrameAttachedTo graph at the /// scope of the model. diff --git a/include/sdf/OutputConfig.hh b/include/sdf/OutputConfig.hh new file mode 100644 index 000000000..d253025fb --- /dev/null +++ b/include/sdf/OutputConfig.hh @@ -0,0 +1,97 @@ +/* + * Copyright 2022 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. + * + */ + +#ifndef SDF_OUTPUT_CONFIG_HH_ +#define SDF_OUTPUT_CONFIG_HH_ + +#include + +#include "sdf/InterfaceElements.hh" +#include "sdf/sdf_config.h" +#include "sdf/system_util.hh" + + +namespace sdf +{ +inline namespace SDF_VERSION_NAMESPACE +{ +// Forward declare private data class. +class OutputConfigPrivate; + +/// This class contains configuration options for SDF output. Output +/// configuration can be used to specify how SDF is generated from in-memory +/// represenations, such as the DOM classes. +/// +/// Example: +/// The default behavior of the `ToElement` functions is to use `` +/// tags if the tags were present during load. You can choose not to use +/// the `` tags using the following snippet. +/// +/// \code{.cpp} +/// // Create an output config; +/// sdf::OutputConfig config; +/// +/// // Set the config so that include tags are not used. +/// config.SetToElementUseIncludeTag(false); +/// +/// // Use the new config when generating the SDFormat element. +/// sdf::Root root; +/// sdf::ElementPtr elem = root.ToElement(config); +/// +/// // You can now output the element as a string. +/// std::cout << elem->ToString("") << std::endl; +/// \endcode +class SDFORMAT_VISIBLE OutputConfig +{ + /// \brief Default constructor + public: OutputConfig(); + + /// Mutable access to a singleton OutputConfig that serves as the global + /// OutputConfig object for all parsing operations that do not specify their + /// own OutputConfig. + /// \return A mutable reference to the singleton OutputConfig object + public: static OutputConfig &GlobalConfig(); + + /// \brief Several DOM classes have ToElement() methods that return an + /// XML Element populated from the contents of the DOM object. When + /// populating the details of a model that was included using the + /// tag, one may wish to retain the exact include tag and + /// URI instead of copying the full details of the included model. + /// This method lets you set this behavior. + /// \param[in] _useIncludeTag When true, the model's URI is used to create + /// an SDF `` rather than a ``. The model's URI must be + /// first set using the `Model::SetUri` function. If the model's URI is + /// empty, then a `` element will be generated. The default is true + /// so that URI values are used when ToElement is called from a + /// World object. Make sure to use `Model::SetUri` even when the model + /// is loaded from an `` tag since the parser will + /// automatically expand an `` element to a `` element. + public: void SetToElementUseIncludeTag(bool _useIncludeTag); + + /// \brief Get the policy value about whether tags are + /// reconstituted in ToElement() invocations. + /// \return True if include tags are reconstituted, or false + /// if the fully populated model is returned instead. + public: bool ToElementUseIncludeTag() const; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) +}; +} +} + +#endif diff --git a/include/sdf/Param.hh b/include/sdf/Param.hh index 827ba3f06..e9404022b 100644 --- a/include/sdf/Param.hh +++ b/include/sdf/Param.hh @@ -75,28 +75,37 @@ namespace sdf struct ParamStreamer { const T &val; + const int precision; // Used to set std::ostream's std::setprecision + explicit ParamStreamer(const T &_val, int _precision = 0) + : val(_val), precision(_precision) {} }; - template ParamStreamer(T) -> ParamStreamer; - template std::ostream& operator<<(std::ostream &os, ParamStreamer s) { - os << s.val; - return os; - } - - template<> - inline std::ostream& operator<<(std::ostream &os, ParamStreamer s) - { - os << std::setprecision(std::numeric_limits::max_digits10) << s.val; - return os; - } + if (s.precision == std::numeric_limits::max()) + { + if constexpr (std::is_same_v + || std::is_same_v + || std::is_same_v + || std::is_same_v + || std::is_same_v + || std::is_same_v) + { + os << std::setprecision(std::numeric_limits::max_digits10); + } + else if constexpr (std::is_same_v + || std::is_same_v) + { + os << std::setprecision(std::numeric_limits::max_digits10); + } + } + else + { + os << std::setprecision(s.precision); + } - template<> - inline std::ostream& operator<<(std::ostream &os, ParamStreamer s) - { - os << std::setprecision(std::numeric_limits::max_digits10) << s.val; + os << s.val; return os; } @@ -104,9 +113,9 @@ namespace sdf std::ostream& operator<<(std::ostream& os, ParamStreamer> sv) { - std::visit([&os](auto const &v) + std::visit([&os, &sv](auto const &v) { - os << ParamStreamer{v}; + os << ParamStreamer{v, sv.precision}; }, sv.val); return os; } @@ -357,7 +366,8 @@ namespace sdf /// \def ParamVariant /// \brief Variant type def. /// Note: When a new variant is added, add variant to functions - /// ParamPrivate::TypeToString and ParamPrivate::ValueFromStringImpl + /// ParamPrivate::TypeToString, ParamPrivate::ValueFromStringImpl, + /// and template std::ostream operator if new variant is floating point public: typedef std::variant &_originalStr, - std::string &_valueStr) const; - /// \brief Data type to string mapping /// \return The type as a string, empty string if unknown type public: template diff --git a/include/sdf/Plane.hh b/include/sdf/Plane.hh index 3ba3d1b6e..c920a3801 100644 --- a/include/sdf/Plane.hh +++ b/include/sdf/Plane.hh @@ -52,7 +52,7 @@ namespace sdf /// \return The plane normal vector. public: ignition::math::Vector3d Normal() const; - /// \brief Set the plane normal vector. The _normal vector will be + /// \brief Set the plane normal vector. The normal vector will be /// normalized. See ignition::math::Vector3d Normal() for more information /// about the normal vector, such as the frame in which it is specified. /// \param[in] _normal The plane normal vector. diff --git a/include/sdf/Plugin.hh b/include/sdf/Plugin.hh index 56b33abea..d6471022f 100644 --- a/include/sdf/Plugin.hh +++ b/include/sdf/Plugin.hh @@ -58,6 +58,16 @@ namespace sdf /// \param[in] _plugin Plugin to copy. public: Plugin(Plugin &&_plugin) noexcept; + /// \brief A constructor that initializes the plugin's filename, name, and + /// optionally the content. + /// \param[in] _filename Filename of the shared library associated with + /// this plugin. + /// \param[in] _name The name of the plugin. + /// \param[in] _xmlContent Optional XML content that will be stored in + /// this plugin. + public: Plugin(const std::string &_filename, const std::string &_name, + const std::string &_xmlContent = ""); + /// \brief Load the plugin based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -97,6 +107,15 @@ namespace sdf /// \param[in] _elem Element to insert. public: void InsertContent(const sdf::ElementPtr _elem); + /// \brief Insert XML content into this plugin. This function does not + /// modify the values in the sdf::ElementPtr returned by the `Element()` + /// function. The provided content must be valid XML. + /// \param[in] _content A string that contains valid XML. The XML is + /// inserted into this plugin if it is valid. + /// \return False if the provided content was invalid, in which case the + /// content of this plugin is not modified. True otherwise + public: bool InsertContent(const std::string _content); + /// \brief Set the filename of the shared library. /// \param[in] _filename Filename of the shared library associated with /// this plugin. diff --git a/include/sdf/PrintConfig.hh b/include/sdf/PrintConfig.hh index 8696339c3..c74bfa590 100644 --- a/include/sdf/PrintConfig.hh +++ b/include/sdf/PrintConfig.hh @@ -75,6 +75,16 @@ namespace sdf /// False if they are to be expanded. public: bool PreserveIncludes() const; + /// \brief Set precision of output stream for float / double types. + /// By default, the output stream uses maximum precision. + /// \param[in] _precision The new precision value. To set back to maximum + /// precision, use std::numeric_limits::max(). + public: void SetOutPrecision(int _precision); + + /// \brief Retrieve the output stream's set precision value. + /// \return The output stream's precision. + public: int OutPrecision() const; + /// \brief Return true if both PrintConfig objects contain the same values. /// \param[in] _config PrintConfig to compare. /// \return True if 'this' == _config. diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 65d63ae71..318fa7e1b 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -20,6 +20,8 @@ #include #include +#include "sdf/OutputConfig.hh" +#include "sdf/ParserConfig.hh" #include "sdf/SDFImpl.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" @@ -202,10 +204,10 @@ namespace sdf /// root. /// Note that parameter passing functionality is not captured with this /// function. - /// \param[in] _useIncludeTag This will pass the _useIncludeTag to - /// sdf::Model::ToElement. + /// \param[in] _config Custom output configuration /// \return SDF element pointer with updated root values. - public: sdf::ElementPtr ToElement(bool _useIncludeTag = true) const; + public: sdf::ElementPtr ToElement( + const OutputConfig &_config = OutputConfig::GlobalConfig()) const; /// \brief Private data pointer IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) diff --git a/include/sdf/Surface.hh b/include/sdf/Surface.hh index 4eb335692..724b45e63 100644 --- a/include/sdf/Surface.hh +++ b/include/sdf/Surface.hh @@ -58,6 +58,103 @@ namespace sdf IGN_UTILS_IMPL_PTR(dataPtr) }; + /// \brief ODE information for a friction. + class SDFORMAT_VISIBLE ODE + { + /// \brief Default constructor + public: ODE(); + + /// \brief Load the ODE based on a element pointer. This is *not* the + /// usual entry point. Typical usage of the SDF DOM is through the Root + /// object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Set the Mu + /// \returns ODE mu + public: double Mu() const; + + /// \brief Set Mu + /// \param[in] _mu ODE mu + public: void SetMu(double _mu); + + /// \brief Get the Mu2 + /// \returns ODE mu2 + public: double Mu2() const; + + /// \brief Set Mu2 + /// \param[in] _mu2 ODE mu2 + public: void SetMu2(double _mu2); + + /// \brief Get the fdir + /// \returns ODE fdir + public: const ignition::math::Vector3d &Fdir1() const; + + /// \brief Set fdir + /// \param[in] _fdir ODE fdir + public: void SetFdir1(const ignition::math::Vector3d &_fdir); + + /// \brief Get the slip1 + /// \returns ODE slip1 + public: double Slip1() const; + + /// \brief Set Slip1 + /// \param[in] _slip1 ODE Slip1 + public: void SetSlip1(double _slip1); + + /// \brief Get the Slip2 + /// \returns ODE Slip2 + public: double Slip2() const; + + /// \brief Set Slip2 + /// \param[in] _slip2 ODE Slip2 + public: void SetSlip2(double _slip2); + + /// \brief Get a pointer to the SDF element that was used during + /// load. + /// \return SDF element pointer. The value will be nullptr if Load has + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + + /// \brief Friction information for a surface. + class SDFORMAT_VISIBLE Friction + { + /// \brief Default constructor + public: Friction(); + + /// \brief Load the friction based on a element pointer. This is *not* the + /// usual entry point. Typical usage of the SDF DOM is through the Root + /// object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get the associated ODE object + /// \returns Pointer to the associated ODE object, + /// nullptr if the Surface doesn't contain a ODE element. + public: const sdf::ODE *ODE() const; + + /// \brief Set the associated ODE object. + /// \param[in] _ode The ODE object. + public: void SetODE(const sdf::ODE &_ode); + + /// \brief Get a pointer to the SDF element that was used during + /// load. + /// \return SDF element pointer. The value will be nullptr if Load has + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + /// \brief Surface information for a collision. class SDFORMAT_VISIBLE Surface { @@ -87,6 +184,15 @@ namespace sdf /// \param[in] _cont The contact object. public: void SetContact(const sdf::Contact &_contact); + /// \brief Get the associated friction object + /// \returns Pointer to the associated friction object, + /// nullptr if the Surface doesn't contain a friction element. + public: const sdf::Friction *Friction() const; + + /// \brief Set the associated friction object. + /// \param[in] _friction The friction object. + public: void SetFriction(const sdf::Friction &_friction); + /// \brief Create and return an SDF element filled with data from this /// surface. /// Note that parameter passing functionality is not captured with this diff --git a/include/sdf/World.hh b/include/sdf/World.hh index c3149b65e..7a3e28f0d 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -27,6 +27,8 @@ #include "sdf/Atmosphere.hh" #include "sdf/Element.hh" #include "sdf/Gui.hh" +#include "sdf/OutputConfig.hh" +#include "sdf/ParserConfig.hh" #include "sdf/Plugin.hh" #include "sdf/Scene.hh" #include "sdf/Types.hh" @@ -431,10 +433,10 @@ namespace sdf /// world. /// Note that parameter passing functionality is not captured with this /// function. - /// \param[in] _useIncludeTag This parameter is passed through to the - /// Model::ToElement function. + /// \param[in] _config Custom parser configuration /// \return SDF element pointer with updated world values. - public: sdf::ElementPtr ToElement(bool _useIncludeTag = true) const; + public: sdf::ElementPtr ToElement( + const OutputConfig &_config = OutputConfig::GlobalConfig()) const; /// \brief Get the plugins attached to this object. /// \return A vector of Plugin, which will be empty if there are no diff --git a/include/sdf/parser.hh b/include/sdf/parser.hh index dae956840..557c8e149 100644 --- a/include/sdf/parser.hh +++ b/include/sdf/parser.hh @@ -46,6 +46,13 @@ namespace sdf SDFORMAT_VISIBLE bool init(SDFPtr _sdf); + /// \brief Initialize the SDF interface from the embedded root spec file + /// \param[out] _sdf Pointer to an SDF object. + /// \param[in] _config Custom parser configuration + /// \return True if successful. + SDFORMAT_VISIBLE + bool init(SDFPtr _sdf, const ParserConfig &_config); + /// \brief Initialize the SDF interface using a file /// \param[in] _filename Name of the SDF file /// \param[out] _sdf Pointer to an SDF object. diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 000000000..cf69d5531 --- /dev/null +++ b/python/CMakeLists.txt @@ -0,0 +1,115 @@ +if(WIN32 AND CMAKE_BUILD_TYPE STREQUAL "Debug") + # pybind11 logic for setting up a debug build when both a debug and release + # python interpreter are present in the system seems to be pretty much broken. + # This works around the issue. + set(PYTHON_LIBRARIES "${PYTHON_DEBUG_LIBRARIES}") +endif() + + +if(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION) + if(${CMAKE_VERSION} VERSION_LESS "3.12.0") + execute_process( + COMMAND "${PYTHON_EXECUTABLE}" -c "if True: + from distutils import sysconfig as sc + print(sc.get_python_lib(plat_specific=True))" + OUTPUT_VARIABLE Python3_SITEARCH + OUTPUT_STRIP_TRAILING_WHITESPACE) + else() + # Get install variable from Python3 module + # Python3_SITEARCH is available from 3.12 on, workaround if needed: + find_package(Python3 COMPONENTS Interpreter) + endif() + + if(USE_DIST_PACKAGES_FOR_PYTHON) + string(REPLACE "site-packages" "dist-packages" IGN_PYTHON_INSTALL_PATH ${Python3_SITEARCH}) + endif() +else() + # If not a system installation, respect local paths + set(IGN_PYTHON_INSTALL_PATH ${IGN_LIB_INSTALL_DIR}/python) +endif() + +# Set the build location and install location for a CPython extension +function(configure_build_install_location _library_name) + # Install library for actual use + install(TARGETS ${_library_name} + DESTINATION "${IGN_PYTHON_INSTALL_PATH}" + ) +endfunction() + +pybind11_add_module(sdformat SHARED + src/sdf/_ignition_sdformat_pybind11.cc + src/sdf/pyBox.cc + src/sdf/pyCapsule.cc + src/sdf/pyCollision.cc + src/sdf/pyCylinder.cc + src/sdf/pyEllipsoid.cc + src/sdf/pyError.cc + src/sdf/pyFrame.cc + src/sdf/pyGeometry.cc + src/sdf/pyJoint.cc + src/sdf/pyJointAxis.cc + src/sdf/pyLink.cc + src/sdf/pyMaterial.cc + src/sdf/pyMesh.cc + src/sdf/pyModel.cc + src/sdf/pyNoise.cc + src/sdf/pyParserConfig.cc + src/sdf/pyPlane.cc + src/sdf/pySemanticPose.cc + src/sdf/pySphere.cc + src/sdf/pySurface.cc + src/sdf/pyVisual.cc + src/sdf/pyWorld.cc +) + +target_link_libraries(sdformat PRIVATE + ${PROJECT_LIBRARY_TARGET_NAME} +) + +configure_build_install_location(sdformat) + +if (BUILD_TESTING) + pybind11_add_module(sdformattest SHARED + test/_ignition_sdformattest_pybind11.cc + ) + + target_link_libraries(sdformattest PRIVATE + ${PROJECT_LIBRARY_TARGET_NAME} + ignition-utils${IGN_UTILS_VER}::ignition-utils${IGN_UTILS_VER} + ) + + set(python_tests + pyBox_TEST + pyCapsule_TEST + pyCollision_TEST + pyCylinder_TEST + pyEllipsoid_TEST + pyError_TEST + pyFrame_TEST + pyGeometry_TEST + pyJoint_TEST + pyJointAxis_TEST + pyLink_TEST + pyMaterial_TEST + pyMesh_TEST + pyModel_TEST + pyNoise_TEST + pyParserConfig_TEST + pyPlane_TEST + pySemanticPose_TEST + pySphere_TEST + pySurface_TEST + pyVisual_TEST + pyWorld_TEST + ) + + foreach (test ${python_tests}) + add_test(NAME ${test}.py COMMAND + "${Python3_EXECUTABLE}" "${CMAKE_SOURCE_DIR}/python/test/${test}.py") + set(_env_vars) + list(APPEND _env_vars "PYTHONPATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/python/:${CMAKE_BINARY_DIR}/lib:$ENV{PYTHONPATH}") + list(APPEND _env_vars "LD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}:$ENV{LD_LIBRARY_PATH}") + set_tests_properties(${test}.py PROPERTIES + ENVIRONMENT "${_env_vars}") + endforeach() +endif() diff --git a/python/src/sdf/_ignition_sdformat_pybind11.cc b/python/src/sdf/_ignition_sdformat_pybind11.cc new file mode 100644 index 000000000..58207e642 --- /dev/null +++ b/python/src/sdf/_ignition_sdformat_pybind11.cc @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2022 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 + +#include "pyBox.hh" +#include "pyCapsule.hh" +#include "pyCollision.hh" +#include "pyCylinder.hh" +#include "pyEllipsoid.hh" +#include "pyError.hh" +#include "pyFrame.hh" +#include "pyGeometry.hh" +#include "pyJoint.hh" +#include "pyJointAxis.hh" +#include "pyLink.hh" +#include "pyMaterial.hh" +#include "pyMesh.hh" +#include "pyModel.hh" +#include "pyNoise.hh" +#include "pyParserConfig.hh" +#include "pyPlane.hh" +#include "pySemanticPose.hh" +#include "pySphere.hh" +#include "pySurface.hh" +#include "pyVisual.hh" +#include "pyWorld.hh" + +PYBIND11_MODULE(sdformat, m) { + m.doc() = "sdformat Python Library."; + + sdf::python::defineBox(m); + sdf::python::defineCapsule(m); + sdf::python::defineCollision(m); + sdf::python::defineContact(m); + sdf::python::defineCylinder(m); + sdf::python::defineEllipsoid(m); + sdf::python::defineError(m); + sdf::python::defineFrame(m); + sdf::python::defineGeometry(m); + sdf::python::defineJoint(m); + sdf::python::defineJointAxis(m); + sdf::python::defineLink(m); + sdf::python::defineMaterial(m); + sdf::python::defineMesh(m); + sdf::python::defineModel(m); + sdf::python::defineNoise(m); + sdf::python::defineParserConfig(m); + sdf::python::definePlane(m); + sdf::python::defineSemanticPose(m); + sdf::python::defineSphere(m); + sdf::python::defineSurface(m); + sdf::python::defineVisual(m); + sdf::python::defineWorld(m); +} diff --git a/python/src/sdf/pyBox.cc b/python/src/sdf/pyBox.cc new file mode 100644 index 000000000..2040c9c2a --- /dev/null +++ b/python/src/sdf/pyBox.cc @@ -0,0 +1,54 @@ +/* + * Copyright (C) 2022 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 "pyBox.hh" + +#include + +#include "sdf/Box.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineBox(pybind11::object module) +{ + pybind11::class_(module, "Box") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("size", &sdf::Box::Size, + "Get the box size in meters.") + .def("set_size", &sdf::Box::SetSize, + "Set the box size in meters.") + .def( + "shape", + pybind11::overload_cast<>(&sdf::Box::Shape, pybind11::const_), + pybind11::return_value_policy::reference, + "Get a mutable Ignition Math representation of this Box.") + .def("__copy__", [](const sdf::Box &self) { + return sdf::Box(self); + }) + .def("__deepcopy__", [](const sdf::Box &self, pybind11::dict) { + return sdf::Box(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyBox.hh b/python/src/sdf/pyBox.hh new file mode 100644 index 000000000..dd1686f15 --- /dev/null +++ b/python/src/sdf/pyBox.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_BOX_HH_ +#define SDFORMAT_PYTHON_BOX_HH_ + +#include + +#include "sdf/Box.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Box +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineBox(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_BOX_HH_ diff --git a/python/src/sdf/pyCapsule.cc b/python/src/sdf/pyCapsule.cc new file mode 100644 index 000000000..4fc2c694d --- /dev/null +++ b/python/src/sdf/pyCapsule.cc @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2022 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 "pyCapsule.hh" + +#include + +#include "sdf/Capsule.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineCapsule(pybind11::object module) +{ + pybind11::class_(module, "Capsule") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("radius", &sdf::Capsule::Radius, + "Get the capsule's radius in meters.") + .def("set_radius", &sdf::Capsule::SetRadius, + "Set the capsule's radius in meters.") + .def("length", &sdf::Capsule::Length, + "Get the capsule's length in meters.") + .def("set_length", &sdf::Capsule::SetLength, + "Set the capsule's length in meters.") + .def( + "shape", + pybind11::overload_cast<>(&sdf::Capsule::Shape, pybind11::const_), + pybind11::return_value_policy::reference, + "Get a mutable Ignition Math representation of this Capsule.") + .def("__copy__", [](const sdf::Capsule &self) { + return sdf::Capsule(self); + }) + .def("__deepcopy__", [](const sdf::Capsule &self, pybind11::dict) { + return sdf::Capsule(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyCapsule.hh b/python/src/sdf/pyCapsule.hh new file mode 100644 index 000000000..246e8341f --- /dev/null +++ b/python/src/sdf/pyCapsule.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_CAPSULE_HH_ +#define SDFORMAT_PYTHON_CAPSULE_HH_ + +#include + +#include "sdf/Capsule.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Capsule +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineCapsule(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_CAPSULE_HH_ diff --git a/python/src/sdf/pyCollision.cc b/python/src/sdf/pyCollision.cc new file mode 100644 index 000000000..c7305063e --- /dev/null +++ b/python/src/sdf/pyCollision.cc @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2022 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 "pyCollision.hh" + +#include + +#include "sdf/Collision.hh" +#include "sdf/Geometry.hh" +#include "sdf/Surface.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineCollision(pybind11::object module) +{ + pybind11::class_ geometryModule(module, "Collision"); + geometryModule + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("name", &sdf::Collision::Name, + "Get the name of the collision. " + "The name of the collision must be unique within the scope of a Link.") + .def("set_name", &sdf::Collision::SetName, + "Set the name of the collision. " + "The name of the collision must be unique within the scope of a Link.") + .def("geometry", &sdf::Collision::Geom, + pybind11::return_value_policy::reference, + "Get a pointer to the collisions's geometry.") + .def("set_geometry", &sdf::Collision::SetGeom, + "Set the collision's geometry") + .def("surface", &sdf::Collision::Surface, + pybind11::return_value_policy::reference, + "Get a pointer to the collisions's surface parameters.") + .def("set_surface", &sdf::Collision::SetSurface, + "Set the collision's surface parameters") + .def("raw_pose", &sdf::Collision::RawPose, + "Get the pose of the collision object. This is the pose of the " + "collison as specified in SDF") + .def("set_raw_pose", &sdf::Collision::SetRawPose, + "Set the pose of the collision object.") + .def("pose_relative_to", &sdf::Collision::PoseRelativeTo, + "Get the name of the coordinate frame relative to which this " + "object's pose is expressed. An empty value indicates that the frame " + "is relative to the parent link.") + .def("set_pose_relative_to", &sdf::Collision::SetPoseRelativeTo, + "Set the name of the coordinate frame relative to which this " + "object's pose is expressed. An empty value indicates that the frame " + "is relative to the parent link.") + .def("semantic_pose", &sdf::Collision::SemanticPose, + "Get SemanticPose object of this object to aid in resolving " + "poses.") + .def("__copy__", [](const sdf::Collision &self) { + return sdf::Collision(self); + }) + .def("__deepcopy__", [](const sdf::Collision &self, pybind11::dict) { + return sdf::Collision(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyCollision.hh b/python/src/sdf/pyCollision.hh new file mode 100644 index 000000000..2d32b00f3 --- /dev/null +++ b/python/src/sdf/pyCollision.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_COLLISION_HH_ +#define SDFORMAT_PYTHON_COLLISION_HH_ + +#include + +#include "sdf/Collision.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Collision +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineCollision(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_COLLISION_HH_ diff --git a/python/src/sdf/pyCylinder.cc b/python/src/sdf/pyCylinder.cc new file mode 100644 index 000000000..49337ceba --- /dev/null +++ b/python/src/sdf/pyCylinder.cc @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2022 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 "pyCylinder.hh" + +#include + +#include "sdf/Cylinder.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineCylinder(pybind11::object module) +{ + pybind11::class_(module, "Cylinder") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("radius", &sdf::Cylinder::Radius, + "Get the cylinder's radius in meters.") + .def("set_radius", &sdf::Cylinder::SetRadius, + "Set the cylinder's radius in meters.") + .def("length", &sdf::Cylinder::Length, + "Get the cylinder's length in meters.") + .def("set_length", &sdf::Cylinder::SetLength, + "Set the cylinder's length in meters.") + .def( + "shape", + pybind11::overload_cast<>(&sdf::Cylinder::Shape, pybind11::const_), + pybind11::return_value_policy::reference, + "Get a mutable Ignition Math representation of this Cylinder.") + .def("__copy__", [](const sdf::Cylinder &self) { + return sdf::Cylinder(self); + }) + .def("__deepcopy__", [](const sdf::Cylinder &self, pybind11::dict) { + return sdf::Cylinder(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyCylinder.hh b/python/src/sdf/pyCylinder.hh new file mode 100644 index 000000000..fecf4bfa9 --- /dev/null +++ b/python/src/sdf/pyCylinder.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_CYLINDER_HH_ +#define SDFORMAT_PYTHON_CYLINDER_HH_ + +#include + +#include "sdf/Cylinder.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Cylinder +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineCylinder(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_CYLINDER_HH_ diff --git a/python/src/sdf/pyEllipsoid.cc b/python/src/sdf/pyEllipsoid.cc new file mode 100644 index 000000000..645ef7b9b --- /dev/null +++ b/python/src/sdf/pyEllipsoid.cc @@ -0,0 +1,54 @@ +/* + * Copyright (C) 2022 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 "pyEllipsoid.hh" + +#include + +#include "sdf/Ellipsoid.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineEllipsoid(pybind11::object module) +{ + pybind11::class_(module, "Ellipsoid") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("radii", &sdf::Ellipsoid::Radii, + "Get the ellipsoid's radii in meters.") + .def("set_radii", &sdf::Ellipsoid::SetRadii, + "Set the ellipsoid's x, y, and z radii in meters.") + .def( + "shape", + pybind11::overload_cast<>(&sdf::Ellipsoid::Shape, pybind11::const_), + pybind11::return_value_policy::reference, + "Get a mutable Ignition Math representation of this Ellipsoid.") + .def("__copy__", [](const sdf::Ellipsoid &self) { + return sdf::Ellipsoid(self); + }) + .def("__deepcopy__", [](const sdf::Ellipsoid &self, pybind11::dict) { + return sdf::Ellipsoid(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyEllipsoid.hh b/python/src/sdf/pyEllipsoid.hh new file mode 100644 index 000000000..96ba1f690 --- /dev/null +++ b/python/src/sdf/pyEllipsoid.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_ELLIPSOID_HH_ +#define SDFORMAT_PYTHON_ELLIPSOID_HH_ + +#include + +#include "sdf/Ellipsoid.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Ellipsoid +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineEllipsoid(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_ELLIPSOID_HH_ diff --git a/python/src/sdf/pyError.cc b/python/src/sdf/pyError.cc new file mode 100644 index 000000000..fb563a604 --- /dev/null +++ b/python/src/sdf/pyError.cc @@ -0,0 +1,145 @@ +/* + * Copyright (C) 2022 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 "pyError.hh" + +#include +#include +#include + +#include +#include + +#include "sdf/Error.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineError(pybind11::object module) +{ + pybind11::class_ errorModule(module, "Error"); + auto toString = [](const sdf::Error &si) { + std::stringstream stream; + stream << si; + return stream.str(); + }; + errorModule + .def(pybind11::init<>()) + .def(pybind11::init()) + .def(pybind11::init()) + .def(pybind11::init()) + .def("code", &sdf::Error::Code, + "Get the error code.") + .def("message", &sdf::Error::Message, + "Get the error message, which is a description of the error.") + .def( + "file_path", &sdf::Error::FilePath, + "Get the file path associated with this error.") + .def( + "set_file_path", &sdf::Error::SetFilePath, + "Sets the file path that is associated with this error.") + .def( + "line_number", &sdf::Error::LineNumber, + "Get the line number associated with this error.") + .def( + "set_line_number", &sdf::Error::SetLineNumber, + "Sets the line number that is associated with this error.") + .def( + "xml_path", &sdf::Error::XmlPath, + "Get the XPath-like trace that is associated with this error.") + .def( + "set_xml_path", &sdf::Error::SetXmlPath, + "Sets the XML path that is associated with this error.") + .def( + "is_valid", + [](const sdf::Error &self) + { + return self == true; + }, + "True if this Error's Code() != NONE. In otherwords, this is " + "true when there is an error, or false otherwise.") + .def("__str__", toString) + .def("__repr__", toString) + .def("__copy__", [](const sdf::Error &self) { + return sdf::Error(self); + }) + .def("__deepcopy__", [](const sdf::Error &self, pybind11::dict) { + return sdf::Error(self); + }, "memo"_a); + + pybind11::enum_(errorModule, "ErrorCode") + .value("NONE", sdf::ErrorCode::NONE) + .value("FILE_READ", sdf::ErrorCode::FILE_READ) + .value("DUPLICATE_NAME", sdf::ErrorCode::DUPLICATE_NAME) + .value("RESERVED_NAME", sdf::ErrorCode::RESERVED_NAME) + .value("ATTRIBUTE_MISSING", sdf::ErrorCode::ATTRIBUTE_MISSING) + .value("ATTRIBUTE_INVALID", sdf::ErrorCode::ATTRIBUTE_INVALID) + .value("ATTRIBUTE_DEPRECATED", sdf::ErrorCode::ATTRIBUTE_DEPRECATED) + .value("ATTRIBUTE_INCORRECT_TYPE", + sdf::ErrorCode::ATTRIBUTE_INCORRECT_TYPE) + .value("ELEMENT_MISSING", sdf::ErrorCode::ELEMENT_MISSING) + .value("ELEMENT_INVALID", sdf::ErrorCode::ELEMENT_INVALID) + .value("ELEMENT_DEPRECATED", sdf::ErrorCode::ELEMENT_DEPRECATED) + .value("ELEMENT_INCORRECT_TYPE", + sdf::ErrorCode::ELEMENT_INCORRECT_TYPE) + .value("URI_INVALID", sdf::ErrorCode::URI_INVALID) + .value("URI_LOOKUP", sdf::ErrorCode::URI_LOOKUP) + .value("DIRECTORY_NONEXISTANT", + sdf::ErrorCode::DIRECTORY_NONEXISTANT) + .value("MODEL_CANONICAL_LINK_INVALID", + sdf::ErrorCode::MODEL_CANONICAL_LINK_INVALID) + .value("MODEL_WITHOUT_LINK", sdf::ErrorCode::MODEL_WITHOUT_LINK) + .value("NESTED_MODELS_UNSUPPORTED", + sdf::ErrorCode::NESTED_MODELS_UNSUPPORTED) + .value("LINK_INERTIA_INVALID", sdf::ErrorCode::LINK_INERTIA_INVALID) + .value("JOINT_CHILD_LINK_INVALID", + sdf::ErrorCode::JOINT_CHILD_LINK_INVALID) + .value("JOINT_PARENT_LINK_INVALID", + sdf::ErrorCode::JOINT_PARENT_LINK_INVALID) + .value("JOINT_PARENT_SAME_AS_CHILD", + sdf::ErrorCode::JOINT_PARENT_SAME_AS_CHILD) + .value("FRAME_ATTACHED_TO_INVALID", + sdf::ErrorCode::FRAME_ATTACHED_TO_INVALID) + .value("FRAME_ATTACHED_TO_CYCLE", + sdf::ErrorCode::FRAME_ATTACHED_TO_CYCLE) + .value("FRAME_ATTACHED_TO_GRAPH_ERROR", + sdf::ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR) + .value("POSE_RELATIVE_TO_INVALID", + sdf::ErrorCode::POSE_RELATIVE_TO_INVALID) + .value("POSE_RELATIVE_TO_CYCLE", + sdf::ErrorCode::POSE_RELATIVE_TO_CYCLE) + .value("POSE_RELATIVE_TO_GRAPH_ERROR", + sdf::ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR) + .value("STRING_READ", sdf::ErrorCode::STRING_READ) + .value("MODEL_PLACEMENT_FRAME_INVALID", + sdf::ErrorCode::MODEL_PLACEMENT_FRAME_INVALID) + .value("VERSION_DEPRECATED", sdf::ErrorCode::VERSION_DEPRECATED) + .value("MERGE_INCLUDE_UNSUPPORTED", + sdf::ErrorCode::MERGE_INCLUDE_UNSUPPORTED); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyError.hh b/python/src/sdf/pyError.hh new file mode 100644 index 000000000..b68a01d6c --- /dev/null +++ b/python/src/sdf/pyError.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_ERROR_HH_ +#define SDFORMAT_PYTHON_ERROR_HH_ + +#include + +#include "sdf/Error.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Error +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineError(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_ERROR_HH_ diff --git a/python/src/sdf/pyFrame.cc b/python/src/sdf/pyFrame.cc new file mode 100644 index 000000000..2d2660049 --- /dev/null +++ b/python/src/sdf/pyFrame.cc @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2022 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 "pyFrame.hh" + +#include +#include + +#include "sdf/Frame.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineFrame(pybind11::object module) +{ + pybind11::class_(module, "Frame") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("name", &sdf::Frame::Name, + "Get the name of the frame.") + .def("set_name", &sdf::Frame::SetName, + "Set the name of the frame.") + .def("attached_to", &sdf::Frame::AttachedTo, + "Get the name of the coordinate frame to which this " + "frame is attached.") + .def("set_attached_to", &sdf::Frame::SetAttachedTo, + "Set the name of the coordinate frame to which this " + "frame is attached.") + .def("raw_pose", &sdf::Frame::RawPose, + "Get the pose of the frame object. This is the pose of the " + "frame as specified in SDF") + .def("set_raw_pose", &sdf::Frame::SetRawPose, + "Set the raw pose of the frame object. This is interpreted " + "relative to the frame named in the //pose/@relative_to attribute.") + .def("pose_relative_to", &sdf::Frame::PoseRelativeTo, + "Get the name of the coordinate frame relative to which this " + "frame's pose is expressed. An empty value indicates that the frame " + "is expressed relative to the attached-to link.") + .def("set_pose_relative_to", &sdf::Frame::SetPoseRelativeTo, + "Set the name of the coordinate frame relative to which this " + "frame's pose is expressed. An empty value indicates that the frame " + "is expressed relative to the attached-to link.") + .def("resolve_attached_to_body", + [](const sdf::Frame &self) + { + std::string body; + auto errors = self.ResolveAttachedToBody(body); + return std::make_tuple(errors, body); + }, + "Resolve the attached-to body of this frame from the " + "FrameAttachedToGraph. Generally, it resolves to the name of a link, " + "but if it is in the world scope, it can resolve to \"world\".") + .def("semantic_pose", &sdf::Frame::SemanticPose, + "Get SemanticPose object of this object to aid in resolving " + "poses.") + .def("__copy__", [](const sdf::Frame &self) { + return sdf::Frame(self); + }) + .def("__deepcopy__", [](const sdf::Frame &self, pybind11::dict) { + return sdf::Frame(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyFrame.hh b/python/src/sdf/pyFrame.hh new file mode 100644 index 000000000..2ad93ac71 --- /dev/null +++ b/python/src/sdf/pyFrame.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_FRAME_HH_ +#define SDFORMAT_PYTHON_FRAME_HH_ + +#include + +#include "sdf/Frame.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Frame +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineFrame(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_FRAME_HH_ diff --git a/python/src/sdf/pyGeometry.cc b/python/src/sdf/pyGeometry.cc new file mode 100644 index 000000000..cffecb0d6 --- /dev/null +++ b/python/src/sdf/pyGeometry.cc @@ -0,0 +1,114 @@ +/* + * Copyright (C) 2022 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 "pyGeometry.hh" + +#include + +#include "sdf/ParserConfig.hh" + +#include "sdf/Box.hh" +#include "sdf/Capsule.hh" +#include "sdf/Cylinder.hh" +#include "sdf/Ellipsoid.hh" +#include "sdf/Geometry.hh" +#include "sdf/Mesh.hh" +#include "sdf/Plane.hh" +#include "sdf/Sphere.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineGeometry(pybind11::object module) +{ + // TODO(ahcorde) Add HeightmapShape and SetHeightmapShape + pybind11::class_ geometryModule(module, "Geometry"); + geometryModule + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("type", &sdf::Geometry::Type, + "Get the type of geometry.") + .def("set_type", &sdf::Geometry::SetType, + "Set the type of geometry.") + .def("box_shape", &sdf::Geometry::BoxShape, + pybind11::return_value_policy::reference, + "Get the box geometry, or None if the contained geometry is " + "not a box.") + .def("set_box_shape", &sdf::Geometry::SetBoxShape, + "Set the box shape.") + .def("capsule_shape", &sdf::Geometry::CapsuleShape, + pybind11::return_value_policy::reference, + "Get the capsule geometry, or None if the contained " + "geometry is not a capsule.") + .def("set_capsule_shape", &sdf::Geometry::SetCapsuleShape, + "Set the capsule shape.") + .def("cylinder_shape", &sdf::Geometry::CylinderShape, + pybind11::return_value_policy::reference, + "Get the cylinder geometry, or None if the contained " + "geometry is not a cylinder.") + .def("set_cylinder_shape", &sdf::Geometry::SetCylinderShape, + "Set the cylinder shape.") + .def("ellipsoid_shape", &sdf::Geometry::EllipsoidShape, + pybind11::return_value_policy::reference, + "Get the ellipsoid geometry, or None if the contained " + "geometry is not a ellipsoid.") + .def("set_ellipsoid_shape", &sdf::Geometry::SetEllipsoidShape, + "Set the elliposid shape.") + .def("sphere_shape", &sdf::Geometry::SphereShape, + pybind11::return_value_policy::reference, + "Get the sphere geometry, or None if the contained " + "geometry is not a sphere.") + .def("set_sphere_shape", &sdf::Geometry::SetSphereShape, + "Set the sphere shape.") + .def("plane_shape", &sdf::Geometry::PlaneShape, + pybind11::return_value_policy::reference, + "Get the plane geometry, or None if the contained " + "geometry is not a plane.") + .def("set_plane_shape", &sdf::Geometry::SetPlaneShape, + "Set the plane shape.") + .def("mesh_shape", &sdf::Geometry::MeshShape, + pybind11::return_value_policy::reference, + "Get the mesh geometry, or None if the contained " + "geometry is not a mesh.") + .def("set_mesh_shape", &sdf::Geometry::SetMeshShape, + "Set the mesh shape.") + .def("__copy__", [](const sdf::Geometry &self) { + return sdf::Geometry(self); + }) + .def("__deepcopy__", [](const sdf::Geometry &self, pybind11::dict) { + return sdf::Geometry(self); + }, "memo"_a); + + pybind11::enum_(geometryModule, "GeometryType") + .value("EMPTY", sdf::GeometryType::EMPTY) + .value("BOX", sdf::GeometryType::BOX) + .value("CYLINDER", sdf::GeometryType::CYLINDER) + .value("PLANE", sdf::GeometryType::PLANE) + .value("SPHERE", sdf::GeometryType::SPHERE) + .value("MESH", sdf::GeometryType::MESH) + .value("HEIGHTMAP", sdf::GeometryType::HEIGHTMAP) + .value("CAPSULE", sdf::GeometryType::CAPSULE) + .value("ELLIPSOID", sdf::GeometryType::ELLIPSOID); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyGeometry.hh b/python/src/sdf/pyGeometry.hh new file mode 100644 index 000000000..ccdb6f703 --- /dev/null +++ b/python/src/sdf/pyGeometry.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_GEOMETRY_HH_ +#define SDFORMAT_PYTHON_GEOMETRY_HH_ + +#include + +#include "sdf/Geometry.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Geometry +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineGeometry(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_GEOMETRY_HH_ diff --git a/python/src/sdf/pyJoint.cc b/python/src/sdf/pyJoint.cc new file mode 100644 index 000000000..02d3e3a07 --- /dev/null +++ b/python/src/sdf/pyJoint.cc @@ -0,0 +1,121 @@ +/* + * Copyright (C) 2022 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 "pyJoint.hh" + +#include +#include + +#include "sdf/Joint.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineJoint(pybind11::object module) +{ + pybind11::class_ jointModule(module, "Joint"); + jointModule + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("name", &sdf::Joint::Name, + "Get the name of joint.") + .def("set_name", &sdf::Joint::SetName, + "Set the name of the joint.") + .def("type", &sdf::Joint::Type, + "Get the joint type") + .def("set_type", &sdf::Joint::SetType, + "Set the joint type.") + .def("parent_link_name", &sdf::Joint::ParentLinkName, + "Get the name of this joint's parent link.") + .def("set_parent_link_name", &sdf::Joint::SetParentLinkName, + "Set the name of the parent link.") + .def("child_link_name", &sdf::Joint::ChildLinkName, + "Get the name of this joint's child link.") + .def("set_child_link_name", &sdf::Joint::SetChildLinkName, + "Set the name of the child link") + .def("resolve_child_link", + [](const sdf::Joint &self) + { + std::string link; + auto errors = self.ResolveChildLink(link); + return std::make_tuple(errors, link); + }, + "Resolve the name of the child link from the " + "FrameAttachedToGraph.") + .def("resolve_parent_link", + [](const sdf::Joint &self) + { + std::string link; + auto errors = self.ResolveParentLink(link); + return std::make_tuple(errors, link); + }, + "Resolve the name of the parent link from the " + "FrameAttachedToGraph. It will return the name of a link or " + "\"world\".") + .def("axis", &sdf::Joint::Axis, + pybind11::return_value_policy::reference, + "Get a joint axis.") + .def("set_axis", &sdf::Joint::SetAxis, + "Set a joint axis.") + .def("raw_pose", &sdf::Joint::RawPose, + "Get the pose of the joint. This is the pose of the joint " + "as specified in SDF ( ... ). " + "Transformations have not been applied to the return value.") + .def("set_raw_pose", &sdf::Joint::SetRawPose, + "Set the pose of the joint.") + .def("pose_relative_to", &sdf::Joint::PoseRelativeTo, + "Get the name of the coordinate frame relative to which this " + "object's pose is expressed. An empty value indicates that the frame " + "is relative to the child link frame.") + .def("set_pose_relative_to", &sdf::Joint::SetPoseRelativeTo, + "Set the name of the coordinate frame relative to which this " + "object's pose is expressed. An empty value indicates that the frame " + "is relative to the child link frame.") + .def("thread_pitch", &sdf::Joint::ThreadPitch, + "Get the thread pitch (only valid for screw joints)") + .def("set_thread_pitch", &sdf::Joint::SetThreadPitch, + "Set the thread pitch (only valid for screw joints)") + .def("semantic_pose", &sdf::Joint::SemanticPose, + "Get SemanticPose object of this object to aid in resolving " + "poses.") + .def("__copy__", [](const sdf::Joint &self) { + return sdf::Joint(self); + }) + .def("__deepcopy__", [](const sdf::Joint &self, pybind11::dict) { + return sdf::Joint(self); + }, "memo"_a); + + pybind11::enum_(jointModule, "JointType") + .value("INVALID", sdf::JointType::INVALID) + .value("BALL", sdf::JointType::BALL) + .value("CONTINUOUS", sdf::JointType::CONTINUOUS) + .value("FIXED", sdf::JointType::FIXED) + .value("GEARBOX", sdf::JointType::GEARBOX) + .value("PRISMATIC", sdf::JointType::PRISMATIC) + .value("REVOLUTE", sdf::JointType::REVOLUTE) + .value("REVOLUTE2", sdf::JointType::REVOLUTE2) + .value("SCREW", sdf::JointType::SCREW) + .value("UNIVERSAL", sdf::JointType::UNIVERSAL); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyJoint.hh b/python/src/sdf/pyJoint.hh new file mode 100644 index 000000000..86567a56a --- /dev/null +++ b/python/src/sdf/pyJoint.hh @@ -0,0 +1,42 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_JOINT_HH_ +#define SDFORMAT_PYTHON_JOINT_HH_ + +#include + +#include "sdf/Joint.hh" +#include "sdf/JointAxis.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Joint +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineJoint(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_JOINT_HH_ diff --git a/python/src/sdf/pyJointAxis.cc b/python/src/sdf/pyJointAxis.cc new file mode 100644 index 000000000..a61f2a3f2 --- /dev/null +++ b/python/src/sdf/pyJointAxis.cc @@ -0,0 +1,121 @@ +/* + * Copyright (C) 2022 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 "pyJointAxis.hh" + +#include +#include + +#include "sdf/JointAxis.hh" + + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineJointAxis(pybind11::object module) +{ + pybind11::class_(module, "JointAxis") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("xyz", &sdf::JointAxis::Xyz, + "Get the x,y,z components of the axis unit vector.") + .def("set_xyz", &sdf::JointAxis::SetXyz, + "Set the x,y,z components of the axis unit vector.") + .def("damping", &sdf::JointAxis::Damping, + "Get the physical velocity dependent viscous damping coefficient " + "of the joint axis. The default value is zero (0.0).") + .def("set_damping", &sdf::JointAxis::SetDamping, + "Set the physical velocity dependent viscous damping coefficient " + "of the joint axis.") + .def("friction", &sdf::JointAxis::Friction, + "Get the physical static friction value of the joint. The " + "default value is zero (0.0).") + .def("set_friction", &sdf::JointAxis::SetFriction, + "Set the physical static friction value of the joint.") + .def("spring_reference", &sdf::JointAxis::SpringReference, + "Get the spring reference position for this joint axis. The " + "default value is zero (0.0).") + .def("set_spring_reference", &sdf::JointAxis::SetSpringReference, + "Set the spring reference position for this joint axis.") + .def("spring_stiffness", &sdf::JointAxis::SpringStiffness, + "Get the spring stiffness for this joint axis. The default " + "value is zero (0.0).") + .def("set_spring_stiffness", &sdf::JointAxis::SetSpringStiffness, + "Set the spring stiffness for this joint axis.") + .def("lower", &sdf::JointAxis::Lower, + " Get the lower joint axis limit (radians for revolute joints, " + "meters for prismatic joints). Not valid if the joint that uses this " + "axis is continuous. The default value is -1e16.") + .def("set_lower", &sdf::JointAxis::SetLower, + "Set the lower joint axis limit (radians for revolute joints, " + "meters for prismatic joints). Not valid if the joint that uses this " + "axis is continuous.") + .def("upper", &sdf::JointAxis::Upper, + "Get the upper joint axis limit (radians for revolute joints, " + "meters for prismatic joints). Not valid if joint that uses this " + "axis is continuous. The default value is 1e16.") + .def("set_upper", &sdf::JointAxis::SetUpper, + "Set the upper joint axis limit (radians for revolute joints, " + "meters for prismatic joints). Not valid if joint that uses this " + "axis is continuous.") + .def("effort", &sdf::JointAxis::Effort, + "Get the value for enforcing the maximum absolute joint effort " + "that can be applied.") + .def("set_effort", &sdf::JointAxis::SetEffort, + "Set the value for enforcing the maximum absolute joint effort " + "that can be applied.") + .def("max_velocity", &sdf::JointAxis::MaxVelocity, + "Get the value for enforcing the maximum absolute joint velocity. " + "The default value is infinity.") + .def("set_max_velocity", &sdf::JointAxis::SetMaxVelocity, + "Set the value for enforcing the maximum absolute joint velocity.") + .def("stiffness", &sdf::JointAxis::Stiffness, + "Get the joint stop stiffness. The default value is 1e8.") + .def("set_stiffness", &sdf::JointAxis::SetStiffness, + "Set the joint stop stiffness.") + .def("dissipation", &sdf::JointAxis::Dissipation, + "Get the joint stop dissipation. The default value is 1.0.") + .def("set_dissipation", &sdf::JointAxis::SetDissipation, + "Set the joint stop dissipation.") + .def("xyz_expressed_in", &sdf::JointAxis::XyzExpressedIn, + "Get the name of the coordinate frame in which this joint axis's " + "unit vector is expressed. An empty value implies the parent (joint) " + "frame.") + .def("set_xyz_expressed_in", &sdf::JointAxis::SetXyzExpressedIn, + "Set the name of the coordinate frame in which this joint axis's " + "unit vector is expressed. An empty value implies the parent (joint) " + "frame.") + .def("resolve_xyz", &sdf::JointAxis::ResolveXyz, + pybind11::arg("_xyz"), + pybind11::arg("_resolveTo") = "", + "Express xyz unit vector of this axis in the coordinates of " + "another named frame.") + .def("__copy__", [](const sdf::JointAxis &self) { + return sdf::JointAxis(self); + }) + .def("__deepcopy__", [](const sdf::JointAxis &self, pybind11::dict) { + return sdf::JointAxis(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyJointAxis.hh b/python/src/sdf/pyJointAxis.hh new file mode 100644 index 000000000..c9d739cb1 --- /dev/null +++ b/python/src/sdf/pyJointAxis.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_JOINTAXIS_HH_ +#define SDFORMAT_PYTHON_JOINTAXIS_HH_ + +#include + +#include "sdf/JointAxis.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::JointAxis +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineJointAxis(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_JOINTAXIS_HH_ diff --git a/python/src/sdf/pyLink.cc b/python/src/sdf/pyLink.cc new file mode 100644 index 000000000..961797b8b --- /dev/null +++ b/python/src/sdf/pyLink.cc @@ -0,0 +1,186 @@ +/* + * Copyright (C) 2022 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 "pyLink.hh" + +#include + +#include + +#include "sdf/Link.hh" +#include "sdf/Visual.hh" +#include "sdf/Collision.hh" +#include "sdf/SemanticPose.hh" +#include "sdf/Light.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineLink(pybind11::object module) +{ + pybind11::class_(module, "Link") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("name", &sdf::Link::Name, + "Get the name of the link.") + .def("set_name", &sdf::Link::SetName, + "Set the name of the link.") + .def("visual_count", + &sdf::Link::VisualCount, + "Get the number of visuals.") + .def("visual_by_index", + pybind11::overload_cast(&sdf::Link::VisualByIndex), + pybind11::return_value_policy::reference, + "Get a visual based on an index.") + .def("visual_name_exists", + &sdf::Link::VisualNameExists, + "Get whether a visual name exists.") + .def("visual_by_name", + pybind11::overload_cast(&sdf::Link::VisualByName), + pybind11::return_value_policy::reference, + "Get a visual based on a name.") + .def("collision_count", + &sdf::Link::CollisionCount, + "Get the number of collisions.") + .def("collision_by_index", + pybind11::overload_cast(&sdf::Link::CollisionByIndex), + pybind11::return_value_policy::reference, + "Get a collision based on an index.") + .def("collision_name_exists", + &sdf::Link::CollisionNameExists, + "Get whether a collision name exists.") + .def("collision_by_name", + pybind11::overload_cast( + &sdf::Link::CollisionByName), + pybind11::return_value_policy::reference, + "Get a collision based on a name.") + // TODO(ahcorde): Enable light + // .def( + // "LightCount", &sdf::Link::LightCount, + // "Get the number of lights.") + // .def( + // "LightByIndex", &sdf::Link::LightByIndex, + // "Get a light based on an index.") + // .def( + // "LightNameExists", &sdf::Link::LightNameExists, + // "Get whether a light name exists.") + // .def( + // "LightByName", &sdf::Link::LightByName, + // "Get a light based on a name.") + // TODO(ahcorde): Enable sensor + // .def( + // "SensorCount", &sdf::Link::SensorCount, + // "Get the number of sensors.") + // .def( + // "SensorByIndex", &sdf::Link::SensorByIndex, + // "Get a sensor based on an index.") + // .def( + // "SensorNameExists", &sdf::Link::SensorNameExists, + // "Get whether a sensor name exists.") + // .def( + // "SensorByName", &sdf::Link::SensorByName, + // "Get a sensor based on a name.") + // TODO(ahcorde): Enable particle emitter + // .def( + // "ParticleEmitterCount", &sdf::Link::ParticleEmitterCount, + // "Get the number of particle emitters.") + // .def( + // "ParticleEmitterByIndex", &sdf::Link::ParticleEmitterByIndex, + // "Get a particle emitter based on an index.") + // .def( + // "ParticleEmitterNameExists", &sdf::Link::ParticleEmitterNameExists, + // "Get whether a particle emitter name exists.") + // .def( + // "ParticleEmitterByName", &sdf::Link::ParticleEmitterByName, + // "Get a particle emitter based on a name.") + .def("inertial", + &sdf::Link::Inertial, + "Get the inertial value for this link") + .def("set_inertial", + &sdf::Link::SetInertial, + "Set the inertial value for this link.") + .def("raw_pose", &sdf::Link::RawPose, + "Get the pose of the link object. This is the pose of the " + "link as specified in SDF") + .def("set_raw_pose", + &sdf::Link::SetRawPose, + "Set the pose of the link.") + .def("pose_relative_to", + &sdf::Link::PoseRelativeTo, + "Get the name of the coordinate frame relative to which this " + "object's pose is expressed. An empty value indicates that the frame " + "is relative to the parent model.") + .def("set_pose_relative_to", + &sdf::Link::SetPoseRelativeTo, + "Set the name of the coordinate frame relative to which this " + "object's pose is expressed. An empty value indicates that the frame " + "is relative to the parent model.") + .def("semantic_pose", + &sdf::Link::SemanticPose, + "Get SemanticPose object of this object to aid in resolving " + "poses.") + .def("enable_wind", + &sdf::Link::EnableWind, + "Check if this link should be subject to wind. " + "If true, this link should be affected by wind.") + .def("set_enable_wind", + &sdf::Link::SetEnableWind, + "Set whether this link should be subject to wind.") + .def("add_collision", + &sdf::Link::AddCollision, + "Add a collision to the link.") + .def("add_visual", + &sdf::Link::AddVisual, + "Add a visual to the link.") + // .def("AddLight", + // &sdf::Link::AddLight, + // "Add a visual to the link.") + // .def("AddSensor", + // &sdf::Link::AddSensor, + // "Add a sensor to the link.") + // .def("AddParticleEmitter", + // &sdf::Link::AddParticleEmitter, + // "Add a particle emitter to the link.") + .def("clear_collisions", + &sdf::Link::ClearCollisions, + "Remove all collisions") + .def("clear_visuals", + &sdf::Link::ClearVisuals, + "Remove all visuals") + // .def("clear_lights", + // &sdf::Link::ClearLights, + // "Remove all lights") + // .def("clear_sensors", + // &sdf::Link::ClearSensors, + // "Remove all sensors") + // .def("clear_particle_emitters", + // &sdf::Link::ClearParticleEmitters, + // "Remove all particle emitters") + .def("__copy__", [](const sdf::Link &self) { + return sdf::Link(self); + }) + .def("__deepcopy__", [](const sdf::Link &self, pybind11::dict) { + return sdf::Link(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyLink.hh b/python/src/sdf/pyLink.hh new file mode 100644 index 000000000..658af1967 --- /dev/null +++ b/python/src/sdf/pyLink.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_LINK_HH_ +#define SDFORMAT_PYTHON_LINK_HH_ + +#include + +#include "sdf/Link.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Link +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineLink(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_LINK_HH_ diff --git a/python/src/sdf/pyMaterial.cc b/python/src/sdf/pyMaterial.cc new file mode 100644 index 000000000..145b859b6 --- /dev/null +++ b/python/src/sdf/pyMaterial.cc @@ -0,0 +1,127 @@ +/* + * Copyright (C) 2022 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 "pyMaterial.hh" + +#include + +#include "sdf/Material.hh" + + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineMaterial(pybind11::object module) +{ + pybind11::class_ materialModule(module, "Material"); + materialModule + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("ambient", &sdf::Material::Ambient, + "Get the ambient color. The ambient color is " + "specified by a set of three numbers representing red/green/blue, " + "each in the range of [0,1].") + .def("set_ambient", &sdf::Material::SetAmbient, + "Set the ambient color. The ambient color is " + "specified by a set of three numbers representing red/green/blue, " + "each in the range of [0,1].") + .def("diffuse", &sdf::Material::Diffuse, + "Get the diffuse color. The diffuse color is " + "specified by a set of three numbers representing red/green/blue, " + "each in the range of [0,1].") + .def("set_diffuse", &sdf::Material::SetDiffuse, + "Set the diffuse color. The diffuse color is " + "specified by a set of three numbers representing red/green/blue, " + "each in the range of [0,1].") + .def("specular", &sdf::Material::Specular, + "Get the specular color. The specular color is " + "specified by a set of three numbers representing red/green/blue, " + "each in the range of [0,1].") + .def("set_specular", &sdf::Material::SetSpecular, + "Set the specular color. The specular color is " + "specified by a set of three numbers representing red/green/blue, " + "each in the range of [0,1].") + .def("emissive", &sdf::Material::Emissive, + "Get the emissive color. The emissive color is " + "specified by a set of three numbers representing red/green/blue, " + "each in the range of [0,1].") + .def("set_emissive", &sdf::Material::SetEmissive, + "Set the emissive color. The emissive color is " + "specified by a set of three numbers representing red/green/blue, " + "each in the range of [0,1].") + .def("render_order", &sdf::Material::RenderOrder, + "Get render order for coplanar polygons. The higher value will " + "be rendered on top of the other coplanar polygons. The default value " + "is zero.") + .def("set_render_order", &sdf::Material::SetRenderOrder, + "Set render order.") + .def("lighting", &sdf::Material::Lighting, + "Get whether dynamic lighting is enabled. The default " + "value is true.") + .def("set_lighting", &sdf::Material::SetLighting, + "Set whether dynamic lighting is enabled.") + .def("double_sided", &sdf::Material::DoubleSided, + "Get whether double sided material is enabled. The default " + "value is false.") + .def("set_double_sided", &sdf::Material::SetDoubleSided, + "Set whether double sided material is enabled.") + .def("script_uri", &sdf::Material::ScriptUri, + "Get the URI of the material script, if one has been set.") + .def("set_script_uri", &sdf::Material::SetScriptUri, + "Set the URI of the material script.") + .def("script_name", &sdf::Material::ScriptName, + "Get the name of the material script, or empty if one has not " + "been specified. The name should match an " + "script element in the script located at the ScriptUri().") + .def("set_script_name", &sdf::Material::SetScriptName, + "Set the name of the material script. The name should match an " + "script element in the script located at the ScriptUri().") + .def("shader", &sdf::Material::Shader, + "Get the type of shader.") + .def("set_shader", &sdf::Material::SetShader, + "Set the type of shader.") + .def("normal_map", &sdf::Material::NormalMap, + "Get the normal map filename. This will be an empty string if " + "a normal map has not been set.") + .def("set_normal_map", &sdf::Material::SetNormalMap, + "Set the normal map filename.") + .def("file_path", &sdf::Material::FilePath, + "The path to the file where this element was loaded from.") + .def("set_file_path", &sdf::Material::SetFilePath, + "Set the path to the file where this element was loaded from.") + .def("__copy__", [](const sdf::Material &self) { + return sdf::Material(self); + }) + .def("__deepcopy__", [](const sdf::Material &self, pybind11::dict) { + return sdf::Material(self); + }, "memo"_a); + + pybind11::enum_(materialModule, "ShaderType") + .value("PIXEL", sdf::ShaderType::PIXEL) + .value("VERTEX", sdf::ShaderType::VERTEX) + .value("NORMAL_MAP_OBJECTSPACE", sdf::ShaderType::NORMAL_MAP_OBJECTSPACE) + .value("NORMAL_MAP_TANGENTSPACE", + sdf::ShaderType::NORMAL_MAP_TANGENTSPACE); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyMaterial.hh b/python/src/sdf/pyMaterial.hh new file mode 100644 index 000000000..d98f95e02 --- /dev/null +++ b/python/src/sdf/pyMaterial.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_MATERIAL_HH_ +#define SDFORMAT_PYTHON_MATERIAL_HH_ + +#include + +#include "sdf/Material.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Material +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineMaterial(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_MATERIAL_HH_ diff --git a/python/src/sdf/pyMesh.cc b/python/src/sdf/pyMesh.cc new file mode 100644 index 000000000..85705f874 --- /dev/null +++ b/python/src/sdf/pyMesh.cc @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2022 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 "pyMesh.hh" + +#include + +#include "sdf/ParserConfig.hh" +#include "sdf/Mesh.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineMesh(pybind11::object module) +{ + pybind11::class_(module, "Mesh") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("uri", &sdf::Mesh::Uri, + "Get the mesh's URI.") + .def("set_uri", &sdf::Mesh::SetUri, + "Set the mesh's URI.") + .def("file_path", &sdf::Mesh::FilePath, + "The path to the file where this element was loaded from.") + .def("set_file_path", &sdf::Mesh::SetFilePath, + "Set the path to the file where this element was loaded from.") + .def("scale", &sdf::Mesh::Scale, + "Get the mesh's scale factor.") + .def("set_scale", &sdf::Mesh::SetScale, + "Set the mesh's scale factor.") + .def("submesh", &sdf::Mesh::Submesh, + "A submesh, contained with the mesh at the specified URI, may " + "optionally be specified. If specified, this submesh should be used " + "instead of the entire mesh.") + .def("set_submesh", &sdf::Mesh::SetSubmesh, + "Set the mesh's submesh. See Submesh() for more information.") + .def("center_submesh", &sdf::Mesh::CenterSubmesh, + "Get whether the submesh should be centered at 0,0,0. This will " + "effectively remove any transformations on the submesh before the " + "poses from parent links and models are applied. The return value is " + "only applicable if a SubMesh has been specified.") + .def("set_center_submesh", &sdf::Mesh::SetCenterSubmesh, + "Set whether the submesh should be centered. See CenterSubmesh() " + "for more information.") + .def("__copy__", [](const sdf::Mesh &self) { + return sdf::Mesh(self); + }) + .def("__deepcopy__", [](const sdf::Mesh &self, pybind11::dict) { + return sdf::Mesh(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyMesh.hh b/python/src/sdf/pyMesh.hh new file mode 100644 index 000000000..60ea35180 --- /dev/null +++ b/python/src/sdf/pyMesh.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_MESH_HH_ +#define SDFORMAT_PYTHON_MESH_HH_ + +#include + +#include "sdf/Mesh.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Mesh +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineMesh(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_MESH_HH_ diff --git a/python/src/sdf/pyModel.cc b/python/src/sdf/pyModel.cc new file mode 100644 index 000000000..28caacb76 --- /dev/null +++ b/python/src/sdf/pyModel.cc @@ -0,0 +1,210 @@ +/* + * Copyright (C) 2022 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 "pyModel.hh" + +#include +#include + +#include "sdf/Frame.hh" +#include "sdf/Joint.hh" +#include "sdf/Link.hh" +#include "sdf/Model.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineModel(pybind11::object module) +{ + pybind11::class_(module, "Model") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("validate_graphs", &sdf::Model::ValidateGraphs, + "Check that the FrameAttachedToGraph and PoseRelativeToGraph " + "are valid.") + .def("name", &sdf::Model::Name, + "Get the name of model.") + .def("set_name", &sdf::Model::SetName, + "Set the name of model.") + .def("static", &sdf::Model::Static, + "Check if this model should be static. " + "A static model is one that is not subject to physical forces (in " + "other words, it's purely kinematic instead of dynamic).") + .def("set_static", &sdf::Model::SetStatic, + "Set this model to be static or not static.") + .def("self_collide", &sdf::Model::SelfCollide, + "Check if this model should self-collide.") + .def("set_self_collide", &sdf::Model::SetSelfCollide, + "Set this model to self-collide or not self-collide.") + .def("allow_auto_disable", &sdf::Model::AllowAutoDisable, + "Check if this model should be allowed to auto-disable. " + "If auto-disable is allowed, a model that is at rest can choose to " + "not update its dynamics.") + .def("set_allow_auto_disable", &sdf::Model::SetAllowAutoDisable, + "Set this model to allow auto-disabling.") + .def("enable_wind", &sdf::Model::EnableWind, + "Check if this model should be subject to wind. " + "If true, all links in the model should be affected by the wind. This " + "can be overridden per link.") + .def("set_enable_wind", &sdf::Model::SetEnableWind, + "Set whether this model should be subject to wind.") + .def("link_count", &sdf::Model::LinkCount, + "Get the number of links that are immediate (not nested) children " + "of this Model object.") + .def("link_by_index", + pybind11::overload_cast( + &sdf::Model::LinkByIndex, pybind11::const_), + pybind11::return_value_policy::reference_internal, + "Get an immediate (not nested) child link based on an index.") + .def("link_by_name", + pybind11::overload_cast( + &sdf::Model::LinkByName, pybind11::const_), + pybind11::return_value_policy::reference_internal, + "Get an immediate (not nested) child link based on an index.") + .def("link_name_exists", &sdf::Model::LinkNameExists, + "Get whether a link name exists.") + .def("joint_count", &sdf::Model::JointCount, + "Get the number of joints that are immediate (not nested) children " + "of this Model object.") + .def("joint_by_index", + pybind11::overload_cast( + &sdf::Model::JointByIndex, pybind11::const_), + pybind11::return_value_policy::reference_internal, + "Get an immediate (not nested) child joint based on an index.") + .def("joint_by_name", + pybind11::overload_cast( + &sdf::Model::JointByName, pybind11::const_), + pybind11::return_value_policy::reference_internal, + "Get an immediate (not nested) mutable child joint based on an " + "index.") + .def("joint_name_exists", &sdf::Model::JointNameExists, + "Get whether a joint name exists.") + .def("frame_count", &sdf::Model::FrameCount, + "Get the number of explicit frames that are immediate (not nested) " + "children of this Model object.") + .def("frame_by_index", + pybind11::overload_cast( + &sdf::Model::FrameByIndex, pybind11::const_), + pybind11::return_value_policy::reference_internal, + "Get an immediate (not nested) child joint frame on an index.") + .def("frame_by_name", + pybind11::overload_cast( + &sdf::Model::FrameByName, pybind11::const_), + pybind11::return_value_policy::reference_internal, + "Get an immediate (not nested) mutable child frame based on an " + "index.") + .def("frame_name_exists", &sdf::Model::FrameNameExists, + "Get whether a frame name exists.") + .def("model_count", &sdf::Model::ModelCount, + "Get the number of explicit model that are immediate (not nested) " + "children of this Model object.") + .def("model_by_index", + pybind11::overload_cast( + &sdf::Model::ModelByIndex, pybind11::const_), + pybind11::return_value_policy::reference_internal, + "Get an immediate (not nested) child joint model on an index.") + .def("model_by_name", + pybind11::overload_cast( + &sdf::Model::ModelByName, pybind11::const_), + pybind11::return_value_policy::reference_internal, + "Get an immediate (not nested) mutable child model based on an " + "index.") + .def("model_name_exists", &sdf::Model::ModelNameExists, + "Get whether a model name exists.") + .def("raw_pose", &sdf::Model::RawPose, + "Get the pose of the model. This is the pose of the model " + "as specified in SDF ( ... ), and is " + "typically used to express the position and rotation of a model in a " + "global coordinate frame.") + .def("set_raw_pose", &sdf::Model::SetRawPose, + "Set the pose of the model.") + .def("canonical_link", &sdf::Model::CanonicalLink, + pybind11::return_value_policy::reference_internal, + "Get the model's canonical link") + .def("canonical_link_name", &sdf::Model::CanonicalLinkName, + "Get the name of the model's canonical link. An empty value " + "indicates that the first link in the model or the first link found " + "in a depth first search of nested models is the canonical link.") + .def("set_canonical_link_name", &sdf::Model::SetCanonicalLinkName, + "Set the name of the model's canonical link. An empty value " + "indicates that the first link in the model or the first link found " + "in a depth first search of nested models is the canonical link.") + .def("pose_relative_to", &sdf::Model::PoseRelativeTo, + "Get the name of the coordinate frame relative to which this " + "frame's pose is expressed. An empty value indicates that the frame " + "is expressed relative to the attached-to link.") + .def("set_pose_relative_to", &sdf::Model::SetPoseRelativeTo, + "Set the name of the coordinate frame relative to which this " + "frame's pose is expressed. An empty value indicates that the frame " + "is expressed relative to the attached-to link.") + .def("semantic_pose", &sdf::Model::SemanticPose, + "Get SemanticPose object of this object to aid in resolving " + "poses.") + .def("placement_frame_name", &sdf::Model::PlacementFrameName, + "Get the name of the placement frame of the model.") + .def("set_placement_frame_name", &sdf::Model::SetPlacementFrameName, + "Set the name of the placement frame of the model.") + .def("canonical_link_and_relative_name", &sdf::Model::CanonicalLinkAndRelativeName, + pybind11::return_value_policy::reference_internal, + "Get the model's canonical link and the nested name of the link " + "relative to the current model, delimited by \"::\".") + .def("name_exists_in_frame_attached_to_graph", + &sdf::Model::NameExistsInFrameAttachedToGraph, + "Check if a given name exists in the FrameAttachedTo graph at the " + "scope of the model..") + .def("add_link", &sdf::Model::AddLink, + "Add a link to the model.") + .def("add_joint", &sdf::Model::AddJoint, + "Add a joint to the model.") + .def("add_model", &sdf::Model::AddModel, + "Add a model to the model.") + .def("add_frame", &sdf::Model::AddFrame, + "Add a frame to the model.") + .def("clear_links", &sdf::Model::ClearLinks, + "Remove all links.") + .def("clear_joints", &sdf::Model::ClearJoints, + "Remove all joints.") + .def("clear_models", &sdf::Model::ClearModels, + "Remove all models.") + .def("clear_frames", &sdf::Model::ClearFrames, + "Remove all frames.") + .def("uri", &sdf::Model::Uri, + "Get the URI associated with this model") + .def("set_uri", &sdf::Model::SetUri, + "Set the URI associated with this model.") + // .def("plugins", + // pybind11::overload_cast<>(&sdf::Model::Plugins, pybind11::const_), + // "Get the plugins attached to this object.") + // .def("clear_plugins", &sdf::Model::ClearPlugins, + // "Remove all plugins") + // .def("add_plugin", &sdf::Model::AddPlugin, + // "Add a plugin to this object.") + .def("__copy__", [](const sdf::Model &self) { + return sdf::Model(self); + }) + .def("__deepcopy__", [](const sdf::Model &self, pybind11::dict) { + return sdf::Model(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyModel.hh b/python/src/sdf/pyModel.hh new file mode 100644 index 000000000..24a9b753f --- /dev/null +++ b/python/src/sdf/pyModel.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_MODEL_HH_ +#define SDFORMAT_PYTHON_MODEL_HH_ + +#include + +#include "sdf/Model.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Model +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineModel(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_MODEL_HH_ diff --git a/python/src/sdf/pyNoise.cc b/python/src/sdf/pyNoise.cc new file mode 100644 index 000000000..159508727 --- /dev/null +++ b/python/src/sdf/pyNoise.cc @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2022 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 "pyNoise.hh" + +#include +#include + +#include "sdf/Noise.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineNoise(pybind11::object module) +{ + pybind11::class_ geometryModule(module, "Noise"); + geometryModule + .def(pybind11::init<>()) + .def(pybind11::init()) + .def(pybind11::self == pybind11::self) + .def(pybind11::self != pybind11::self) + .def("type", &sdf::Noise::Type, + "Get the type of noise.") + .def("set_type", &sdf::Noise::SetType, + "Set the type of noise.") + .def("mean", &sdf::Noise::Mean, + "Get the mean of the Gaussian distribution " + "from which noise values are drawn. This is applicable to " + "\"gaussian*\" noise types.") + .def("set_mean", &sdf::Noise::SetMean, + "Set the mean of the Gaussian distribution " + "from which noise values are drawn. This is applicable to " + "\"gaussian*\" noise types.") + .def("std_dev", &sdf::Noise::StdDev, + "Get the StdDev of the Gaussian distribution " + "from which noise values are drawn. This is applicable to " + "\"gaussian*\" noise types.") + .def("set_std_dev", &sdf::Noise::SetStdDev, + "Set the StdDev of the Gaussian distribution " + "from which noise values are drawn. This is applicable to " + "\"gaussian*\" noise types.") + .def("bias_mean", &sdf::Noise::BiasMean, + "Get the mean of the Gaussian distribution " + "from which bias values are drawn. This is applicable to \"gaussian*\"" + "noise types.") + .def("set_bias_mean", &sdf::Noise::SetBiasMean, + "Set the mean of the Gaussian distribution " + "from which bias values are drawn. This is applicable to \"gaussian*\"" + "noise types.") + .def("bias_std_dev", &sdf::Noise::BiasStdDev, + "Get the standard deviation of the Gaussian distribution " + "from which bias values are drawn. This is applicable to \"gaussian*\"" + "noise types.") + .def("set_bias_std_dev", &sdf::Noise::SetBiasStdDev, + "Set the standard deviation of the Gaussian distribution " + "from which bias values are drawn. This is applicable to \"gaussian*\"" + "noise types.") + .def("precision", &sdf::Noise::Precision, + "For type \"gaussian_quantized\", get the precision of output " + "signals. A value of zero implies infinite precision / no " + "quantization.") + .def("set_precision", &sdf::Noise::SetPrecision, + "For type \"gaussian_quantized\", set the precision of output " + "signals. A value of zero implies infinite precision / no " + "quantization.") + .def("dynamic_bias_std_dev", &sdf::Noise::DynamicBiasStdDev, + "For type \"gaussian*\", get the standard deviation of the noise " + "used to drive a process to model slow variations in a sensor bias.") + .def("set_dynamic_bias_std_dev", &sdf::Noise::SetDynamicBiasStdDev, + "For type \"gaussian*\", set the standard deviation of the noise " + "used to drive a process to model slow variations in a sensor bias.") + .def("dynamic_bias_correlation_time", + &sdf::Noise::DynamicBiasCorrelationTime, + "For type \"gaussian*\", get the correlation time of the noise " + "used to drive a process to model slow variations in a sensor bias.") + .def("set_dynamic_bias_correlation_time", + &sdf::Noise::SetDynamicBiasCorrelationTime, + "For type \"gaussian*\", set the correlation time in seconds of " + "the noise used to drive a process to model slow variations in a " + "sensor bias.A typical value, when used, would be on the order of " + "3600 seconds (1 hour).") + .def("__copy__", [](const sdf::Noise &self) { + return sdf::Noise(self); + }) + .def("__deepcopy__", [](const sdf::Noise &self, pybind11::dict) { + return sdf::Noise(self); + }, "memo"_a); + + pybind11::enum_(geometryModule, "NoiseType") + .value("NONE", sdf::NoiseType::NONE) + .value("GAUSSIAN", sdf::NoiseType::GAUSSIAN) + .value("GAUSSIAN_QUANTIZED", sdf::NoiseType::GAUSSIAN_QUANTIZED); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyNoise.hh b/python/src/sdf/pyNoise.hh new file mode 100644 index 000000000..bd93fa05a --- /dev/null +++ b/python/src/sdf/pyNoise.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_NOISE_HH_ +#define SDFORMAT_PYTHON_NOISE_HH_ + +#include + +#include "sdf/Noise.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Noise +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineNoise(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_NOISE_HH_ diff --git a/python/src/sdf/pyParserConfig.cc b/python/src/sdf/pyParserConfig.cc new file mode 100644 index 000000000..345cd1251 --- /dev/null +++ b/python/src/sdf/pyParserConfig.cc @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2022 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 "pyParserConfig.hh" + +#include +#include +#include +#include + +#include "sdf/ParserConfig.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineParserConfig(pybind11::object module) +{ + pybind11::class_ parseConfigModule(module, "ParserConfig"); + parseConfigModule + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("global_config", + &sdf::ParserConfig::GlobalConfig, + "Mutable access to a singleton ParserConfig that serves as the global " + "ParserConfig object for all parsing operations that do not specify " + "their own ParserConfig") + .def("find_file_callback", + &sdf::ParserConfig::FindFileCallback, + "Get the find file callback function") + .def("set_find_callback", + &sdf::ParserConfig::SetFindCallback, + "Set the callback to use when libsdformat can't find a file.") + .def("uri_path_map", + &sdf::ParserConfig::URIPathMap, + "Get the URI scheme to search directories map") + .def("add_uri_path", + &sdf::ParserConfig::AddURIPath, + "Associate paths to a URI.") + .def("set_warnings_policy", + &sdf::ParserConfig::SetWarningsPolicy, + "Set the warning enforcment policy.") + .def("warnings_policy", + &sdf::ParserConfig::WarningsPolicy, + "Get the current warning enforcement policy") + .def("set_unrecognized_elements_policy", + &sdf::ParserConfig::SetUnrecognizedElementsPolicy, + "Set the policy for unrecogonized elements without an xmlns") + .def("unrecognized_elements_policy", + &sdf::ParserConfig::UnrecognizedElementsPolicy, + "Get the current unrecognized elements policy") + .def("set_deprecated_elements_policy", + &sdf::ParserConfig::SetDeprecatedElementsPolicy, + "Set the policy for deprecated elements.") + .def("reset_deprecated_elements_policy", + &sdf::ParserConfig::ResetDeprecatedElementsPolicy, + "Resets the policy for deprecated elements so that it follows " + "WarningsPolicy.") + .def("deprecated_elements_policy", + &sdf::ParserConfig::DeprecatedElementsPolicy, + "Get the current deprecated elements policy") + .def("urdf_set_preserve_fixed_joint", + &sdf::ParserConfig::URDFSetPreserveFixedJoint, + "Set the preserveFixedJoint flag.") + .def("urdf_preserve_fixed_joint", + &sdf::ParserConfig::URDFPreserveFixedJoint, + "Get the preserveFixedJoint flag value.") + .def("__copy__", [](const sdf::ParserConfig &self) { + return sdf::ParserConfig(self); + }) + .def("__deepcopy__", [](const sdf::ParserConfig &self, pybind11::dict) { + return sdf::ParserConfig(self); + }, "memo"_a); + + pybind11::enum_( + parseConfigModule, "EnforcementPolicy") + .value("ERR", sdf::EnforcementPolicy::ERR) + .value("WARN", sdf::EnforcementPolicy::WARN) + .value("LOG", sdf::EnforcementPolicy::LOG); + +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyParserConfig.hh b/python/src/sdf/pyParserConfig.hh new file mode 100644 index 000000000..db240eac9 --- /dev/null +++ b/python/src/sdf/pyParserConfig.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_PARSERCONFIG_HH_ +#define SDFORMAT_PYTHON_PARSERCONFIG_HH_ + +#include + +#include "sdf/ParserConfig.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::ParserConfig +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineParserConfig(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_PARSERCONFIG_HH_ diff --git a/python/src/sdf/pyPlane.cc b/python/src/sdf/pyPlane.cc new file mode 100644 index 000000000..ac3fef7c4 --- /dev/null +++ b/python/src/sdf/pyPlane.cc @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2022 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 "pyPlane.hh" + +#include + +#include "sdf/Plane.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void definePlane(pybind11::object module) +{ + pybind11::class_(module, "Plane") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("size", &sdf::Plane::Size, + "Get the plane size in meters.") + .def("set_size", &sdf::Plane::SetSize, + "Set the plane size in meters.") + .def("normal", &sdf::Plane::Normal, + "Get the plane normal vector. When a Plane is used as a geometry " + "for a Visual or Collision object, then the normal is specified in " + "the Visual or Collision frame, respectively.") + .def("set_normal", &sdf::Plane::SetNormal, + "Set the plane normal vector. The normal vector will be " + "normalized. See ignition::math::Vector3d Normal() for more " + "information about the normal vector, such as the frame in which it " + "is specified.") + .def( + "shape", + pybind11::overload_cast<>(&sdf::Plane::Shape, pybind11::const_), + pybind11::return_value_policy::reference, + "Get a mutable Ignition Math representation of this Plane.") + .def("__copy__", [](const sdf::Plane &self) { + return sdf::Plane(self); + }) + .def("__deepcopy__", [](const sdf::Plane &self, pybind11::dict) { + return sdf::Plane(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyPlane.hh b/python/src/sdf/pyPlane.hh new file mode 100644 index 000000000..21fd2a12d --- /dev/null +++ b/python/src/sdf/pyPlane.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_PLANE_HH_ +#define SDFORMAT_PYTHON_PLANE_HH_ + +#include + +#include "sdf/Plane.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Plane +/** + * \param[in] module a pybind11 module to add the definition to + */ +void definePlane(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_PLANE_HH_ diff --git a/python/src/sdf/pySemanticPose.cc b/python/src/sdf/pySemanticPose.cc new file mode 100644 index 000000000..8541e8077 --- /dev/null +++ b/python/src/sdf/pySemanticPose.cc @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2022 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 "pySemanticPose.hh" + +#include +#include + +#include + +#include "sdf/SemanticPose.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineSemanticPose(pybind11::object module) +{ + pybind11::class_(module, "SemanticPose") + .def(pybind11::init()) + .def("raw_pose", &sdf::SemanticPose::RawPose, + "Get the raw Pose3 transform.") + .def("relative_to", &sdf::SemanticPose::RelativeTo, + "Get the name of the coordinate frame relative to which this " + "object's pose is expressed. An empty value indicates that the frame " + "is relative to the default parent object.") + .def("resolve", &sdf::SemanticPose::Resolve, + pybind11::arg("_pose"), + pybind11::arg("_resolveTo") = "", + "Resolve pose of this object with respect to another named frame. " + "If there are any errors resolving the pose, the output will not be " + "modified.") + .def("__copy__", [](const sdf::SemanticPose &self) { + return sdf::SemanticPose(self); + }) + .def("__deepcopy__", [](const sdf::SemanticPose &self, pybind11::dict) { + return sdf::SemanticPose(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pySemanticPose.hh b/python/src/sdf/pySemanticPose.hh new file mode 100644 index 000000000..4ae96a397 --- /dev/null +++ b/python/src/sdf/pySemanticPose.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_SEMANTICPOSE_HH_ +#define SDFORMAT_PYTHON_SEMANTICPOSE_HH_ + +#include + +#include "sdf/SemanticPose.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::SemanticPose +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineSemanticPose(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_SEMANTICPOSE_HH_ diff --git a/python/src/sdf/pySphere.cc b/python/src/sdf/pySphere.cc new file mode 100644 index 000000000..f9c59c2d1 --- /dev/null +++ b/python/src/sdf/pySphere.cc @@ -0,0 +1,54 @@ +/* + * Copyright (C) 2022 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 "pySphere.hh" + +#include + +#include "sdf/Sphere.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineSphere(pybind11::object module) +{ + pybind11::class_(module, "Sphere") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("radius", &sdf::Sphere::Radius, + "Get the sphere's radius in meters.") + .def("set_radius", &sdf::Sphere::SetRadius, + "Set the sphere's radius in meters.") + .def( + "shape", + pybind11::overload_cast<>(&sdf::Sphere::Shape, pybind11::const_), + pybind11::return_value_policy::reference, + "Get a mutable Ignition Math representation of this Sphere.") + .def("__copy__", [](const sdf::Sphere &self) { + return sdf::Sphere(self); + }) + .def("__deepcopy__", [](const sdf::Sphere &self, pybind11::dict) { + return sdf::Sphere(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pySphere.hh b/python/src/sdf/pySphere.hh new file mode 100644 index 000000000..fdfb92c79 --- /dev/null +++ b/python/src/sdf/pySphere.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_SPHERE_HH_ +#define SDFORMAT_PYTHON_SPHERE_HH_ + +#include + +#include "sdf/Sphere.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Sphere +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineSphere(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_SPHERE_HH_ diff --git a/python/src/sdf/pySurface.cc b/python/src/sdf/pySurface.cc new file mode 100644 index 000000000..add480b34 --- /dev/null +++ b/python/src/sdf/pySurface.cc @@ -0,0 +1,67 @@ +/* + * Copyright (C) 2022 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 "pySurface.hh" + +#include + +#include "sdf/Surface.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineContact(pybind11::object module) +{ + pybind11::class_(module, "Contact") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("collide_bitmask", &sdf::Contact::CollideBitmask, + "Get the collide bitmask parameter.") + .def("set_collide_bitmask", &sdf::Contact::SetCollideBitmask, + "Set the collide bitmask parameter.") + .def("__copy__", [](const sdf::Contact &self) { + return sdf::Contact(self); + }) + .def("__deepcopy__", [](const sdf::Contact &self, pybind11::dict) { + return sdf::Contact(self); + }, "memo"_a); +} +///////////////////////////////////////////////// +void defineSurface(pybind11::object module) +{ + pybind11::class_(module, "Surface") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("contact", &sdf::Surface::Contact, + pybind11::return_value_policy::reference, + "Get the associated contact object") + .def("set_contact", &sdf::Surface::SetContact, + "Set the associated contact object.") + .def("__copy__", [](const sdf::Surface &self) { + return sdf::Surface(self); + }) + .def("__deepcopy__", [](const sdf::Surface &self, pybind11::dict) { + return sdf::Surface(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pySurface.hh b/python/src/sdf/pySurface.hh new file mode 100644 index 000000000..723b2e407 --- /dev/null +++ b/python/src/sdf/pySurface.hh @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_SURFACE_HH_ +#define SDFORMAT_PYTHON_SURFACE_HH_ + +#include + +#include "sdf/Surface.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Contact +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineContact(pybind11::object module); + +/// Define a pybind11 wrapper for an sdf::Surface +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineSurface(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_SURFACE_HH_ diff --git a/python/src/sdf/pyVisual.cc b/python/src/sdf/pyVisual.cc new file mode 100644 index 000000000..3cc3fe393 --- /dev/null +++ b/python/src/sdf/pyVisual.cc @@ -0,0 +1,109 @@ +/* + * Copyright (C) 2022 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 "pyVisual.hh" + +#include + +#include "sdf/Visual.hh" +#include "sdf/Geometry.hh" +#include "sdf/Material.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineVisual(pybind11::object module) +{ + pybind11::class_(module, "Visual") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("name", &sdf::Visual::Name, + "Get the name of the visual. " + "The name of the visual must be unique within the scope of a Link.") + .def("set_name", &sdf::Visual::SetName, + "Set the name of the visual. " + "The name of the visual must be unique within the scope of a Link.") + .def("cast_shadows", &sdf::Visual::CastShadows, + "Get whether the visual casts shadows") + .def("set_cast_shadows", &sdf::Visual::SetCastShadows, + "Set whether the visual casts shadows") + .def("transparency", &sdf::Visual::Transparency, + "Get the transparency value of the visual") + .def("set_transparency", &sdf::Visual::SetTransparency, + "Set the transparency value for the visual") + .def("geometry", &sdf::Visual::Geom, + pybind11::return_value_policy::reference_internal, + "Get a pointer to the visual's geometry.") + .def("set_geometry", &sdf::Visual::SetGeom, + "Set the visual's geometry") + .def("raw_pose", &sdf::Visual::RawPose, + "Get the pose of the visual object. This is the pose of the " + "collison as specified in SDF") + .def("set_raw_pose", &sdf::Visual::SetRawPose, + "Set the pose of the visual object.") + .def("pose_relative_to", &sdf::Visual::PoseRelativeTo, + "Get the name of the coordinate frame relative to which this " + "object's pose is expressed. An empty value indicates that the frame " + "is relative to the parent link.") + .def("set_pose_relative_to", &sdf::Visual::SetPoseRelativeTo, + "Set the name of the coordinate frame relative to which this " + "object's pose is expressed. An empty value indicates that the frame " + "is relative to the parent link.") + .def("semantic_pose", &sdf::Visual::SemanticPose, + "Get SemanticPose object of this object to aid in resolving " + "poses.") + .def("material", &sdf::Visual::Material, + pybind11::return_value_policy::reference_internal, + "Get a pointer to the visual's material properties. This can" + "be a nullptr if material properties have not been set.") + .def("set_material", &sdf::Visual::SetMaterial, + "Set the visual's material") + .def("visibility_flags", &sdf::Visual::VisibilityFlags, + "Get the visibility flags of a visual") + .def("set_visibility_flags", &sdf::Visual::SetVisibilityFlags, + "Set the visibility flags of a visual") + .def("set_has_laser_retro", &sdf::Visual::SetHasLaserRetro, + "Set whether the lidar reflective intensity has been specified.") + .def("has_laser_retro", &sdf::Visual::HasLaserRetro, + "Get whether the lidar reflective intensity was set was set.") + .def("laser_retro", &sdf::Visual::LaserRetro, + "Get the flidar reflective intensity.") + .def("set_laser_retro", &sdf::Visual::SetLaserRetro, + "Set the lidar reflective intensity.") + // TODO(ahcorde) Enable these methods when sdf::Plugins is converted + // .def("plugins", + // pybind11::overload_cast<>(&sdf::Visual::Plugins, pybind11::const_), + // "Get the plugins attached to this object.") + // .def("clear_plugins", &sdf::Visual::ClearPlugins, + // "Remove all plugins") + // .def("add_pPlugin", &sdf::Visual::AddPlugin, + // "Add a plugin to this object.") + .def("__copy__", [](const sdf::Visual &self) { + return sdf::Visual(self); + }) + .def("__deepcopy__", [](const sdf::Visual &self, pybind11::dict) { + return sdf::Visual(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyVisual.hh b/python/src/sdf/pyVisual.hh new file mode 100644 index 000000000..f6efe5553 --- /dev/null +++ b/python/src/sdf/pyVisual.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_VISUAL_HH_ +#define SDFORMAT_PYTHON_VISUAL_HH_ + +#include + +#include "sdf/Visual.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Visual +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineVisual(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_VISUAL_HH_ diff --git a/python/src/sdf/pyWorld.cc b/python/src/sdf/pyWorld.cc new file mode 100644 index 000000000..71b993329 --- /dev/null +++ b/python/src/sdf/pyWorld.cc @@ -0,0 +1,174 @@ +/* + * Copyright (C) 2022 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 "pyWorld.hh" + +#include +#include + +#include "sdf/Actor.hh" +#include "sdf/Atmosphere.hh" +#include "sdf/Frame.hh" +#include "sdf/Joint.hh" +#include "sdf/Light.hh" +#include "sdf/Link.hh" +#include "sdf/Model.hh" +#include "sdf/Physics.hh" +#include "sdf/World.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineWorld(pybind11::object module) +{ + // TODO(ahcorde) + // - Added Scene methods + // - Added Light methods + // - Added Physics methods + + pybind11::class_ geometryModule(module, "World"); + geometryModule + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("validate_graphs", &sdf::World::ValidateGraphs, + "Check that the FrameAttachedToGraph and PoseRelativeToGraph " + "are valid.") + .def("name", &sdf::World::Name, + "Get the name of model.") + .def("set_name", &sdf::World::SetName, + "Set the name of model.") + .def("audio_device", &sdf::World::AudioDevice, + "Get the audio device name. The audio device can be used to " + "playback audio files. A value of \"default\" or an empty string " + "indicates that the system's default audio device should be used.") + .def("set_audio_device", &sdf::World::SetAudioDevice, + "Set the audio device name. See std::string AudioDevice() const " + "for more information.") + .def("wind_linear_velocity", &sdf::World::WindLinearVelocity, + "Get the wind linear velocity in the global/world coordinate " + "frame. Units are meters per second.") + .def("set_wind_linear_velocity", &sdf::World::SetWindLinearVelocity, + "Set the wind linear velocity in the global/world coordinate " + "frame. Units are meters per second.") + .def("gravity", &sdf::World::Gravity, + "Get the acceleration due to gravity. Units are meters per " + "second squared. The default value is " + "Earth's standard gravity at sea level, which equals " + "[0, 0, -9.80665].") + .def("set_gravity", &sdf::World::SetGravity, + "Set the acceleration due to gravity. Units are meters per " + "second squared.") + .def("magnetic_field", &sdf::World::MagneticField, + "Get the magnetic vector in Tesla, expressed in " + "a coordinate frame defined by the SphericalCoordinates property. " + "A spherical coordinate can be specified in SDF using the " + " element.") + .def("set_magnetic_field", &sdf::World::SetMagneticField, + "Set the magnetic vector in Tesla, expressed in " + "a coordinate frame defined by the SphericalCoordinate. " + "A spherical coordinate can be specified in SDF using the " + " element.") + .def("spherical_coordinates", &sdf::World::SphericalCoordinates, + pybind11::return_value_policy::reference_internal, + "Get the spherical coordinates for the world origin.") + .def("set_spherical_coordinates", &sdf::World::SetSphericalCoordinates, + "Set the spherical coordinates for the world origin.") + .def("model_count", &sdf::World::ModelCount, + "Get the number of explicit models that are immediate (not nested) " + "children of this World object.") + .def("model_by_index", + pybind11::overload_cast( + &sdf::World::ModelByIndex), + pybind11::return_value_policy::reference_internal, + "Get an immediate (not nested) child joint model on an index.") + .def("model_by_name", + pybind11::overload_cast( + &sdf::World::ModelByName), + pybind11::return_value_policy::reference_internal, + "Get an immediate (not nested) mutable child model based on an " + "index.") + .def("model_name_exists", &sdf::World::ModelNameExists, + "Get whether a model name exists.") + .def("add_model", &sdf::World::AddModel, + "Add a model to the world.") + // .def("add_actor", &sdf::World::AddActor, + // "Add a actor to the world.") + // .def("add_light", &sdf::World::AddLight, + // "Add a light to the world.") + // .def("add_physics", &sdf::World::AddPhysics, + // "Add a physics object to the world.") + .def("add_frame", &sdf::World::AddFrame, + "Add a frame object to the world.") + .def("clear_models", &sdf::World::ClearModels, + "Remove all models.") + // .def("clear_actors", &sdf::World::ClearActors, + // "Remove all actors.") + // .def("clear_lights", &sdf::World::ClearLights, + // "Remove all lights.") + // .def("clear_physics", &sdf::World::ClearPhysics, + // "Remove all physics objects.") + .def("clear_frames", &sdf::World::ClearFrames, + "Remove all frames.") + .def("actor_count", &sdf::World::ActorCount, + "Get the number of actors.") + .def("frame_count", &sdf::World::FrameCount, + "Get the number of explicit frames that are immediate (not nested) " + "children of this World object.") + .def("frame_by_index", + pybind11::overload_cast( + &sdf::World::FrameByIndex), + pybind11::return_value_policy::reference_internal, + "Get an immediate (not nested) explicit frame based on an index.") + .def("frame_by_name", + pybind11::overload_cast( + &sdf::World::FrameByName), + pybind11::return_value_policy::reference_internal, + "Get an immediate (not nested) mutable child frame based on an " + "index.") + .def("frame_name_exists", &sdf::World::FrameNameExists, + "Get whether a frame name exists.") + .def("atmosphere", &sdf::World::Atmosphere, + "Get a pointer to the atmosphere model associated with this " + "world. A nullptr indicates that an atmosphere model has not been " + "set.") + .def("set_atmosphere", &sdf::World::SetAtmosphere, + "Set the atmosphere model associated with this world.") + .def("physics_count", &sdf::World::PhysicsCount, + "Get the number of physics profiles.") + // TODO(ahcorde) Enable plugins when sdf::plugins is converted + // .def("plugins", + // pybind11::overload_cast<>(&sdf::World::Plugins, pybind11::const_), + // "Get the plugins attached to this object.") + // .def("clear_plugins", &sdf::World::ClearPlugins, + // "Remove all plugins") + // .def("add_plugin", &sdf::World::AddPlugin, + // "Add a plugin to this object.") + .def("__copy__", [](const sdf::World &self) { + return sdf::World(self); + }) + .def("__deepcopy__", [](const sdf::World &self, pybind11::dict) { + return sdf::World(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyWorld.hh b/python/src/sdf/pyWorld.hh new file mode 100644 index 000000000..982e053b2 --- /dev/null +++ b/python/src/sdf/pyWorld.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 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. + */ + +#ifndef SDFORMAT_PYTHON_WORLD_HH_ +#define SDFORMAT_PYTHON_WORLD_HH_ + +#include + +#include "sdf/World.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::World +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineWorld(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_WORLD_HH_ diff --git a/python/test/_ignition_sdformattest_pybind11.cc b/python/test/_ignition_sdformattest_pybind11.cc new file mode 100644 index 000000000..b833ca3c0 --- /dev/null +++ b/python/test/_ignition_sdformattest_pybind11.cc @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2022 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 + +#include "test_config.h" + +PYBIND11_MODULE(sdformattest, m) { + m.doc() = "sdformat test Python Library."; + + m.def( + "source_file", + []() + { + return sdf::testing::SourceFile(); + }, + "Retrieve a file from the project source directory"); + + m.def( + "test_file", + []() + { + return sdf::testing::TestFile(); + }, + "Retrieve a file from the project source directory"); +} diff --git a/python/test/pyBox_TEST.py b/python/test/pyBox_TEST.py new file mode 100644 index 000000000..a305f88a0 --- /dev/null +++ b/python/test/pyBox_TEST.py @@ -0,0 +1,59 @@ +# Copyright (C) 2022 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. + +from ignition.math import Vector3d +from sdformat import Box +import unittest + +class BoxTEST(unittest.TestCase): + + def test_default_construction(self): + box = Box() + + self.assertEqual(Vector3d.ONE, box.size()) + + box.set_size(Vector3d.ZERO) + self.assertEqual(Vector3d.ZERO, box.size()) + + + def test_assignment(self): + size = Vector3d(1, 2, 3) + + box = Box() + box.set_size(size) + + box2 = box; + self.assertEqual(size, box2.size()) + + self.assertEqual(1 * 2 * 3, box2.shape().volume()) + self.assertEqual(size, box2.shape().size()) + + def test_copy_construction(self): + size = Vector3d(0.1, 0.2, 0.3) + + box = Box() + box.set_size(size) + + box2 = Box(box) + self.assertEqual(size, box2.size()) + + def test_shape(self): + box = Box() + self.assertEqual(Vector3d.ONE, box.size()) + + box.shape().set_size(Vector3d(1, 2, 3)) + self.assertEqual(Vector3d(1, 2, 3), box.size()) + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyCapsule_TEST.py b/python/test/pyCapsule_TEST.py new file mode 100644 index 000000000..31a37a18e --- /dev/null +++ b/python/test/pyCapsule_TEST.py @@ -0,0 +1,113 @@ +# Copyright (C) 2022 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. + +import copy + +from ignition.math import Vector3d, Capsuled + +import math + +from sdformat import Capsule + +import unittest + + +class CapsuleTEST(unittest.TestCase): + + def test_default_construction(self): + capsule = Capsule() + + self.assertEqual(math.pi * math.pow(0.5, 2) * (1.0 + 4./3. * 0.5), + capsule.shape().volume()) + + self.assertEqual(0.5, capsule.radius()) + self.assertEqual(1.0, capsule.length()) + + capsule.set_radius(0.5) + capsule.set_length(2.3) + + self.assertEqual(0.5, capsule.radius()) + self.assertEqual(2.3, capsule.length()) + + def test_move_construction(self): + capsule = Capsule() + capsule.set_radius(0.2) + capsule.set_length(3.0) + self.assertEqual(math.pi * math.pow(0.2, 2) * (3.0 + 4./3. * 0.2), + capsule.shape().volume()) + + capsule2 = capsule + self.assertEqual(0.2, capsule2.radius()) + self.assertEqual(3.0, capsule2.length()) + + self.assertEqual(math.pi * math.pow(0.2, 2) * (3.0 + 4./3. * 0.2), + capsule2.shape().volume()) + self.assertEqual(0.2, capsule2.shape().radius()) + self.assertEqual(3.0, capsule2.shape().length()) + + + def test_copy_construction(self): + capsule = Capsule(); + capsule.set_radius(0.2) + capsule.set_length(3.0) + + capsule2 = Capsule(capsule) + self.assertEqual(0.2, capsule2.radius()) + self.assertEqual(3.0, capsule2.length()) + + def test_copy_construction(self): + capsule = Capsule(); + capsule.set_radius(0.2) + capsule.set_length(3.0) + + capsule2 = copy.deepcopy(capsule); + self.assertEqual(0.2, capsule2.radius()) + self.assertEqual(3.0, capsule2.length()) + + + def test_assignment_after_move(self): + capsule1 = Capsule(); + capsule1.set_radius(0.2) + capsule1.set_length(3.0) + + capsule2 = Capsule(); + capsule2.set_radius(2) + capsule2.set_length(30.0) + + # This is similar to what std::swap does except it uses std::move for each + # assignment + tmp = capsule1 + capsule1 = copy.deepcopy(capsule2); + capsule2 = tmp; + + self.assertEqual(2, capsule1.radius()) + self.assertEqual(30, capsule1.length()) + + self.assertEqual(0.2, capsule2.radius()) + self.assertEqual(3.0, capsule2.length()) + + + def test_shape(self): + capsule = Capsule(); + self.assertEqual(0.5, capsule.radius()) + self.assertEqual(1.0, capsule.length()) + + capsule.shape().set_radius(0.123) + capsule.shape().set_length(0.456) + self.assertEqual(0.123, capsule.radius()) + self.assertEqual(0.456, capsule.length()) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyCollision_TEST.py b/python/test/pyCollision_TEST.py new file mode 100644 index 000000000..5fea7a180 --- /dev/null +++ b/python/test/pyCollision_TEST.py @@ -0,0 +1,133 @@ +# Copyright (C) 2022 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. + +import copy +from ignition.math import Pose3d +from sdformat import (Box, Collision, Contact, Cylinder, Error, Geometry, + Plane, Surface, Sphere) +import unittest +import math + +class CollisionTEST(unittest.TestCase): + + def test_default_construction(self): + collision = Collision() + self.assertTrue(not collision.name()) + + collision.set_name("test_collison") + self.assertEqual(collision.name(), "test_collison") + + self.assertEqual(Pose3d.ZERO, collision.raw_pose()) + self.assertTrue(not collision.pose_relative_to()) + + semanticPose = collision.semantic_pose() + self.assertEqual(collision.raw_pose(), semanticPose.raw_pose()) + self.assertTrue(not semanticPose.relative_to()) + pose = Pose3d() + # expect errors when trying to resolve pose + semanticPose.resolve(pose) + # self.assertFalse() + + collision.set_raw_pose(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi)) + self.assertEqual(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi), + collision.raw_pose()) + + collision.set_pose_relative_to("link") + self.assertEqual("link", collision.pose_relative_to()) + + semanticPose = collision.semantic_pose() + self.assertEqual(collision.raw_pose(), semanticPose.raw_pose()) + self.assertEqual("link", semanticPose.relative_to()) + pose = Pose3d() + # expect errors when trying to resolve pose + # self.assertFalse(semanticPose.resolve(pose).empty()) + + self.assertNotEqual(None, collision.geometry()) + self.assertEqual(Geometry.GeometryType.EMPTY, collision.geometry().type()) + self.assertEqual(None, collision.geometry().box_shape()) + self.assertEqual(None, collision.geometry().cylinder_shape()) + self.assertEqual(None, collision.geometry().plane_shape()) + self.assertEqual(None, collision.geometry().sphere_shape()) + + self.assertNotEqual(None, collision.surface()) + self.assertNotEqual(None, collision.surface().contact()) + + + def test_assignment(self): + collision = Collision() + collision.set_raw_pose(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi)) + + collision2 = collision + self.assertEqual(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi), + collision2.raw_pose()) + + + def test_deepcopy(self): + collision = Collision() + collision.set_raw_pose(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi)) + + collision2 = copy.deepcopy(collision) + self.assertEqual(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi), + collision2.raw_pose()) + + + def test_deepcopy_after_move(self): + collision1 = Collision() + collision1.set_raw_pose(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi)) + + collision2 = Collision() + collision2.set_raw_pose(Pose3d(-20, -30, -40, math.pi, math.pi, math.pi)) + + # This is similar to what swap + tmp = copy.deepcopy(collision1) + collision1 = collision2 + collision2 = tmp + + self.assertEqual(Pose3d(-20, -30, -40, math.pi, math.pi, math.pi), + collision1.raw_pose()) + self.assertEqual(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi), + collision2.raw_pose()) + + + def test_set_geometry(self): + collision = Collision() + self.assertTrue(not collision.name()) + + geometry = Geometry() + geometry.set_type(Geometry.GeometryType.BOX) + + collision.set_geometry(geometry) + + self.assertNotEqual(None, collision.geometry()) + self.assertEqual(Geometry.GeometryType.BOX, collision.geometry().type()) + + + def test_set_surface(self): + collision = Collision() + + surface = Surface() + self.assertNotEqual(None, surface.contact()) + contact = Contact() + contact.set_collide_bitmask(0x2) + surface.set_contact(contact) + + collision.set_surface(surface) + + self.assertNotEqual(None, collision.surface()) + self.assertNotEqual(None, collision.surface().contact()) + self.assertEqual(collision.surface().contact().collide_bitmask(), 0x2) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyCylinder_TEST.py b/python/test/pyCylinder_TEST.py new file mode 100644 index 000000000..7d56961cc --- /dev/null +++ b/python/test/pyCylinder_TEST.py @@ -0,0 +1,114 @@ +# Copyright (C) 2022 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. + +import copy + +from ignition.math import Vector3d + +import math + +from sdformat import Cylinder + +import unittest + + +class CylinderTEST(unittest.TestCase): + + def test_default_construction(self): + cylinder = Cylinder() + + self.assertEqual(math.pi * math.pow(0.5, 2) * 1.0, + cylinder.shape().volume()) + + self.assertEqual(0.5, cylinder.radius()) + self.assertEqual(1.0, cylinder.length()) + + cylinder.set_radius(0.5) + cylinder.set_length(2.3) + + self.assertEqual(0.5, cylinder.radius()) + self.assertEqual(2.3, cylinder.length()) + + def test_assignment(self): + cylinder = Cylinder() + cylinder.set_radius(0.2) + cylinder.set_length(3.0) + self.assertEqual(math.pi * math.pow(0.2, 2) * 3.0, + cylinder.shape().volume()) + + cylinder2 = cylinder + self.assertEqual(0.2, cylinder2.radius()) + self.assertEqual(3.0, cylinder2.length()) + + self.assertEqual(math.pi * math.pow(0.2, 2) * 3.0, + cylinder2.shape().volume()) + self.assertEqual(0.2, cylinder2.shape().radius()) + self.assertEqual(3.0, cylinder2.shape().length()) + + cylinder.set_radius(2.0) + cylinder.set_length(0.3) + + self.assertEqual(2.0, cylinder.radius()) + self.assertEqual(0.3, cylinder.length()) + self.assertEqual(2.0, cylinder2.radius()) + self.assertEqual(0.3, cylinder2.length()) + + + def test_copy_construction(self): + cylinder = Cylinder(); + cylinder.set_radius(0.2) + cylinder.set_length(3.0) + + cylinder2 = Cylinder(cylinder) + self.assertEqual(0.2, cylinder2.radius()) + self.assertEqual(3.0, cylinder2.length()) + + cylinder.set_radius(2.) + cylinder.set_length(0.3) + + self.assertEqual(2, cylinder.radius()) + self.assertEqual(0.3, cylinder.length()) + self.assertEqual(0.2, cylinder2.radius()) + self.assertEqual(3.0, cylinder2.length()) + + def test_deepcopy(self): + cylinder = Cylinder(); + cylinder.set_radius(0.2) + cylinder.set_length(3.0) + + cylinder2 = copy.deepcopy(cylinder); + self.assertEqual(0.2, cylinder2.radius()) + self.assertEqual(3.0, cylinder2.length()) + + cylinder.set_radius(2.) + cylinder.set_length(0.3) + + self.assertEqual(2, cylinder.radius()) + self.assertEqual(0.3, cylinder.length()) + self.assertEqual(0.2, cylinder2.radius()) + self.assertEqual(3.0, cylinder2.length()) + + def test_shape(self): + cylinder = Cylinder(); + self.assertEqual(0.5, cylinder.radius()) + self.assertEqual(1.0, cylinder.length()) + + cylinder.shape().set_radius(0.123) + cylinder.shape().set_length(0.456) + self.assertEqual(0.123, cylinder.radius()) + self.assertEqual(0.456, cylinder.length()) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyEllipsoid_TEST.py b/python/test/pyEllipsoid_TEST.py new file mode 100644 index 000000000..31726d6d3 --- /dev/null +++ b/python/test/pyEllipsoid_TEST.py @@ -0,0 +1,82 @@ +# Copyright (C) 2022 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. + +import copy +from ignition.math import Vector3d, Ellipsoidd +import math +from sdformat import Ellipsoid +import unittest + + +class BoxTEST(unittest.TestCase): + + def test_default_construction(self): + ellipsoid = Ellipsoid() + # A default ellipsoid has all three radii set to 1 + self.assertEqual(math.pi * 4. / 3., ellipsoid.shape().volume()) + self.assertEqual(Vector3d.ONE, ellipsoid.shape().radii()) + + expectedradii = Vector3d(1.0, 2.0, 3.0) + ellipsoid.set_radii(expectedradii) + self.assertEqual(expectedradii, ellipsoid.shape().radii()) + + + def test_assignment(self): + ellipsoid = Ellipsoid() + expectedradii = Vector3d(1.0, 2.0, 3.0) + ellipsoid.set_radii(expectedradii) + + ellipsoid2 = ellipsoid + self.assertEqual(expectedradii, ellipsoid2.shape().radii()) + + + def test_deepcopy(self): + ellipsoid = Ellipsoid() + expectedradii = Vector3d(1.0, 2.0, 3.0) + ellipsoid.set_radii(expectedradii) + + ellipsoid2 = copy.deepcopy(ellipsoid) + self.assertEqual(expectedradii, ellipsoid2.shape().radii()) + + + def test_deepcopy_after_assignment(self): + ellipsoid1 = Ellipsoid(); + expectedradii1 = Vector3d(1.0, 2.0, 3.0) + ellipsoid1.set_radii(expectedradii1) + + ellipsoid2 = Ellipsoid() + expectedradii2 = Vector3d(10.0, 20.0, 30.0) + ellipsoid2.set_radii(expectedradii2) + + # This is similar to what std::swap does except it uses std::move for each + # assignment + tmp = copy.deepcopy(ellipsoid1) + ellipsoid1 = ellipsoid2; + ellipsoid2 = tmp; + + self.assertEqual(expectedradii1, ellipsoid2.shape().radii()) + self.assertEqual(expectedradii2, ellipsoid1.shape().radii()) + + + def test_shape(self): + ellipsoid = Ellipsoid() + self.assertEqual(Vector3d.ONE, ellipsoid.radii()) + + expectedradii = Vector3d(1.0, 2.0, 3.0) + ellipsoid.shape().set_radii(expectedradii) + self.assertEqual(expectedradii, ellipsoid.radii()) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyError_TEST.py b/python/test/pyError_TEST.py new file mode 100644 index 000000000..380ad58ba --- /dev/null +++ b/python/test/pyError_TEST.py @@ -0,0 +1,133 @@ +# Copyright (C) 2022 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. + +import copy +from sdformat import Error +import unittest + +class ErrorColor(unittest.TestCase): + + def test_default_construction(self): + error = Error() + + self.assertEqual(error.is_valid(), False) + self.assertEqual(error.code(), Error.ErrorCode.NONE) + self.assertFalse(error.message()) + self.assertFalse(error.xml_path()) + self.assertFalse(error.file_path()) + self.assertFalse(error.line_number()) + + error.set_xml_path("/sdf/world") + self.assertTrue(error.xml_path()) + + self.assertEqual("/sdf/world", error.xml_path()) + + error.set_file_path("/tmp/test_file.sdf") + self.assertTrue(error.file_path()) + self.assertEqual("/tmp/test_file.sdf", error.file_path()) + + error.set_line_number(5) + self.assertTrue(error.line_number()) + self.assertEqual(5, error.line_number()) + + + def test_value_construction_without_file(self): + error = Error(Error.ErrorCode.FILE_READ, "Unable to read a file") + self.assertEqual(error.is_valid(), True) + self.assertEqual(error.code(), Error.ErrorCode.FILE_READ) + self.assertEqual(error.message(), "Unable to read a file") + self.assertFalse(error.xml_path()) + self.assertFalse(error.file_path()) + self.assertFalse(error.line_number()) + + + def test_deepcopy(self): + error = Error(Error.ErrorCode.FILE_READ, "Unable to read a file") + + error2 = copy.deepcopy(error) + self.assertEqual(error.is_valid(), error2.is_valid()) + self.assertEqual(error.code(), error2.code()) + self.assertEqual(error.message(), error2.message()) + self.assertEqual(error.xml_path(), error2.xml_path()) + self.assertEqual(error.file_path(), error2.file_path()) + self.assertEqual(error.line_number(), error2.line_number()) + + error.set_file_path("file.sdf") + error.set_line_number(5) + error.set_xml_path("/sdf/mode") + self.assertEqual(error.file_path(), "file.sdf") + self.assertEqual(error2.file_path(), None) + self.assertEqual(error.line_number(), 5) + self.assertEqual(error2.line_number(), None) + self.assertEqual(error.xml_path(), "/sdf/mode") + self.assertEqual(error2.xml_path(), None) + + def test_value_construction_with_file(self): + emptyfile_path = "Empty/file/path"; + error = Error( + Error.ErrorCode.FILE_READ, + "Unable to read a file", + emptyfile_path) + self.assertEqual(error.is_valid(), True) + self.assertEqual(error.code(), Error.ErrorCode.FILE_READ) + self.assertEqual(error.message(), "Unable to read a file") + self.assertFalse(error.xml_path()) + self.assertTrue(error.file_path()) + self.assertEqual(error.file_path(), emptyfile_path) + self.assertFalse(error.line_number()) + + + def test_value_construction_with_line_number(self): + emptyfile_path = "Empty/file/path"; + line_number = 10; + error = Error( + Error.ErrorCode.FILE_READ, + "Unable to read a file", + emptyfile_path, + line_number) + self.assertEqual(error.is_valid(), True) + self.assertEqual(error.code(), Error.ErrorCode.FILE_READ) + self.assertEqual(error.message(), "Unable to read a file") + self.assertFalse(error.xml_path()) + self.assertTrue(error.file_path()) + self.assertEqual(error.file_path(), emptyfile_path) + self.assertTrue(error.line_number()) + self.assertEqual(error.line_number(), line_number) + + + def test_value_construction_with_xmlpath(self): + emptyfile_path = "Empty/file/path"; + line_number = 10; + error = Error( + Error.ErrorCode.FILE_READ, + "Unable to read a file", + emptyfile_path, + line_number) + self.assertEqual(error.is_valid(), True) + self.assertEqual(error.code(), Error.ErrorCode.FILE_READ) + self.assertEqual(error.message(), "Unable to read a file") + self.assertTrue(error.file_path()) + self.assertEqual(error.file_path(), emptyfile_path) + self.assertTrue(error.line_number()) + self.assertEqual(error.line_number(), line_number) + + emptyxml_path = "/sdf/model"; + self.assertFalse(error.xml_path()) + error.set_xml_path(emptyxml_path) + self.assertTrue(error.xml_path()) + self.assertEqual(error.xml_path(), emptyxml_path) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyFrame_TEST.py b/python/test/pyFrame_TEST.py new file mode 100644 index 000000000..eba27801a --- /dev/null +++ b/python/test/pyFrame_TEST.py @@ -0,0 +1,71 @@ +# Copyright (C) 2022 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. + +import copy +from ignition.math import Pose3d +from sdformat import Frame, Error +import unittest +import math + +class FrameColor(unittest.TestCase): + + def test_default_construction(self): + frame = Frame() + self.assertFalse(frame.name()) + + frame.set_name("test_frame") + self.assertEqual(frame.name(), "test_frame") + + self.assertFalse(frame.attached_to()) + self.assertEqual(Pose3d.ZERO, frame.raw_pose()) + self.assertFalse(frame.pose_relative_to()) + + semanticPose = frame.semantic_pose() + self.assertEqual(Pose3d.ZERO, semanticPose.raw_pose()) + self.assertFalse(semanticPose.relative_to()) + pose = Pose3d() + # expect errors when trying to resolve pose + self.assertTrue(semanticPose.resolve(pose)) + + frame.set_attached_to("attachment") + self.assertEqual("attachment", frame.attached_to()) + + frame.set_raw_pose(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi)) + self.assertEqual(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi), + frame.raw_pose()) + + semanticPose = frame.semantic_pose() + self.assertEqual(frame.raw_pose(), semanticPose.raw_pose()) + self.assertEqual("attachment", semanticPose.relative_to()) + pose = Pose3d() + # expect errors when trying to resolve pose + self.assertTrue(semanticPose.resolve(pose)) + + frame.set_pose_relative_to("link") + self.assertEqual("link", frame.pose_relative_to()) + + semanticPose = frame.semantic_pose() + self.assertEqual(frame.raw_pose(), semanticPose.raw_pose()) + self.assertEqual("link", semanticPose.relative_to()) + pose = Pose3d() + # expect errors when trying to resolve pose + self.assertEqual(1, len(semanticPose.resolve(pose))) + + errors, resolveAttachedToBody = frame.resolve_attached_to_body(); + self.assertEqual(1, len(errors)) + self.assertFalse(resolveAttachedToBody) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py new file mode 100644 index 000000000..01c656d8a --- /dev/null +++ b/python/test/pyGeometry_TEST.py @@ -0,0 +1,187 @@ +# Copyright (C) 2022 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. + +import copy +from sdformat import Geometry, Box, Capsule, Cylinder, Ellipsoid, Mesh, Plane, Sphere +from ignition.math import Vector3d, Vector2d +import unittest + + +class GeometryTEST(unittest.TestCase): + + def test_default_construction(self): + geom = Geometry() + self.assertEqual(Geometry.GeometryType.EMPTY, geom.type()) + + geom.set_type(Geometry.GeometryType.BOX) + self.assertEqual(Geometry.GeometryType.BOX, geom.type()) + + geom.set_type(Geometry.GeometryType.CAPSULE) + self.assertEqual(Geometry.GeometryType.CAPSULE, geom.type()) + + geom.set_type(Geometry.GeometryType.CYLINDER) + self.assertEqual(Geometry.GeometryType.CYLINDER, geom.type()) + + geom.set_type(Geometry.GeometryType.ELLIPSOID) + self.assertEqual(Geometry.GeometryType.ELLIPSOID, geom.type()) + + geom.set_type(Geometry.GeometryType.PLANE) + self.assertEqual(Geometry.GeometryType.PLANE, geom.type()) + + geom.set_type(Geometry.GeometryType.SPHERE) + self.assertEqual(Geometry.GeometryType.SPHERE, geom.type()) + + + def test_assignment(self): + geometry = Geometry() + geometry.set_type(Geometry.GeometryType.BOX) + + geometry2 = geometry + self.assertEqual(Geometry.GeometryType.BOX, geometry2.type()) + + + def test_deepcopy_construction(self): + geometry = Geometry() + geometry.set_type(Geometry.GeometryType.BOX) + boxShape = Box() + boxShape.set_size(Vector3d(1, 2, 3)) + geometry.set_box_shape(boxShape) + + geometry2 = copy.deepcopy(geometry) + self.assertEqual(Geometry.GeometryType.BOX, geometry2.type()) + + + def test_deepcopy_after_assignment(self): + geometry1 = Geometry() + geometry1.set_type(Geometry.GeometryType.BOX) + + geometry2 = Geometry() + geometry2.set_type(Geometry.GeometryType.SPHERE) + + # This is similar to what std::swap does except it uses std::move for each + # assignment + tmp = copy.deepcopy(geometry1) + geometry1 = geometry2 + geometry2 = tmp + + self.assertEqual(Geometry.GeometryType.SPHERE, geometry1.type()) + self.assertEqual(Geometry.GeometryType.BOX, geometry2.type()) + + + def test_box(self): + geom = Geometry() + geom.set_type(Geometry.GeometryType.BOX) + + boxShape = Box() + boxShape.set_size(Vector3d(1, 2, 3)) + geom.set_box_shape(boxShape) + + self.assertEqual(Geometry.GeometryType.BOX, geom.type()) + self.assertNotEqual(None, geom.box_shape()) + self.assertEqual(Vector3d(1, 2, 3), geom.box_shape().size()) + + + def test_sphere(self): + geom = Geometry() + geom.set_type(Geometry.GeometryType.SPHERE) + + sphereShape = Sphere() + sphereShape.set_radius(0.123) + geom.set_sphere_shape(sphereShape) + + self.assertEqual(Geometry.GeometryType.SPHERE, geom.type()) + self.assertNotEqual(None, geom.sphere_shape()) + self.assertEqual(0.123, geom.sphere_shape().radius()) + + + def test_capsule(self): + geom = Geometry() + geom.set_type(Geometry.GeometryType.CAPSULE) + + capsuleShape = Capsule() + capsuleShape.set_radius(0.123) + capsuleShape.set_length(4.56) + geom.set_capsule_shape(capsuleShape) + + self.assertEqual(Geometry.GeometryType.CAPSULE, geom.type()) + self.assertNotEqual(None, geom.capsule_shape()) + self.assertEqual(0.123, geom.capsule_shape().radius()) + self.assertEqual(4.56, geom.capsule_shape().length()) + + + def test_cylinder(self): + geom = Geometry() + geom.set_type(Geometry.GeometryType.CYLINDER) + + cylinderShape = Cylinder() + cylinderShape.set_radius(0.123) + cylinderShape.set_length(4.56) + geom.set_cylinder_shape(cylinderShape) + + self.assertEqual(Geometry.GeometryType.CYLINDER, geom.type()) + self.assertNotEqual(None, geom.cylinder_shape()) + self.assertEqual(0.123, geom.cylinder_shape().radius()) + self.assertEqual(4.56, geom.cylinder_shape().length()) + + + def test_ellipsoid(self): + geom = Geometry() + geom.set_type(Geometry.GeometryType.ELLIPSOID) + + ellipsoidShape = Ellipsoid() + expectedRadii = Vector3d(1, 2, 3) + ellipsoidShape.set_radii(expectedRadii) + geom.set_ellipsoid_shape(ellipsoidShape) + + self.assertEqual(Geometry.GeometryType.ELLIPSOID, geom.type()) + self.assertNotEqual(None, geom.ellipsoid_shape()) + self.assertEqual(expectedRadii, geom.ellipsoid_shape().radii()) + + + def test_mesh(self): + geom = Geometry() + geom.set_type(Geometry.GeometryType.MESH) + + meshShape = Mesh() + meshShape.set_scale(Vector3d(1, 2, 3)) + meshShape.set_uri("banana") + meshShape.set_submesh("orange") + meshShape.set_center_submesh(True) + geom.set_mesh_shape(meshShape) + + self.assertEqual(Geometry.GeometryType.MESH, geom.type()) + self.assertNotEqual(None, geom.mesh_shape()) + self.assertEqual(Vector3d(1, 2, 3), geom.mesh_shape().scale()) + self.assertEqual("banana", geom.mesh_shape().uri()) + self.assertEqual("orange", geom.mesh_shape().submesh()) + self.assertTrue(geom.mesh_shape().center_submesh()) + + + def test_plane(self): + geom = Geometry() + geom.set_type(Geometry.GeometryType.PLANE) + + planeShape = Plane() + planeShape.set_normal(Vector3d.UNIT_X) + planeShape.set_size(Vector2d(9, 8)) + geom.set_plane_shape(planeShape) + + self.assertEqual(Geometry.GeometryType.PLANE, geom.type()) + self.assertNotEqual(None, geom.plane_shape()) + self.assertEqual(Vector3d.UNIT_X, geom.plane_shape().normal()) + self.assertEqual(Vector2d(9, 8), geom.plane_shape().size()) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyJointAxis_TEST.py b/python/test/pyJointAxis_TEST.py new file mode 100644 index 000000000..1178be05f --- /dev/null +++ b/python/test/pyJointAxis_TEST.py @@ -0,0 +1,107 @@ +# Copyright (C) 2022 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. + +import copy +from ignition.math import Pose3d, Vector3d +from sdformat import JointAxis, Error +import math +import unittest + + +class JointAxisTEST(unittest.TestCase): + + def test_default_construction(self): + axis = JointAxis() + self.assertEqual(Vector3d.UNIT_Z, axis.xyz()) + self.assertFalse(axis.xyz_expressed_in()) + self.assertAlmostEqual(0.0, axis.damping()) + self.assertAlmostEqual(0.0, axis.friction()) + self.assertAlmostEqual(0.0, axis.spring_reference()) + self.assertAlmostEqual(0.0, axis.spring_stiffness()) + self.assertAlmostEqual(-1e16, axis.lower()) + self.assertAlmostEqual(1e16, axis.upper()) + self.assertAlmostEqual(math.inf, axis.effort()) + self.assertAlmostEqual(math.inf, axis.max_velocity()) + self.assertAlmostEqual(1e8, axis.stiffness()) + self.assertAlmostEqual(1.0, axis.dissipation()) + + errors = axis.set_xyz(Vector3d(0, 1, 0)) + self.assertEqual(0, len(errors)) + self.assertEqual(Vector3d.UNIT_Y, axis.xyz()) + + axis.set_xyz_expressed_in("__model__") + self.assertEqual("__model__", axis.xyz_expressed_in()) + + # expect errors when trying to resolve axis without graph + vec3 = Vector3d() + self.assertEqual(1, len(axis.resolve_xyz(vec3))) + + axis.set_damping(0.2) + self.assertAlmostEqual(0.2, axis.damping()) + + axis.set_friction(1.3) + self.assertAlmostEqual(1.3, axis.friction()) + + axis.set_spring_reference(2.4) + self.assertAlmostEqual(2.4, axis.spring_reference()) + + axis.set_spring_stiffness(-1.2) + self.assertAlmostEqual(-1.2, axis.spring_stiffness()) + + axis.set_lower(-10.8) + self.assertAlmostEqual(-10.8, axis.lower()) + + axis.set_upper(123.4) + self.assertAlmostEqual(123.4, axis.upper()) + + axis.set_effort(3.2) + self.assertAlmostEqual(3.2, axis.effort()) + + axis.set_max_velocity(54.2) + self.assertAlmostEqual(54.2, axis.max_velocity()) + + axis.set_stiffness(1e2) + self.assertAlmostEqual(1e2, axis.stiffness()) + + axis.set_dissipation(1.5) + self.assertAlmostEqual(1.5, axis.dissipation()) + + + def test_copy_construction(self): + jointAxis = JointAxis() + self.assertEqual(0, len(jointAxis.set_xyz(Vector3d(0, 1, 0)))) + + jointAxisCopy = JointAxis(jointAxis) + self.assertEqual(jointAxis.xyz(), jointAxisCopy.xyz()) + + + def test_copy_construction(self): + jointAxis = JointAxis() + self.assertEqual(0, len(jointAxis.set_xyz(Vector3d(0, 1, 0)))) + + jointAxisCopy = copy.deepcopy(jointAxis) + self.assertEqual(jointAxis.xyz(), jointAxisCopy.xyz()) + + + def test_zero_norm_vector_returns_error(self): + axis = JointAxis() + self.assertEqual(0, len(axis.set_xyz(Vector3d(1.0, 0, 0)))) + + errors = axis.set_xyz(Vector3d.ZERO) + self.assertTrue(errors) + self.assertEqual(errors[0].message(), "The norm of the xyz vector cannot be zero") + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyJoint_TEST.py b/python/test/pyJoint_TEST.py new file mode 100644 index 000000000..0a40964f1 --- /dev/null +++ b/python/test/pyJoint_TEST.py @@ -0,0 +1,163 @@ +# Copyright (C) 2022 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. + +import copy +from ignition.math import Pose3d, Vector3d +from sdformat import Joint, JointAxis, Error, SemanticPose +import math +import unittest + +class JointTEST(unittest.TestCase): + + def test_default_construction(self): + joint = Joint() + self.assertFalse(joint.name()) + self.assertEqual(Joint.JointType.INVALID, joint.type()) + self.assertFalse(joint.parent_link_name()) + self.assertFalse(joint.child_link_name()) + self.assertEqual(Pose3d.ZERO, joint.raw_pose()) + self.assertFalse(joint.pose_relative_to()) + + semanticPose = joint.semantic_pose() + self.assertEqual(Pose3d.ZERO, semanticPose.raw_pose()) + self.assertFalse(semanticPose.relative_to()) + pose = Pose3d() + # expect errors when trying to resolve pose without graph + self.assertTrue(semanticPose.resolve(pose)) + + joint.set_raw_pose(Pose3d(-1, -2, -3, math.pi, math.pi, 0)) + self.assertEqual(Pose3d(-1, -2, -3, math.pi, math.pi, 0), + joint.raw_pose()) + + joint.set_pose_relative_to("link") + self.assertEqual("link", joint.pose_relative_to()) + + semanticPose = joint.semantic_pose() + self.assertEqual(joint.raw_pose(), semanticPose.raw_pose()) + self.assertEqual("link", semanticPose.relative_to()) + pose = Pose3d() + # expect errors when trying to resolve pose without graph + self.assertTrue(semanticPose.resolve(pose)) + + joint.set_name("test_joint") + self.assertEqual("test_joint", joint.name()) + + joint.set_parent_link_name("parent") + self.assertEqual("parent", joint.parent_link_name()) + + joint.set_child_link_name("child") + self.assertEqual("child", joint.child_link_name()) + + errors, resolveChildLink = joint.resolve_child_link() + self.assertEqual(1, len(errors)) + self.assertFalse(resolveChildLink) + errors, resolveParentLink = joint.resolve_parent_link() + self.assertEqual(1, len(errors)) + self.assertFalse(resolveParentLink) + + joint.set_type(Joint.JointType.BALL) + self.assertEqual(Joint.JointType.BALL, joint.type()) + joint.set_type(Joint.JointType.CONTINUOUS) + self.assertEqual(Joint.JointType.CONTINUOUS, joint.type()) + joint.set_type(Joint.JointType.GEARBOX) + self.assertEqual(Joint.JointType.GEARBOX, joint.type()) + joint.set_type(Joint.JointType.PRISMATIC) + self.assertEqual(Joint.JointType.PRISMATIC, joint.type()) + joint.set_type(Joint.JointType.REVOLUTE) + self.assertEqual(Joint.JointType.REVOLUTE, joint.type()) + joint.set_type(Joint.JointType.REVOLUTE2) + self.assertEqual(Joint.JointType.REVOLUTE2, joint.type()) + joint.set_type(Joint.JointType.SCREW) + self.assertEqual(Joint.JointType.SCREW, joint.type()) + joint.set_type(Joint.JointType.UNIVERSAL) + self.assertEqual(Joint.JointType.UNIVERSAL, joint.type()) + + self.assertEqual(None, joint.axis(0)) + self.assertEqual(None, joint.axis(1)) + axis = JointAxis() + self.assertEqual(0, len(axis.set_xyz(Vector3d(1, 0, 0)))) + joint.set_axis(0, axis) + axis1 = JointAxis() + self.assertEqual(0, len(axis1.set_xyz(Vector3d(0, 1, 0)))) + joint.set_axis(1, axis1) + self.assertNotEqual(None, joint.axis(0)) + self.assertNotEqual(None, joint.axis(1)) + self.assertEqual(axis.xyz(), joint.axis(0).xyz()) + self.assertEqual(axis1.xyz(), joint.axis(1).xyz()) + + self.assertAlmostEqual(1.0, joint.thread_pitch()) + threadPitch = 0.1 + joint.set_thread_pitch(threadPitch) + self.assertAlmostEqual(threadPitch, joint.thread_pitch()) + + # TODO(ahcorde): Add sensor when sdf::sensors class is converted + # self.assertEqual(0u, joint.SensorCount()) + # self.assertEqual(None, joint.SensorByIndex(0)) + # self.assertEqual(None, joint.SensorByIndex(1)) + # self.assertEqual(None, joint.SensorByName("empty")) + # self.assertFalse(joint.SensorNameExists("")) + # self.assertFalse(joint.SensorNameExists("default")) + + + def test_copy_construction(self): + joint = Joint() + joint.set_name("test_joint") + axis = JointAxis() + self.assertEqual(0, len(axis.set_xyz(Vector3d(1, 0, 0)))) + joint.set_axis(0, axis) + axis1 = JointAxis() + self.assertEqual(0, len(axis1.set_xyz(Vector3d(0, 1, 0)))) + joint.set_axis(1, axis1) + + joint2 = Joint(joint) + + self.assertEqual("test_joint", joint.name()) + self.assertIsNotNone(joint.axis(0)) + self.assertIsNotNone(joint.axis(1)) + self.assertEqual(axis.xyz(), joint.axis(0).xyz()) + self.assertEqual(axis1.xyz(), joint.axis(1).xyz()) + + self.assertEqual("test_joint", joint2.name()) + self.assertIsNotNone(joint2.axis(0)) + self.assertIsNotNone(joint2.axis(1)) + self.assertEqual(axis.xyz(), joint2.axis(0).xyz()) + self.assertEqual(axis1.xyz(), joint2.axis(1).xyz()) + + + def test_deepcopy(self): + joint = Joint() + joint.set_name("test_joint") + axis = JointAxis() + self.assertEqual(0, len(axis.set_xyz(Vector3d(1, 0, 0)))) + joint.set_axis(0, axis) + axis1 = JointAxis() + self.assertEqual(0, len(axis1.set_xyz(Vector3d(0, 1, 0)))) + joint.set_axis(1, axis1) + + joint2 = copy.deepcopy(joint) + + self.assertEqual("test_joint", joint.name()) + self.assertIsNotNone(joint.axis(0)) + self.assertIsNotNone(joint.axis(1)) + self.assertEqual(axis.xyz(), joint.axis(0).xyz()) + self.assertEqual(axis1.xyz(), joint.axis(1).xyz()) + + self.assertEqual("test_joint", joint2.name()) + self.assertIsNotNone(joint2.axis(0)) + self.assertIsNotNone(joint2.axis(1)) + self.assertEqual(axis.xyz(), joint2.axis(0).xyz()) + self.assertEqual(axis1.xyz(), joint2.axis(1).xyz()) + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyLink_TEST.py b/python/test/pyLink_TEST.py new file mode 100644 index 000000000..99925edc1 --- /dev/null +++ b/python/test/pyLink_TEST.py @@ -0,0 +1,375 @@ +# Copyright (C) 2022 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. + +import copy +from ignition.math import Pose3d, Inertiald, MassMatrix3d, Vector3d +from sdformat import (Collision, Link, Visual) +import unittest +import math + +class LinkTEST(unittest.TestCase): + + def test_default_construction(self): + + link = Link() + self.assertTrue(not link.name()) + + link.set_name("test_link") + self.assertEqual("test_link", link.name()) + + self.assertEqual(0, link.visual_count()) + self.assertEqual(None, link.visual_by_index(0)) + self.assertEqual(None, link.visual_by_index(1)) + self.assertFalse(link.visual_name_exists("")) + self.assertFalse(link.visual_name_exists("default")) + + # self.assertEqual(0, link.light_count()) + # self.assertEqual(None, link.light_by_index(0)) + # self.assertEqual(None, link.light_by_index(1)) + # self.assertFalse(link.light_name_exists("")) + # self.assertFalse(link.light_name_exists("default")) + # self.assertEqual(None, link.light_by_name("no_such_light")) + + # self.assertEqual(0, link.particle_emitter_count()) + # self.assertEqual(None, link.ParticleEmitterByIndex(0)) + # self.assertEqual(None, link.ParticleEmitterByIndex(1)) + # self.assertFalse(link.particle_emitter_name_exists("")) + # self.assertFalse(link.particle_emitter_name_exists("default")) + # self.assertEqual(None, link.ParticleEmitterByName("no_such_emitter")) + + self.assertFalse(link.enable_wind()) + link.set_enable_wind(True) + self.assertTrue(link.enable_wind()) + + # self.assertEqual(0, link.sensor_count()) + # self.assertEqual(None, link.sensor_by_index(0)) + # self.assertEqual(None, link.sensor_by_index(1)) + # self.assertEqual(None, link.sensor_by_name("empty")) + # self.assertFalse(link.sensor_name_exists("")) + # self.assertFalse(link.sensor_name_exists("default")) + + self.assertEqual(Pose3d.ZERO, link.raw_pose()) + self.assertFalse(link.pose_relative_to()) + + semantic_pose = link.semantic_pose() + self.assertEqual(Pose3d.ZERO, semantic_pose.raw_pose()) + self.assertFalse(semantic_pose.relative_to()) + pose = Pose3d() + # expect errors when trying to resolve pose + self.assertFalse(not semantic_pose.resolve(pose)) + + link.set_raw_pose(Pose3d(10, 20, 30, 0, math.pi, 0)) + self.assertEqual(Pose3d(10, 20, 30, 0, math.pi, 0), link.raw_pose()) + + link.set_pose_relative_to("model") + self.assertEqual("model", link.pose_relative_to()) + + semantic_pose = link.semantic_pose() + self.assertEqual(link.raw_pose(), semantic_pose.raw_pose()) + self.assertEqual("model", semantic_pose.relative_to()) + pose = Pose3d() + # expect errors when trying to resolve pose + self.assertFalse(not semantic_pose.resolve(pose)) + + # Get the default inertial + inertial = link.inertial() + self.assertAlmostEqual(1.0, inertial.mass_matrix().mass()) + self.assertAlmostEqual(1.0, inertial.mass_matrix().diagonal_moments().x()) + self.assertAlmostEqual(1.0, inertial.mass_matrix().diagonal_moments().y()) + self.assertAlmostEqual(1.0, inertial.mass_matrix().diagonal_moments().z()) + self.assertAlmostEqual(0.0, inertial.mass_matrix().off_diagonal_moments().x()) + self.assertAlmostEqual(0.0, inertial.mass_matrix().off_diagonal_moments().y()) + self.assertAlmostEqual(0.0, inertial.mass_matrix().off_diagonal_moments().z()) + self.assertTrue(inertial.mass_matrix().is_valid()) + + self.assertEqual(0, link.collision_count()) + self.assertEqual(None, link.collision_by_index(0)) + self.assertEqual(None, link.collision_by_index(1)) + self.assertFalse(link.collision_name_exists("")) + self.assertFalse(link.collision_name_exists("default")) + + inertial2 = Inertiald( + MassMatrix3d(2.3, + Vector3d(1.4, 2.3, 3.2), + Vector3d(0.1, 0.2, 0.3)), + Pose3d(1, 2, 3, 0, 0, 0)) + + self.assertTrue(link.set_inertial(inertial2)) + + inertial = link.inertial() + self.assertAlmostEqual(2.3, inertial2.mass_matrix().mass()) + self.assertAlmostEqual(2.3, inertial.mass_matrix().mass()) + self.assertAlmostEqual(1.4, inertial.mass_matrix().diagonal_moments().x()) + self.assertAlmostEqual(2.3, inertial.mass_matrix().diagonal_moments().y()) + self.assertAlmostEqual(3.2, inertial.mass_matrix().diagonal_moments().z()) + self.assertAlmostEqual(0.1, inertial.mass_matrix().off_diagonal_moments().x()) + self.assertAlmostEqual(0.2, inertial.mass_matrix().off_diagonal_moments().y()) + self.assertAlmostEqual(0.3, inertial.mass_matrix().off_diagonal_moments().z()) + self.assertTrue(inertial.mass_matrix().is_valid()) + + + def test_copy_construction(self): + link = Link() + link.set_name("test_link") + + link2 = Link(link) + self.assertEqual("test_link", link2.name()) + + + def test_deepcopy(self): + link = Link() + link.set_name("test_link") + + link2 = copy.deepcopy(link) + self.assertEqual("test_link", link2.name()) + + + def test_invalid_inertial(self): + link = Link() + self.assertFalse(link.name()) + + invalidInertial = Inertiald( + MassMatrix3d(2.3, Vector3d(0.1, 0.2, 0.3), + Vector3d(1.2, 2.3, 3.4)), + Pose3d(1, 2, 3, 0, 0, 0)) + + self.assertFalse(link.set_inertial(invalidInertial)) + + inertial = link.inertial() + self.assertAlmostEqual(2.3, inertial.mass_matrix().mass()) + self.assertAlmostEqual(0.1, inertial.mass_matrix().diagonal_moments().x()) + self.assertAlmostEqual(0.2, inertial.mass_matrix().diagonal_moments().y()) + self.assertAlmostEqual(0.3, inertial.mass_matrix().diagonal_moments().z()) + self.assertAlmostEqual(1.2, inertial.mass_matrix().off_diagonal_moments().x()) + self.assertAlmostEqual(2.3, inertial.mass_matrix().off_diagonal_moments().y()) + self.assertAlmostEqual(3.4, inertial.mass_matrix().off_diagonal_moments().z()) + self.assertFalse(link.inertial().mass_matrix().is_valid()) + + + def test_add_collision(self): + link = Link() + self.assertEqual(0, link.collision_count()) + + collision = Collision() + collision.set_name("collision1") + self.assertTrue(link.add_collision(collision)) + self.assertEqual(1, link.collision_count()) + self.assertFalse(link.add_collision(collision)) + self.assertEqual(1, link.collision_count()) + + link.clear_collisions() + self.assertEqual(0, link.collision_count()) + + self.assertTrue(link.add_collision(collision)) + self.assertEqual(1, link.collision_count()) + collisionFromLink = link.collision_by_index(0) + self.assertNotEqual(None, collisionFromLink) + self.assertEqual(collisionFromLink.name(), collision.name()) + + + def test_add_visual(self): + link = Link() + self.assertEqual(0, link.visual_count()) + + visual = Visual() + visual.set_name("visual1") + self.assertTrue(link.add_visual(visual)) + self.assertEqual(1, link.visual_count()) + self.assertFalse(link.add_visual(visual)) + self.assertEqual(1, link.visual_count()) + + link.clear_visuals() + self.assertEqual(0, link.visual_count()) + + self.assertTrue(link.add_visual(visual)) + self.assertEqual(1, link.visual_count()) + visualFromLink = link.visual_by_index(0) + self.assertNotEqual(None, visualFromLink) + self.assertEqual(visualFromLink.name(), visual.name()) + + + # ///////////////////////////////////////////////// + # TEST(DOMLink, AddLight) + # { + # sdf::Link link + # self.assertEqual(0, link.light_count()) + # + # sdf::Light light + # light.set_name("light1") + # self.assertTrue(link.AddLight(light)) + # self.assertEqual(1, link.light_count()) + # self.assertFalse(link.AddLight(light)) + # self.assertEqual(1, link.light_count()) + # + # link.ClearLights() + # self.assertEqual(0, link.light_count()) + # + # self.assertTrue(link.AddLight(light)) + # self.assertEqual(1, link.light_count()) + # const sdf::Light *lightFromLink = link.light_by_index(0) + # self.assertNotEqual(None, lightFromLink) + # self.assertEqual(lightFromLink.name(), light.name()) + # ) + # + # ///////////////////////////////////////////////// + # TEST(DOMLink, AddSensor) + # { + # sdf::Link link + # self.assertEqual(0, link.sensor_count()) + # + # sdf::Sensor sensor + # sensor.set_name("sensor1") + # self.assertTrue(link.AddSensor(sensor)) + # self.assertEqual(1, link.sensor_count()) + # self.assertFalse(link.AddSensor(sensor)) + # self.assertEqual(1, link.sensor_count()) + # + # link.ClearSensors() + # self.assertEqual(0, link.sensor_count()) + # + # self.assertTrue(link.AddSensor(sensor)) + # self.assertEqual(1, link.sensor_count()) + # const sdf::Sensor *sensorFromLink = link.sensor_by_index(0) + # self.assertNotEqual(None, sensorFromLink) + # self.assertEqual(sensorFromLink.name(), sensor.name()) + # ) + + + def test_mutable_by_index(self): + link = Link() + link.set_name("my-name") + + visual = Visual() + visual.set_name("visual1") + self.assertTrue(link.add_visual(visual)) + + collision = Collision() + collision.set_name("collision1") + self.assertTrue(link.add_collision(collision)) + + # sdf::Light light + # light.set_name("light1") + # self.assertTrue(link.AddLight(light)) + # + # sdf::Sensor sensor + # sensor.set_name("sensor1") + # self.assertTrue(link.AddSensor(sensor)) + # + # sdf::ParticleEmitter pe + # pe.set_name("pe1") + # self.assertTrue(link.AddParticleEmitter(pe)) + + # Modify the visual + v = link.visual_by_index(0) + self.assertNotEqual(None, v) + self.assertEqual("visual1", v.name()) + v.set_name("visual2") + self.assertEqual("visual2", link.visual_by_index(0).name()) + + # Modify the collision + c = link.collision_by_index(0) + self.assertNotEqual(None, c) + self.assertEqual("collision1", c.name()) + c.set_name("collision2") + self.assertEqual("collision2", link.collision_by_index(0).name()) + + # # Modify the light + # sdf::Light *l = link.light_by_index(0) + # self.assertNotEqual(None, l) + # self.assertEqual("light1", l.name()) + # l.set_name("light2") + # self.assertEqual("light2", link.light_by_index(0).name()) + # + # # Modify the sensor + # sdf::Sensor *s = link.sensor_by_index(0) + # self.assertNotEqual(None, s) + # self.assertEqual("sensor1", s.name()) + # s.set_name("sensor2") + # self.assertEqual("sensor2", link.sensor_by_index(0).name()) + # + # # Modify the particle emitter + # sdf::ParticleEmitter *p = link.ParticleEmitterByIndex(0) + # self.assertNotEqual(None, p) + # self.assertEqual("pe1", p.name()) + # p.set_name("pe2") + # self.assertEqual("pe2", link.ParticleEmitterByIndex(0).name()) + + + def test_mutable_by_name(self): + link = Link() + link.set_name("my-name") + + visual = Visual() + visual.set_name("visual1") + self.assertTrue(link.add_visual(visual)) + + collision = Collision() + collision.set_name("collision1") + self.assertTrue(link.add_collision(collision)) + + # sdf::Light light + # light.set_name("light1") + # self.assertTrue(link.AddLight(light)) + # + # sdf::Sensor sensor + # sensor.set_name("sensor1") + # self.assertTrue(link.AddSensor(sensor)) + # + # sdf::ParticleEmitter pe + # pe.set_name("pe1") + # self.assertTrue(link.AddParticleEmitter(pe)) + + # Modify the visual + v = link.visual_by_name("visual1") + self.assertNotEqual(None, v) + self.assertEqual("visual1", v.name()) + v.set_name("visual2") + self.assertFalse(link.visual_name_exists("visual1")) + self.assertTrue(link.visual_name_exists("visual2")) + + # Modify the collision + c = link.collision_by_name("collision1") + self.assertNotEqual(None, c) + self.assertEqual("collision1", c.name()) + c.set_name("collision2") + self.assertFalse(link.collision_name_exists("collision1")) + self.assertTrue(link.collision_name_exists("collision2")) + + # // Modify the light + # sdf::Light *l = link.light_by_name("light1") + # self.assertNotEqual(None, l) + # self.assertEqual("light1", l.name()) + # l.set_name("light2") + # self.assertFalse(link.light_name_exists("light1")) + # self.assertTrue(link.light_name_exists("light2")) + # + # // Modify the sensor + # sdf::Sensor *s = link.sensor_by_name("sensor1") + # self.assertNotEqual(None, s) + # self.assertEqual("sensor1", s.name()) + # s.set_name("sensor2") + # self.assertFalse(link.sensor_name_exists("sensor1")) + # self.assertTrue(link.sensor_name_exists("sensor2")) + # + # // Modify the particle emitter + # sdf::ParticleEmitter *p = link.ParticleEmitterByName("pe1") + # self.assertNotEqual(None, p) + # self.assertEqual("pe1", p.name()) + # p.set_name("pe2") + # self.assertFalse(link.particle_emitter_name_exists("pe1")) + # self.assertTrue(link.particle_emitter_name_exists("pe2")) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyMaterial_TEST.py b/python/test/pyMaterial_TEST.py new file mode 100644 index 000000000..b4a684f59 --- /dev/null +++ b/python/test/pyMaterial_TEST.py @@ -0,0 +1,289 @@ +# Copyright (C) 2022 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. + +import copy +from sdformat import Material +from ignition.math import Color +import unittest + + +class MaterialTEST(unittest.TestCase): + + def test_default_construction(self): + material = Material() + self.assertEqual(Color(0, 0, 0, 1), material.ambient()) + self.assertEqual(Color(0, 0, 0, 1), material.diffuse()) + self.assertEqual(Color(0, 0, 0, 1), material.specular()) + self.assertEqual(Color(0, 0, 0, 1), material.emissive()) + self.assertTrue(material.lighting()) + self.assertEqual(0, material.render_order()) + self.assertFalse(material.double_sided()) + self.assertEqual("", material.script_uri()) + self.assertEqual("", material.script_name()) + self.assertEqual(Material.ShaderType.PIXEL, material.shader()) + self.assertEqual("", material.normal_map()) + # TODO(ahcorde) Add Pbr python interface + # self.assertEqual(nullptr, material.PbrMaterial()) + self.assertEqual("", material.file_path()) + + + def test_assignment(self): + material = Material() + material.set_ambient(Color(0.1, 0.2, 0.3, 0.5)) + material.set_diffuse(Color(0.2, 0.3, 0.4, 0.6)) + material.set_specular(Color(0.3, 0.4, 0.5, 0.7)) + material.set_emissive(Color(0.4, 0.5, 0.6, 0.8)) + material.set_lighting(False) + material.set_render_order(2) + material.set_double_sided(True) + material.set_script_uri("banana") + material.set_script_name("orange") + material.set_shader(Material.ShaderType.VERTEX) + material.set_normal_map("blueberry") + material.set_file_path("/tmp/path") + + material2 = material + self.assertEqual(Color(0.1, 0.2, 0.3, 0.5), material2.ambient()) + self.assertEqual(Color(0.2, 0.3, 0.4, 0.6), material2.diffuse()) + self.assertEqual(Color(0.3, 0.4, 0.5, 0.7), material2.specular()) + self.assertEqual(Color(0.4, 0.5, 0.6, 0.8), material2.emissive()) + self.assertFalse(material2.lighting()) + self.assertTrue(material2.double_sided()) + self.assertEqual(2.0, material2.render_order()) + self.assertEqual("banana", material2.script_uri()) + self.assertEqual("orange", material2.script_name()) + self.assertEqual(Material.ShaderType.VERTEX, material2.shader()) + self.assertEqual("blueberry", material2.normal_map()) + # TODO(ahcorde) Add Pbr python interface + # self.assertEqual(nullptr, material2.PbrMaterial()) + self.assertEqual("/tmp/path", material2.file_path()) + + material.set_ambient(Color(0.3, 0.1, 0.2, 0.3)) + material.set_diffuse(Color(0.4, 0.4, 0.5, 0.6)) + material.set_specular(Color(0.5, 0.6, 0.7, 0.8)) + material.set_emissive(Color(0.6, 0.7, 0.8, 0.9)) + material.set_lighting(True) + material.set_render_order(2) + material.set_double_sided(False) + material.set_script_uri("apple") + material.set_script_name("melon") + material.set_shader(Material.ShaderType.VERTEX) + material.set_normal_map("banana") + material.set_file_path("/tmp/foo") + + self.assertEqual(Color(0.3, 0.1, 0.2, 0.3), material2.ambient()) + self.assertEqual(Color(0.4, 0.4, 0.5, 0.6), material2.diffuse()) + self.assertEqual(Color(0.5, 0.6, 0.7, 0.8), material2.specular()) + self.assertEqual(Color(0.6, 0.7, 0.8, 0.9), material2.emissive()) + self.assertTrue(material2.lighting()) + self.assertFalse(material2.double_sided()) + self.assertEqual(2, material2.render_order()) + self.assertEqual("apple", material2.script_uri()) + self.assertEqual("melon", material2.script_name()) + self.assertEqual(Material.ShaderType.VERTEX, material2.shader()) + self.assertEqual("banana", material2.normal_map()) + # TODO(ahcorde) Add Pbr python interface + # self.assertEqual(nullptr, material2.PbrMaterial()) + self.assertEqual("/tmp/foo", material2.file_path()) + + def test_copy_construction(self): + material = Material() + material.set_ambient(Color(0.1, 0.2, 0.3, 0.5)) + material.set_diffuse(Color(0.2, 0.3, 0.4, 0.6)) + material.set_specular(Color(0.3, 0.4, 0.5, 0.7)) + material.set_emissive(Color(0.4, 0.5, 0.6, 0.8)) + material.set_lighting(False) + material.set_render_order(4) + material.set_double_sided(True) + material.set_script_uri("banana") + material.set_script_name("orange") + material.set_shader(Material.ShaderType.VERTEX) + material.set_normal_map("blueberry") + material.set_file_path("/tmp/other") + + material2 = Material(material) + self.assertEqual(Color(0.1, 0.2, 0.3, 0.5), material2.ambient()) + self.assertEqual(Color(0.2, 0.3, 0.4, 0.6), material2.diffuse()) + self.assertEqual(Color(0.3, 0.4, 0.5, 0.7), + material2.specular()) + self.assertEqual(Color(0.4, 0.5, 0.6, 0.8), + material2.emissive()) + self.assertFalse(material2.lighting()) + self.assertTrue(material2.double_sided()) + self.assertEqual(4, material2.render_order()) + self.assertEqual("banana", material2.script_uri()) + self.assertEqual("orange", material2.script_name()) + self.assertEqual(Material.ShaderType.VERTEX, material2.shader()) + self.assertEqual("blueberry", material2.normal_map()) + # TODO(ahcorde) Add Pbr python interface + # self.assertEqual(nullptr, material2.PbrMaterial()) + self.assertEqual("/tmp/other", material2.file_path()) + + material.set_ambient(Color(0.3, 0.1, 0.2, 0.3)) + material.set_diffuse(Color(0.4, 0.4, 0.5, 0.6)) + material.set_specular(Color(0.5, 0.6, 0.7, 0.8)) + material.set_emissive(Color(0.6, 0.7, 0.8, 0.9)) + material.set_lighting(True) + material.set_render_order(2) + material.set_double_sided(False) + material.set_script_uri("apple") + material.set_script_name("melon") + material.set_shader(Material.ShaderType.VERTEX) + material.set_normal_map("banana") + material.set_file_path("/tmp/foo") + + self.assertEqual(Color(0.1, 0.2, 0.3, 0.5), material2.ambient()) + self.assertEqual(Color(0.2, 0.3, 0.4, 0.6), material2.diffuse()) + self.assertEqual(Color(0.3, 0.4, 0.5, 0.7), + material2.specular()) + self.assertEqual(Color(0.4, 0.5, 0.6, 0.8), + material2.emissive()) + self.assertFalse(material2.lighting()) + self.assertTrue(material2.double_sided()) + self.assertEqual(4, material2.render_order()) + self.assertEqual("banana", material2.script_uri()) + self.assertEqual("orange", material2.script_name()) + self.assertEqual(Material.ShaderType.VERTEX, material2.shader()) + self.assertEqual("blueberry", material2.normal_map()) + # TODO(ahcorde) Add Pbr python interface + # self.assertEqual(nullptr, material2.PbrMaterial()) + self.assertEqual("/tmp/other", material2.file_path()) + + def test_deepcopy(self): + material = Material() + material.set_ambient(Color(0.1, 0.2, 0.3, 0.5)) + material.set_diffuse(Color(0.2, 0.3, 0.4, 0.6)) + material.set_specular(Color(0.3, 0.4, 0.5, 0.7)) + material.set_emissive(Color(0.4, 0.5, 0.6, 0.8)) + material.set_lighting(False) + material.set_render_order(4) + material.set_double_sided(True) + material.set_script_uri("banana") + material.set_script_name("orange") + material.set_shader(Material.ShaderType.VERTEX) + material.set_normal_map("blueberry") + material.set_file_path("/tmp/other") + + material2 = copy.deepcopy(material) + self.assertEqual(Color(0.1, 0.2, 0.3, 0.5), material2.ambient()) + self.assertEqual(Color(0.2, 0.3, 0.4, 0.6), material2.diffuse()) + self.assertEqual(Color(0.3, 0.4, 0.5, 0.7), + material2.specular()) + self.assertEqual(Color(0.4, 0.5, 0.6, 0.8), + material2.emissive()) + self.assertFalse(material2.lighting()) + self.assertTrue(material2.double_sided()) + self.assertEqual(4, material2.render_order()) + self.assertEqual("banana", material2.script_uri()) + self.assertEqual("orange", material2.script_name()) + self.assertEqual(Material.ShaderType.VERTEX, material2.shader()) + self.assertEqual("blueberry", material2.normal_map()) + # TODO(ahcorde) Add Pbr python interface + # self.assertEqual(nullptr, material2.PbrMaterial()) + self.assertEqual("/tmp/other", material2.file_path()) + + material.set_ambient(Color(0.3, 0.1, 0.2, 0.3)) + material.set_diffuse(Color(0.4, 0.4, 0.5, 0.6)) + material.set_specular(Color(0.5, 0.6, 0.7, 0.8)) + material.set_emissive(Color(0.6, 0.7, 0.8, 0.9)) + material.set_lighting(True) + material.set_render_order(2) + material.set_double_sided(False) + material.set_script_uri("apple") + material.set_script_name("melon") + material.set_shader(Material.ShaderType.VERTEX) + material.set_normal_map("banana") + material.set_file_path("/tmp/foo") + + self.assertEqual(Color(0.1, 0.2, 0.3, 0.5), material2.ambient()) + self.assertEqual(Color(0.2, 0.3, 0.4, 0.6), material2.diffuse()) + self.assertEqual(Color(0.3, 0.4, 0.5, 0.7), + material2.specular()) + self.assertEqual(Color(0.4, 0.5, 0.6, 0.8), + material2.emissive()) + self.assertFalse(material2.lighting()) + self.assertTrue(material2.double_sided()) + self.assertEqual(4, material2.render_order()) + self.assertEqual("banana", material2.script_uri()) + self.assertEqual("orange", material2.script_name()) + self.assertEqual(Material.ShaderType.VERTEX, material2.shader()) + self.assertEqual("blueberry", material2.normal_map()) + # TODO(ahcorde) Add Pbr python interface + # self.assertEqual(nullptr, material2.PbrMaterial()) + self.assertEqual("/tmp/other", material2.file_path()) + + def test_set(self): + material = Material() + self.assertEqual(Color(0, 0, 0, 1), material.ambient()) + material.set_ambient(Color(0.1, 0.2, 0.3, 0.5)) + self.assertEqual(Color(0.1, 0.2, 0.3, 0.5), material.ambient()) + + self.assertEqual(Color(0, 0, 0, 1), material.diffuse()) + material.set_diffuse(Color(0.2, 0.3, 0.4, 0.6)) + self.assertEqual(Color(0.2, 0.3, 0.4, 0.6), material.diffuse()) + + self.assertEqual(Color(0, 0, 0, 1), material.specular()) + material.set_specular(Color(0.3, 0.4, 0.5, 0.7)) + self.assertEqual(Color(0.3, 0.4, 0.5, 0.7), material.specular()) + + self.assertEqual(Color(0, 0, 0, 1), material.emissive()) + material.set_emissive(Color(0.4, 0.5, 0.6, 0.8)) + self.assertEqual(Color(0.4, 0.5, 0.6, 0.8), material.emissive()) + + self.assertTrue(material.lighting()) + material.set_lighting(False) + self.assertFalse(material.lighting()) + + self.assertEqual(0, material.render_order()) + material.set_render_order(5) + self.assertEqual(5, material.render_order()) + + self.assertFalse(material.double_sided()) + material.set_double_sided(True) + self.assertTrue(material.double_sided()) + + self.assertEqual("", material.script_uri()) + material.set_script_uri("uri") + self.assertEqual("uri", material.script_uri()) + + self.assertEqual("", material.script_name()) + material.set_script_name("name") + self.assertEqual("name", material.script_name()) + + self.assertEqual(Material.ShaderType.PIXEL, material.shader()) + material.set_shader(Material.ShaderType.VERTEX) + self.assertEqual(Material.ShaderType.VERTEX, material.shader()) + + self.assertEqual("", material.normal_map()) + material.set_normal_map("map") + self.assertEqual("map", material.normal_map()) + + self.assertEqual("", material.file_path()) + material.set_file_path("/my/path") + self.assertEqual("/my/path", material.file_path()) + + # TODO(ahcorde) Add Pbr python interface + # set pbr material + # sdf::Pbr pbr + # sdf::PbrWorkflow workflow + # workflow.SetType(sdf::PbrWorkflowType::METAL) + # pbr.SetWorkflow(workflow.Type(), workflow) + # material.SetPbrMaterial(pbr) + # EXPECT_NE(material.PbrMaterial(), nullptr) + # self.assertEqual(workflow, + # *material.PbrMaterial()->Workflow(sdf::PbrWorkflowType::METAL)) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyMesh_TEST.py b/python/test/pyMesh_TEST.py new file mode 100644 index 000000000..2c2383c73 --- /dev/null +++ b/python/test/pyMesh_TEST.py @@ -0,0 +1,117 @@ +# Copyright (C) 2022 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. + +import copy +from sdformat import Mesh +from ignition.math import Vector3d +import unittest + + +class MeshTEST(unittest.TestCase): + + def test_default_construction(self): + mesh = Mesh() + + self.assertEqual("", mesh.file_path()) + self.assertEqual("", mesh.uri()) + self.assertEqual("", mesh.submesh()) + self.assertTrue(Vector3d(1, 1, 1) == mesh.scale()) + self.assertFalse(mesh.center_submesh()) + + + def test_assigment(self): + mesh = Mesh() + mesh.set_uri("banana") + mesh.set_submesh("watermelon") + mesh.set_center_submesh(True) + mesh.set_scale(Vector3d(0.5, 0.6, 0.7)) + mesh.set_file_path("/pear") + + mesh2 = mesh + self.assertEqual("banana", mesh2.uri()) + self.assertEqual("watermelon", mesh2.submesh()) + self.assertEqual(Vector3d(0.5, 0.6, 0.7), mesh2.scale()) + self.assertTrue(mesh2.center_submesh()) + self.assertEqual("/pear", mesh2.file_path()) + + mesh.set_file_path("/apple") + self.assertEqual("/apple", mesh2.file_path()) + + mesh.set_scale(Vector3d(0.3, 0.2, 0.4)) + self.assertEqual(Vector3d(0.3, 0.2, 0.4), mesh2.scale()) + + mesh.set_center_submesh(False) + self.assertFalse(mesh2.center_submesh()) + + mesh.set_submesh("melon") + self.assertEqual("melon", mesh2.submesh()) + + mesh.set_uri("pineapple") + self.assertEqual("pineapple", mesh2.uri()) + + + def test_deepcopy_construction(self): + mesh = Mesh() + mesh.set_uri("banana") + mesh.set_submesh("watermelon") + mesh.set_center_submesh(True) + mesh.set_scale(Vector3d(0.5, 0.6, 0.7)) + mesh.set_file_path("/pear") + + mesh2 = copy.deepcopy(mesh) + self.assertEqual("banana", mesh2.uri()) + self.assertEqual("watermelon", mesh2.submesh()) + self.assertEqual(Vector3d(0.5, 0.6, 0.7), mesh2.scale()) + self.assertTrue(mesh2.center_submesh()) + self.assertEqual("/pear", mesh2.file_path()) + + mesh.set_file_path("/apple") + mesh.set_scale(Vector3d(0.3, 0.2, 0.4)) + mesh.set_center_submesh(False) + mesh.set_submesh("melon") + mesh.set_uri("pineapple") + + self.assertEqual("banana", mesh2.uri()) + self.assertEqual("watermelon", mesh2.submesh()) + self.assertEqual(Vector3d(0.5, 0.6, 0.7), mesh2.scale()) + self.assertTrue(mesh2.center_submesh()) + self.assertEqual("/pear", mesh2.file_path()) + + + def test_set(self): + mesh = Mesh() + + self.assertEqual("", mesh.uri()) + mesh.set_uri("http://myuri.com") + self.assertEqual("http://myuri.com", mesh.uri()) + + self.assertEqual("", mesh.submesh()) + mesh.set_submesh("my_submesh") + self.assertEqual("my_submesh", mesh.submesh()) + + self.assertTrue(Vector3d(1, 1, 1) == mesh.scale()) + mesh.set_scale(Vector3d(0.2, 1.4, 7.8)) + self.assertTrue(Vector3d(0.2, 1.4, 7.8) == mesh.scale()) + + self.assertFalse(mesh.center_submesh()) + mesh.set_center_submesh(True) + self.assertTrue(mesh.center_submesh()) + + self.assertEqual("", mesh.file_path()) + mesh.set_file_path("/mypath") + self.assertEqual("/mypath", mesh.file_path()) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyModel_TEST.py b/python/test/pyModel_TEST.py new file mode 100644 index 000000000..2057c021f --- /dev/null +++ b/python/test/pyModel_TEST.py @@ -0,0 +1,372 @@ +# Copyright (C) 2022 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. + +import copy +from ignition.math import Pose3d, Vector3d +from sdformat import Model, Joint, Link, Error, Frame, SemanticPose +import math +import unittest + +class ModelTEST(unittest.TestCase): + + def test_default_construction(self): + model = Model() + self.assertFalse(model.name()) + + model.set_name("test_model") + self.assertEqual("test_model", model.name()) + + self.assertFalse(model.static()) + model.set_static(True) + self.assertTrue(model.static()) + + self.assertFalse(model.self_collide()) + model.set_self_collide(True) + self.assertTrue(model.self_collide()) + + self.assertTrue(model.allow_auto_disable()) + model.set_allow_auto_disable(False) + self.assertFalse(model.allow_auto_disable()) + + self.assertFalse(model.enable_wind()) + model.set_enable_wind(True) + self.assertTrue(model.enable_wind()) + + self.assertEqual(0, model.model_count()) + self.assertEqual(None, model.model_by_index(0)) + self.assertEqual(None, model.model_by_index(1)) + self.assertEqual(None, model.model_by_name("")) + self.assertEqual(None, model.model_by_name("default")) + self.assertEqual(None, model.model_by_name("a::b")) + self.assertEqual(None, model.model_by_name("a::b::c")) + self.assertEqual(None, model.model_by_name("::::")) + self.assertFalse(model.model_name_exists("")) + self.assertFalse(model.model_name_exists("default")) + self.assertFalse(model.model_name_exists("a::b")) + self.assertFalse(model.model_name_exists("a::b::c")) + self.assertFalse(model.model_name_exists("::::")) + + self.assertEqual(0, model.link_count()) + self.assertEqual(None, model.link_by_index(0)) + self.assertEqual(None, model.link_by_index(1)) + self.assertEqual(None, model.link_by_name("")) + self.assertEqual(None, model.link_by_name("default")) + self.assertEqual(None, model.link_by_name("a::b")) + self.assertEqual(None, model.link_by_name("a::b::c")) + self.assertEqual(None, model.link_by_name("::::")) + self.assertFalse(model.link_name_exists("")) + self.assertFalse(model.link_name_exists("default")) + self.assertFalse(model.link_name_exists("a::b")) + self.assertFalse(model.link_name_exists("a::b::c")) + self.assertFalse(model.link_name_exists("::::")) + + self.assertEqual(0, model.joint_count()) + self.assertEqual(None, model.joint_by_index(0)) + self.assertEqual(None, model.joint_by_index(1)) + self.assertEqual(None, model.joint_by_name("")) + self.assertEqual(None, model.joint_by_name("default")) + self.assertEqual(None, model.joint_by_name("a::b")) + self.assertEqual(None, model.joint_by_name("a::b::c")) + self.assertEqual(None, model.joint_by_name("::::")) + self.assertFalse(model.joint_name_exists("")) + self.assertFalse(model.joint_name_exists("default")) + self.assertFalse(model.joint_name_exists("a::b")) + self.assertFalse(model.joint_name_exists("a::b::c")) + self.assertFalse(model.joint_name_exists("::::")) + + self.assertEqual(0, model.frame_count()) + self.assertEqual(None, model.frame_by_index(0)) + self.assertEqual(None, model.frame_by_index(1)) + self.assertEqual(None, model.frame_by_name("")) + self.assertEqual(None, model.frame_by_name("default")) + self.assertEqual(None, model.frame_by_name("a::b")) + self.assertEqual(None, model.frame_by_name("a::b::c")) + self.assertEqual(None, model.frame_by_name("::::")) + self.assertFalse(model.frame_name_exists("")) + self.assertFalse(model.frame_name_exists("default")) + self.assertFalse(model.frame_name_exists("a::b")) + self.assertFalse(model.frame_name_exists("a::b::c")) + self.assertFalse(model.frame_name_exists("::::")) + + self.assertFalse(model.canonical_link_name()) + self.assertEqual(None, model.canonical_link()) + model.set_canonical_link_name("link") + self.assertEqual("link", model.canonical_link_name()) + self.assertEqual(None, model.canonical_link()) + + self.assertFalse(model.placement_frame_name()) + model.set_placement_frame_name("test_frame") + self.assertEqual("test_frame", model.placement_frame_name()) + + self.assertEqual(Pose3d.ZERO, model.raw_pose()) + self.assertFalse(model.pose_relative_to()) + + semanticPose = model.semantic_pose() + self.assertEqual(model.raw_pose(), semanticPose.raw_pose()) + self.assertFalse(semanticPose.relative_to()) + pose = Pose3d() + # expect errors when trying to resolve pose + self.assertEqual(1, len(semanticPose.resolve(pose))) + + model.set_raw_pose(Pose3d(1, 2, 3, 0, 0, math.pi)) + self.assertEqual(Pose3d(1, 2, 3, 0, 0, math.pi), model.raw_pose()) + + model.set_pose_relative_to("world") + self.assertEqual("world", model.pose_relative_to()) + + semanticPose = model.semantic_pose() + self.assertEqual(model.raw_pose(), semanticPose.raw_pose()) + self.assertEqual("world", semanticPose.relative_to()) + pose = Pose3d() + # expect errors when trying to resolve pose + self.assertNotEqual(0, semanticPose.resolve(pose)) + + errors = model.validate_graphs() + self.assertEqual(2, len(errors)) + self.assertEqual(errors[0].code(), Error.ErrorCode.FRAME_ATTACHED_TO_GRAPH_ERROR) + self.assertEqual(errors[0].message(), + "FrameAttachedToGraph error: scope does not point to a valid graph.") + self.assertEqual(errors[1].code(), Error.ErrorCode.POSE_RELATIVE_TO_GRAPH_ERROR) + self.assertEqual(errors[1].message(), + "PoseRelativeToGraph error: scope does not point to a valid graph.") + + + def test_copy_construction(self): + model = Model() + model.set_name("test_model") + + model2 = Model(model) + self.assertEqual("test_model", model2.name()) + + + def test_add_link(self): + model = Model() + self.assertEqual(0, model.link_count()) + + link = Link() + link.set_name("link1") + self.assertTrue(model.add_link(link)) + self.assertEqual(1, model.link_count()) + self.assertFalse(model.add_link(link)) + self.assertEqual(1, model.link_count()) + + model.clear_links() + self.assertEqual(0, model.link_count()) + + self.assertTrue(model.add_link(link)) + self.assertEqual(1, model.link_count()) + linkFromModel = model.link_by_index(0) + self.assertNotEqual(None, linkFromModel) + self.assertEqual(linkFromModel.name(), link.name()) + + + def test_add_joint(self): + model = Model() + self.assertEqual(0, model.joint_count()) + + joint = Joint() + joint.set_name("joint1") + self.assertTrue(model.add_joint(joint)) + self.assertEqual(1, model.joint_count()) + self.assertFalse(model.add_joint(joint)) + self.assertEqual(1, model.joint_count()) + + model.clear_joints() + self.assertEqual(0, model.joint_count()) + + self.assertTrue(model.add_joint(joint)) + self.assertEqual(1, model.joint_count()) + jointFromModel = model.joint_by_index(0) + self.assertNotEqual(None, jointFromModel) + self.assertEqual(jointFromModel.name(), joint.name()) + + + def test_add_model(self): + model = Model() + self.assertEqual(0, model.model_count()) + + nestedModel = Model() + nestedModel.set_name("model1") + self.assertTrue(model.add_model(nestedModel)) + self.assertEqual(1, model.model_count()) + self.assertFalse(model.add_model(nestedModel)) + self.assertEqual(1, model.model_count()) + + model.clear_models() + self.assertEqual(0, model.model_count()) + + self.assertTrue(model.add_model(nestedModel)) + self.assertEqual(1, model.model_count()) + modelFromModel = model.model_by_index(0) + self.assertNotEqual(None, modelFromModel) + self.assertEqual(modelFromModel.name(), nestedModel.name()) + + + def test_add_modify_frame(self): + model = Model() + self.assertEqual(0, model.frame_count()) + + frame = Frame() + frame.set_name("frame1") + self.assertTrue(model.add_frame(frame)) + self.assertEqual(1, model.frame_count()) + self.assertFalse(model.add_frame(frame)) + self.assertEqual(1, model.frame_count()) + + model.clear_frames() + self.assertEqual(0, model.frame_count()) + + self.assertTrue(model.add_frame(frame)) + self.assertEqual(1, model.frame_count()) + + frameFromModel = model.frame_by_index(0) + self.assertNotEqual(None, frameFromModel) + self.assertEqual(frameFromModel.name(), frame.name()) + + mutableFrame = model.frame_by_index(0) + mutableFrame.set_name("newname1") + self.assertEqual(mutableFrame.name(), model.frame_by_index(0).name()) + + mutableframe_by_name = model.frame_by_name("frame1") + self.assertEqual(None, mutableframe_by_name) + mutableframe_by_name = model.frame_by_name("newname1") + self.assertNotEqual(None, mutableframe_by_name) + self.assertEqual("newname1", model.frame_by_name("newname1").name()) + + + def test_uri(self): + model = Model() + uri = "https:#fuel.ignitionrobotics.org/1.0/openrobotics/models/my-model" + + model.set_uri(uri) + self.assertEqual(uri, model.uri()) + + + def test_mutable_by_index(self): + model = Model() + model.set_name("model1") + self.assertEqual(0, model.model_count()) + + link = Link() + link.set_name("link1") + self.assertTrue(model.add_link(link)) + + joint = Joint() + joint.set_name("joint1") + self.assertTrue(model.add_joint(joint)) + + nestedModel = Model() + nestedModel.set_name("child1") + self.assertTrue(model.add_model(nestedModel)) + + # Modify the link + l = model.link_by_index(0) + self.assertNotEqual(None, l) + self.assertEqual("link1", l.name()) + l.set_name("link2") + self.assertEqual("link2", model.link_by_index(0).name()) + + # Modify the joint + j = model.joint_by_index(0) + self.assertNotEqual(None, j) + self.assertEqual("joint1", j.name()) + j.set_name("joint2") + self.assertEqual("joint2", model.joint_by_index(0).name()) + + # Modify the nested model + m = model.model_by_index(0) + self.assertNotEqual(None, m) + self.assertEqual("child1", m.name()) + m.set_name("child2") + self.assertEqual("child2", model.model_by_index(0).name()) + + + def test_mutable_by_name(self): + model = Model() + model.set_name("model1") + self.assertEqual(0, model.model_count()) + + link = Link() + link.set_name("link1") + self.assertTrue(model.add_link(link)) + + joint = Joint() + joint.set_name("joint1") + self.assertTrue(model.add_joint(joint)) + + nestedModel = Model() + nestedModel.set_name("child1") + self.assertTrue(model.add_model(nestedModel)) + + frame = Frame() + frame.set_name("frame1") + self.assertTrue(model.add_frame(frame)) + + # Modify the link + l = model.link_by_name("link1") + self.assertNotEqual(None, l) + self.assertEqual("link1", l.name()) + l.set_name("link2") + self.assertFalse(model.link_name_exists("link1")) + self.assertTrue(model.link_name_exists("link2")) + + # Modify the joint + j = model.joint_by_name("joint1") + self.assertNotEqual(None, j) + self.assertEqual("joint1", j.name()) + j.set_name("joint2") + self.assertFalse(model.joint_name_exists("joint1")) + self.assertTrue(model.joint_name_exists("joint2")) + + # Modify the nested model + m = model.model_by_name("child1") + self.assertNotEqual(None, m) + self.assertEqual("child1", m.name()) + m.set_name("child2") + self.assertFalse(model.model_name_exists("child1")) + self.assertTrue(model.model_name_exists("child2")) + + # Modify the frame + f = model.frame_by_name("frame1") + self.assertNotEqual(None, f) + self.assertEqual("frame1", f.name()) + f.set_name("frame2") + self.assertFalse(model.frame_name_exists("frame1")) + self.assertTrue(model.frame_name_exists("frame2")) + + + # TODO(ahcorde): Uncomment this when sdf::Plugin is enabled + # def test_mutable_by_name(self): + # model = Model() + # self.assertTrue(model.plugins().empty()) + # + # plugin = Plugin() + # plugin.set_name("name1") + # plugin.set_filename("filename1") + # + # model.add_plugin(plugin) + # ASSERT_EQ(1, model.plugins().size()) + # + # plugin.set_name("name2") + # model.add_plugin(plugin) + # ASSERT_EQ(2, model.plugins().size()) + # + # self.assertEqual("name1", model.plugins()[0].name()) + # self.assertEqual("name2", model.plugins()[1].name()) + # + # model.clear_plugins() + # self.assertTrue(model.plugins().empty()) + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyNoise_TEST.py b/python/test/pyNoise_TEST.py new file mode 100644 index 000000000..da8188a47 --- /dev/null +++ b/python/test/pyNoise_TEST.py @@ -0,0 +1,70 @@ +# Copyright (C) 2022 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. + +import copy +from sdformat import Noise +import math +import unittest + +class NoiseTEST(unittest.TestCase): + + def test_construct_and_set(self): + noise = Noise() + + self.assertEqual(Noise.NoiseType.NONE, noise.type()) + noise.set_type(Noise.NoiseType.GAUSSIAN) + self.assertEqual(Noise.NoiseType.GAUSSIAN, noise.type()) + + self.assertAlmostEqual(0.0, noise.mean()) + noise.set_mean(1.2) + self.assertAlmostEqual(1.2, noise.mean()) + + self.assertAlmostEqual(0.0, noise.std_dev()) + noise.set_std_dev(2.3) + self.assertAlmostEqual(2.3, noise.std_dev()) + + self.assertAlmostEqual(0.0, noise.bias_mean()) + noise.set_bias_mean(4.5) + self.assertAlmostEqual(4.5, noise.bias_mean()) + + self.assertAlmostEqual(0.0, noise.bias_std_dev()) + noise.set_bias_std_dev(6.7) + self.assertAlmostEqual(6.7, noise.bias_std_dev()) + + self.assertAlmostEqual(0.0, noise.precision()) + noise.set_precision(8.9) + self.assertAlmostEqual(8.9, noise.precision()) + + self.assertAlmostEqual(0.0, noise.dynamic_bias_std_dev()) + noise.set_dynamic_bias_std_dev(9.1) + self.assertAlmostEqual(9.1, noise.dynamic_bias_std_dev()) + + self.assertAlmostEqual(0.0, noise.dynamic_bias_correlation_time()) + noise.set_dynamic_bias_correlation_time(19.12) + self.assertAlmostEqual(19.12, noise.dynamic_bias_correlation_time()) + + # Copy Constructor + noise2 = Noise(noise) + self.assertEqual(noise, noise2) + + # Copy operator + noise3 = noise + self.assertEqual(noise, noise3) + + noise4 = copy.deepcopy(noise) + self.assertEqual(noise, noise4) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyParserConfig_TEST.py b/python/test/pyParserConfig_TEST.py new file mode 100644 index 000000000..d2516d98c --- /dev/null +++ b/python/test/pyParserConfig_TEST.py @@ -0,0 +1,119 @@ +# Copyright (C) 2022 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. + +import copy +from sdformat import ParserConfig +from sdformattest import source_file, test_file +import unittest + + +class ParserConfigColor(unittest.TestCase): + + def test_construction(self): + config = ParserConfig() + self.assertFalse(config.find_file_callback()) + self.assertFalse(config.uri_path_map()) + self.assertFalse(config.find_file_callback()) + + testDir = source_file(); + config.add_uri_path("file://", testDir) + + self.assertTrue(config.uri_path_map()) + it = config.uri_path_map().get("file://") + self.assertEqual(1, len(it)) + self.assertEqual(it[0], testDir) + + def testFunc(argument): + return "test/dir2"; + + config.set_find_callback(testFunc) + self.assertTrue(config.find_file_callback()) + self.assertEqual("test/dir2", config.find_file_callback()("empty")) + + + def test_copy_construction(self): + # The directory used in add_uri_path must exist in the filesystem + # so we'll use the source path + testDir1 = source_file() + testDir2 = test_file() + + config1 = ParserConfig() + config1.add_uri_path("file://", testDir1) + it = config1.uri_path_map().get("file://") + self.assertEqual(1, len(it)) + self.assertEqual(it[0], testDir1) + + config2 = ParserConfig(config1) + it = config2.uri_path_map().get("file://") + self.assertEqual(1, len(it)) + self.assertEqual(it[0], testDir1) + + config2.add_uri_path("file://", testDir2) + it = config2.uri_path_map().get("file://") + self.assertEqual(2, len(it)) + self.assertEqual(it[1], testDir2) + + # Updating config2 should not affect config1 + it = config1.uri_path_map().get("file://") + self.assertEqual(1, len(it)) + self.assertEqual(it[0], testDir1) + + + def test_deepcopy(self): + # The directory used in add_uri_path must exist in the filesystem + # so we'll use the source path + testDir1 = source_file() + testDir2 = test_file() + + config1 = ParserConfig() + config1.add_uri_path("file://", testDir1) + it = config1.uri_path_map().get("file://") + self.assertEqual(1, len(it)) + self.assertEqual(it[0], testDir1) + + config2 = copy.deepcopy(config1) + it = config2.uri_path_map().get("file://") + self.assertEqual(1, len(it)) + self.assertEqual(it[0], testDir1) + + config2.add_uri_path("file://", testDir2) + it = config2.uri_path_map().get("file://") + self.assertEqual(2, len(it)) + self.assertEqual(it[1], testDir2) + + # Updating config2 should not affect config1 + it = config1.uri_path_map().get("file://") + self.assertEqual(1, len(it)) + self.assertEqual(it[0], testDir1) + + def test_copy(self): + # The directory used in add_uri_path must exist in the filesystem, + # so we'll use the source path + testDir1 = source_file() + + config1 = ParserConfig(); + config1.add_uri_path("file://", testDir1) + + it = config1.uri_path_map().get("file://") + self.assertEqual(1, len(it)) + self.assertEqual(it[0], testDir1) + + config2 = config1 + it = config2.uri_path_map().get("file://") + self.assertEqual(1, len(it)) + self.assertEqual(it[0], testDir1) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyPlane_TEST.py b/python/test/pyPlane_TEST.py new file mode 100644 index 000000000..3a2889bfa --- /dev/null +++ b/python/test/pyPlane_TEST.py @@ -0,0 +1,89 @@ +# Copyright (C) 2022 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. + +import copy +from sdformat import Plane +from ignition.math import Vector3d, Vector2d, Planed +import unittest + + +class PlaneTEST(unittest.TestCase): + + def test_default_construction(self): + plane = Plane() + + self.assertEqual(Vector3d.UNIT_Z, plane.normal()) + self.assertEqual(Vector2d.ONE, plane.size()) + + plane.set_normal(Vector3d(1, 0, 0)) + self.assertEqual(Vector3d.UNIT_X, plane.normal()) + + plane.set_normal(Vector3d(1, 0, 1)) + self.assertEqual(Vector3d(0.707107, 0, 0.707107), plane.normal()) + + plane.set_size(Vector2d(1.2, 3.4)) + self.assertEqual(Vector2d(1.2, 3.4), plane.size()) + + + def test_assigment(self): + plane = Plane() + plane.set_normal(Vector3d(1, 0, 0)) + plane.set_size(Vector2d(1.2, 3.4)) + + plane2 = plane + self.assertEqual(Vector3d.UNIT_X, plane2.normal()) + self.assertEqual(Vector2d(1.2, 3.4), plane2.size()) + + self.assertEqual(Vector3d.UNIT_X, plane2.shape().normal()) + self.assertEqual(Vector2d(1.2, 3.4), plane2.shape().size()) + + plane.set_normal(Vector3d(0, 1, 0)) + plane.set_size(Vector2d(3.3, 2.4)) + + self.assertEqual(Vector3d(0, 1, 0), plane2.normal()) + self.assertEqual(Vector2d(3.3, 2.4), plane2.size()) + + self.assertEqual(Vector3d(0, 1, 0), plane.normal()) + self.assertEqual(Vector2d(3.3, 2.4), plane.size()) + + def test_deepcopy_construction(self): + plane = Plane() + plane.set_normal(Vector3d(1, 0, 0)) + plane.set_size(Vector2d(1.2, 3.4)) + + plane2 = copy.deepcopy(plane) + self.assertEqual(Vector3d.UNIT_X, plane2.normal()) + self.assertEqual(Vector2d(1.2, 3.4), plane2.size()) + + plane.set_normal(Vector3d(0, 1, 0)) + plane.set_size(Vector2d(2.1, 4.3)) + + self.assertEqual(Vector3d.UNIT_X, plane2.normal()) + self.assertEqual(Vector2d(1.2, 3.4), plane2.size()) + self.assertEqual(Vector2d(2.1, 4.3), plane.size()) + self.assertEqual(Vector3d(0, 1, 0), plane.normal()) + + + def test_shape(self): + + plane = Plane() + self.assertEqual(Vector2d.ONE, plane.size()) + + plane.shape().set(plane.shape().normal(), Vector2d(1, 2), + plane.shape().offset()) + self.assertEqual(Vector2d(1, 2), plane.size()) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pySemanticPose_TEST.py b/python/test/pySemanticPose_TEST.py new file mode 100644 index 000000000..dcac3bb44 --- /dev/null +++ b/python/test/pySemanticPose_TEST.py @@ -0,0 +1,57 @@ +# Copyright (C) 2022 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. + +import copy +from ignition.math import Pose3d +from sdformat import Link, SemanticPose +import unittest + +class semantic_poseTEST(unittest.TestCase): + + + def test_default_construction(self): + link = Link() + rawPose = Pose3d(1, 0, 0, 0, 0, 0) + link.set_raw_pose(rawPose) + semPose = link.semantic_pose() + self.assertEqual(rawPose, semPose.raw_pose()) + + + def test_copy_construction(self): + link = Link() + rawPose = Pose3d(1, 0, 0, 0, 0, 0) + link.set_raw_pose(rawPose) + semPose1 = link.semantic_pose() + + semPose2 = SemanticPose(semPose1) + self.assertEqual(rawPose, semPose2.raw_pose()) + + + def test_deepcopy(self): + link = Link() + rawPose = Pose3d(1, 0, 0, 0, 0, 0) + link.set_raw_pose(rawPose) + semPose1 = link.semantic_pose() + + # Create another semantic_pose object from another Link + link2 = Link() + semPose2 = link2.semantic_pose() + self.assertEqual(Pose3d.ZERO, semPose2.raw_pose()) + + semPose2 = copy.deepcopy(semPose1) + self.assertEqual(rawPose, semPose2.raw_pose()) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pySphere_TEST.py b/python/test/pySphere_TEST.py new file mode 100644 index 000000000..ddfd7b89f --- /dev/null +++ b/python/test/pySphere_TEST.py @@ -0,0 +1,82 @@ +# Copyright (C) 2022 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. + +import copy +from ignition.math import Sphered +import math +from sdformat import Sphere +import unittest + +class SphereTEST(unittest.TestCase): + + def test_default_construction(self): + sphere = Sphere(); + self.assertEqual(1.0, sphere.radius()); + sphere.set_radius(0.5); + self.assertEqual(0.5, sphere.radius()); + + + def test_assignment(self): + sphere = Sphere(); + sphere.set_radius(0.2); + + sphere2 = sphere; + self.assertEqual(0.2, sphere2.radius()); + + + def test_copy_construction(self): + sphere = Sphere(); + sphere.set_radius(0.2); + + sphere2 = Sphere(sphere); + self.assertEqual(0.2, sphere2.radius()); + + self.assertEqual(4.0/3.0*math.pi*math.pow(0.2, 3), sphere2.shape().volume()); + self.assertEqual(0.2, sphere2.shape().radius()); + + + def test_deepcopy_construction(self): + sphere = Sphere(); + sphere.set_radius(0.2); + + sphere2 = copy.deepcopy(sphere); + self.assertEqual(0.2, sphere2.radius()); + + + def test_deepcopy_after_assignment(self): + sphere1 = Sphere(); + sphere1.set_radius(0.1); + + sphere2 = Sphere(); + sphere2.set_radius(0.2); + + # This is similar to what swap does + tmp = copy.deepcopy(sphere1); + sphere1 = sphere2; + sphere2 = tmp; + + self.assertEqual(0.2, sphere1.radius()); + self.assertEqual(0.1, sphere2.radius()); + + + def test_shape(self): + sphere = Sphere(); + self.assertEqual(1.0, sphere.radius()); + + sphere.shape().set_radius(0.123); + self.assertEqual(0.123, sphere.radius()); + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pySurface_TEST.py b/python/test/pySurface_TEST.py new file mode 100644 index 000000000..0f665e715 --- /dev/null +++ b/python/test/pySurface_TEST.py @@ -0,0 +1,86 @@ +# Copyright (C) 2022 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. + +import copy +from sdformat import Surface, Contact +import unittest + + +class SurfaceTEST(unittest.TestCase): + + def test_default_construction(self): + surface = Surface() + self.assertEqual(surface.contact().collide_bitmask(), 0xFF) + + + def test_assigment_construction(self): + surface1 = Surface() + contact = Contact() + contact.set_collide_bitmask(0x12) + surface1.set_contact(contact) + + surface2 = surface1 + self.assertEqual(surface2.contact().collide_bitmask(), 0x12) + + contact.set_collide_bitmask(0x21) + surface1.set_contact(contact) + self.assertEqual(surface1.contact().collide_bitmask(), 0x21) + self.assertEqual(surface2.contact().collide_bitmask(), 0x21) + + + def test_copy_construction(self): + surface1 = Surface() + contact = Contact() + contact.set_collide_bitmask(0x12) + surface1.set_contact(contact) + + surface2 = Surface(surface1) + self.assertEqual(surface2.contact().collide_bitmask(), 0x12) + + contact.set_collide_bitmask(0x21) + surface1.set_contact(contact) + self.assertEqual(surface1.contact().collide_bitmask(), 0x21) + self.assertEqual(surface2.contact().collide_bitmask(), 0x12) + + + def test_deepcopy(self): + surface1 = Surface() + contact = Contact() + contact.set_collide_bitmask(0x12) + surface1.set_contact(contact) + + surface2 = copy.deepcopy(surface1) + self.assertEqual(surface2.contact().collide_bitmask(), 0x12) + + contact.set_collide_bitmask(0x21) + surface1.set_contact(contact) + self.assertEqual(surface1.contact().collide_bitmask(), 0x21) + self.assertEqual(surface2.contact().collide_bitmask(), 0x12) + + + def test_default_contact_construction(self): + contact = Contact() + self.assertEqual(contact.collide_bitmask(), 0xFF) + + + def test_copy_contact_construction(self): + contact1 = Contact() + contact1.set_collide_bitmask(0x12) + + contact2 = Contact(contact1) + self.assertEqual(contact2.collide_bitmask(), 0x12) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyVisual_TEST.py b/python/test/pyVisual_TEST.py new file mode 100644 index 000000000..1c98aee12 --- /dev/null +++ b/python/test/pyVisual_TEST.py @@ -0,0 +1,245 @@ +# Copyright (C) 2022 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. + +import copy +from ignition.math import Pose3d, Color +from sdformat import Geometry, Material, Visual +import unittest +import math + +class VisualTEST(unittest.TestCase): + + def test_default_construction(self): + visual = Visual() + self.assertTrue(not visual.name()) + + visual.set_name("test_visual") + self.assertEqual(visual.name(), "test_visual") + + visual.set_cast_shadows(False) + self.assertFalse(visual.cast_shadows()) + + # check default transparency is 0 + self.assertEqual(0.0, visual.transparency()) + + visual.set_transparency(0.34) + self.assertAlmostEqual(0.34, visual.transparency()) + + self.assertEqual(Pose3d.ZERO, visual.raw_pose()) + self.assertTrue(not visual.pose_relative_to()) + + semanticPose = visual.semantic_pose() + self.assertEqual(visual.raw_pose(), semanticPose.raw_pose()) + self.assertEqual('', semanticPose.relative_to()) + pose = Pose3d() + # expect errors when trying to resolve pose + self.assertFalse(not semanticPose.resolve(pose)) + + visual.set_raw_pose(Pose3d(0, -20, 30, math.pi/2, -math.pi, math.pi/2)) + self.assertEqual(Pose3d(0, -20, 30, math.pi/2, -math.pi, math.pi/2), + visual.raw_pose()) + + visual.set_pose_relative_to("link") + self.assertEqual("link", visual.pose_relative_to()) + + semanticPose = visual.semantic_pose() + self.assertEqual(visual.raw_pose(), semanticPose.raw_pose()) + self.assertEqual("link", semanticPose.relative_to()) + pose = Pose3d() + # expect errors when trying to resolve pose + self.assertFalse(not semanticPose.resolve(pose)) + + self.assertNotEqual(None, visual.geometry()) + self.assertEqual(Geometry.GeometryType.EMPTY, visual.geometry().type()) + self.assertEqual(None, visual.geometry().box_shape()) + self.assertEqual(None, visual.geometry().cylinder_shape()) + self.assertEqual(None, visual.geometry().plane_shape()) + self.assertEqual(None, visual.geometry().sphere_shape()) + + self.assertEqual(None, visual.material()) + + # visibility flags + self.assertEqual(4294967295, visual.visibility_flags()) + visual.set_visibility_flags(1) + self.assertEqual(1, visual.visibility_flags()) + + + def test_copy_construction(self): + visual = Visual() + visual.set_name("test_visual") + visual.set_cast_shadows(False) + visual.set_transparency(0.345) + visual.set_raw_pose(Pose3d(0, -20, 30, math.pi/2, -math.pi, math.pi/2)) + visual.set_visibility_flags(2) + + visual.set_pose_relative_to("link") + + mat = Material() + mat.set_ambient(Color(0.1, 0.1, 0.1)) + visual.set_material(mat) + + visual2 = Visual(visual) + + self.assertEqual("test_visual", visual.name()) + self.assertFalse(visual.cast_shadows()) + self.assertAlmostEqual(0.345, visual.transparency()) + self.assertEqual(Pose3d(0, -20, 30, math.pi/2, -math.pi, math.pi/2), + visual.raw_pose()) + self.assertEqual("link", visual.pose_relative_to()) + material = visual.material() + self.assertTrue(None != visual.material()) + self.assertEqual(mat.ambient(), visual.material().ambient()) + self.assertEqual(2, visual.visibility_flags()) + + self.assertEqual("test_visual", visual2.name()) + self.assertFalse(visual2.cast_shadows()) + self.assertAlmostEqual(0.345, visual2.transparency()) + self.assertEqual(Pose3d(0, -20, 30, math.pi/2, -math.pi, math.pi/2), + visual2.raw_pose()) + self.assertEqual("link", visual2.pose_relative_to()) + self.assertTrue(None != visual2.material()) + self.assertEqual(mat.ambient(), visual2.material().ambient()) + self.assertEqual(2, visual2.visibility_flags()) + + + def test_deepcopy(self): + visual = Visual() + visual.set_name("test_visual") + visual.set_cast_shadows(False) + visual.set_transparency(0.345) + visual.set_raw_pose(Pose3d(0, -20, 30, math.pi/2, -math.pi, math.pi/2)) + visual.set_visibility_flags(2) + + visual.set_pose_relative_to("link") + + mat = Material() + mat.set_ambient(Color(0.1, 0.1, 0.1)) + visual.set_material(mat) + + visual2 = copy.deepcopy(visual) + + self.assertEqual("test_visual", visual.name()) + self.assertFalse(visual.cast_shadows()) + self.assertAlmostEqual(0.345, visual.transparency()) + self.assertEqual(Pose3d(0, -20, 30, math.pi/2, -math.pi, math.pi/2), + visual.raw_pose()) + self.assertEqual("link", visual.pose_relative_to()) + self.assertTrue(None != visual.material()) + self.assertEqual(mat.ambient(), visual.material().ambient()) + self.assertEqual(2, visual.visibility_flags()) + + self.assertEqual("test_visual", visual2.name()) + self.assertFalse(visual2.cast_shadows()) + self.assertAlmostEqual(0.345, visual2.transparency()) + self.assertEqual(Pose3d(0, -20, 30, math.pi/2, -math.pi, math.pi/2), + visual2.raw_pose()) + self.assertEqual("link", visual2.pose_relative_to()) + self.assertTrue(None != visual2.material()) + self.assertEqual(mat.ambient(), visual2.material().ambient()) + self.assertEqual(2, visual2.visibility_flags()) + + + def test_assignment(self): + visual = Visual() + visual.set_name("test_visual") + visual.set_cast_shadows(False) + visual.set_transparency(0.345) + visual.set_raw_pose(Pose3d(0, -20, 30, math.pi/2, -math.pi, math.pi/2)) + visual.set_visibility_flags(2) + + visual.set_pose_relative_to("link") + + mat = Material() + mat.set_ambient(Color(0.1, 0.1, 0.1)) + visual.set_material(mat) + + visual2 = visual + + self.assertEqual("test_visual", visual2.name()) + self.assertFalse(visual2.cast_shadows()) + self.assertAlmostEqual(0.345, visual2.transparency()) + self.assertEqual(Pose3d(0, -20, 30, math.pi/2, -math.pi, math.pi/2), + visual2.raw_pose()) + self.assertEqual("link", visual2.pose_relative_to()) + self.assertTrue(None != visual2.material()) + self.assertEqual(mat.ambient(), visual2.material().ambient()) + self.assertEqual(2, visual2.visibility_flags()) + + + def test_set_geometry(self): + visual = Visual() + self.assertTrue(not visual.name()) + + geometry = Geometry() + geometry.set_type(Geometry.GeometryType.BOX) + + visual.set_geometry(geometry) + + self.assertNotEqual(None, visual.geometry()) + self.assertEqual(Geometry.GeometryType.BOX, visual.geometry().type()) + + + def test_set_material(self): + visual = Visual() + self.assertTrue(not visual.name()) + + material = Material() + material.set_ambient(Color(0, 0.5, 0)) + material.set_diffuse(Color(1, 0, 0)) + material.set_specular(Color(0., 0.1, 0.9)) + + visual.set_material(material) + + self.assertNotEqual(None, visual.material()) + self.assertEqual(Color(0, 0.5, 0), visual.material().ambient()) + self.assertEqual(Color(1, 0, 0), visual.material().diffuse()) + self.assertEqual(Color(0., 0.1, 0.9), + visual.material().specular()) + + + def test_set_laser_retro(self): + visual = Visual() + self.assertTrue(not visual.name()) + + visual.set_laser_retro(150) + + self.assertTrue(visual.has_laser_retro()) + self.assertEqual(150, visual.laser_retro()) + + + # TODO(ahcorde) enable this when sdf::Plugins is converted + # def test_plugins(self): + # visual = Visual() + # self.assertTrue(visual.Plugins().empty()) + # + # plugin = Plugin() + # plugin.set_name("name1") + # plugin.set_filename("filename1") + # + # visual.add_plugin(plugin) + # self.assertEqual(1, visual.plugins().size()) + # + # plugin.set_name("name2") + # visual.add_plugin(plugin) + # self.assertEqual(2, visual.plugins().size()) + # + # self.assertEqual("name1", visual.plugins()[0].name()) + # self.assertEqual("name2", visual.plugins()[1].name()) + # + # visual.clear_plugins() + # self.assertTrue(visual.Plugins())) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py new file mode 100644 index 000000000..15cd8fa68 --- /dev/null +++ b/python/test/pyWorld_TEST.py @@ -0,0 +1,399 @@ +# Copyright (C) 2022 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. + +import copy +from ignition.math import Pose3d, Vector3d, SphericalCoordinates +from sdformat import World, Model, Frame, Error +import unittest +import math + +# TODO(ahcorde) +# - Add Atmosphere, GUI, Actor, Light, Scene and Plugin tests when the sdf::Classes are ported + +class WorldTEST(unittest.TestCase): + + def test_default_construction(self): + world = World() + self.assertFalse(world.name()) + self.assertEqual(Vector3d(0, 0, -9.80665), world.gravity()) + self.assertEqual(Vector3d(5.5645e-6, 22.8758e-6, -42.3884e-6), + world.magnetic_field()) + self.assertEqual("default", world.audio_device()) + self.assertEqual(Vector3d.ZERO, world.wind_linear_velocity()) + + self.assertEqual(0, world.model_count()) + self.assertEqual(None, world.model_by_index(0)) + self.assertEqual(None, world.model_by_index(1)) + self.assertFalse(world.model_name_exists("")) + self.assertFalse(world.model_name_exists("default")) + self.assertFalse(world.model_name_exists("a::b")) + self.assertFalse(world.model_name_exists("a::b::c")) + self.assertFalse(world.model_name_exists("::::")) + self.assertEqual(None, world.model_by_name("")) + self.assertEqual(None, world.model_by_name("default")) + self.assertEqual(None, world.model_by_name("a::b")) + self.assertEqual(None, world.model_by_name("a::b::c")) + self.assertEqual(None, world.model_by_name("::::")) + + self.assertEqual(0, world.frame_count()) + self.assertEqual(None, world.frame_by_index(0)) + self.assertEqual(None, world.frame_by_index(1)) + self.assertFalse(world.frame_name_exists("")) + self.assertFalse(world.frame_name_exists("default")) + + self.assertEqual(0, world.frame_count()) + self.assertEqual(None, world.frame_by_index(0)) + self.assertEqual(None, world.frame_by_index(1)) + self.assertFalse(world.frame_name_exists("")) + self.assertFalse(world.frame_name_exists("default")) + + self.assertEqual(1, world.physics_count()) + + errors = world.validate_graphs() + self.assertEqual(2, len(errors)) + self.assertEqual(errors[0].code(), Error.ErrorCode.FRAME_ATTACHED_TO_GRAPH_ERROR) + self.assertEqual(errors[0].message(), + "FrameAttachedToGraph error: scope does not point to a valid graph.") + self.assertEqual(errors[1].code(), Error.ErrorCode.POSE_RELATIVE_TO_GRAPH_ERROR) + self.assertEqual(errors[1].message(), + "PoseRelativeToGraph error: scope does not point to a valid graph.") + + + def test_copy_construction(self): + world = World() + # atmosphere = Atmosphere() + # atmosphere.set_pressure(0.1) + # world.set_Atmosphere(atmosphere) + world.set_audio_device("test_audio_device") + world.set_gravity(Vector3d(1, 0, 0)) + world.set_spherical_coordinates(SphericalCoordinates()) + + # sdf::Gui gui + # gui.set_Fullscreen(true) + # world.set_Gui(gui) + # + # sdf::Scene scene + # scene.set_Grid(true) + # world.set_Scene(scene) + + world.set_magnetic_field(Vector3d(0, 1, 0)) + world.set_name("test_world") + + world.set_wind_linear_velocity(Vector3d(0, 0, 1)) + + world2 = World(world) + + # self.assertTrue(None != world.Atmosphere()) + # self.assertAlmostEqual(0.1, world.Atmosphere().Pressure()) + self.assertTrue(None != world.spherical_coordinates()) + self.assertEqual(SphericalCoordinates.EARTH_WGS84, + world.spherical_coordinates().surface()) + self.assertEqual("test_audio_device", world.audio_device()) + self.assertEqual(Vector3d.UNIT_X, world.gravity()) + + # self.assertTrue(None != world.Gui()) + # self.assertEqual(gui.Fullscreen(), world.Gui().Fullscreen()) + + # self.assertTrue(None != world.Scene()) + # self.assertEqual(scene.Grid(), world.Scene().Grid()) + + self.assertEqual(Vector3d.UNIT_Y, world.magnetic_field()) + self.assertEqual(Vector3d.UNIT_Z, world.wind_linear_velocity()) + self.assertEqual("test_world", world.name()) + + # self.assertTrue(None != world2.Atmosphere()) + # self.assertAlmostEqual(0.1, world2.Atmosphere().Pressure()) + self.assertEqual("test_audio_device", world2.audio_device()) + self.assertEqual(Vector3d.UNIT_X, world2.gravity()) + + # self.assertTrue(None != world2.Gui()) + # self.assertEqual(gui.Fullscreen(), world2.Gui().Fullscreen()) + # + # self.assertTrue(None != world2.Scene()) + # self.assertEqual(scene.Grid(), world2.Scene().Grid()) + + self.assertEqual(Vector3d.UNIT_Y, world2.magnetic_field()) + self.assertEqual(Vector3d.UNIT_Z, world2.wind_linear_velocity()) + self.assertEqual("test_world", world2.name()) + + + def test_set(self): + world = World() + self.assertFalse(world.name()) + + world.set_name("default") + self.assertEqual("default", world.name()) + + world.set_audio_device("/dev/audio") + self.assertEqual("/dev/audio", world.audio_device()) + + world.set_wind_linear_velocity(Vector3d(0, 1 , 2)) + self.assertEqual(Vector3d(0, 1, 2), world.wind_linear_velocity()) + + world.set_gravity(Vector3d(1, -2, 4)) + self.assertEqual(Vector3d(1, -2, 4), world.gravity()) + + world.set_magnetic_field(Vector3d(1.2, -2.3, 4.5)) + self.assertEqual(Vector3d(1.2, -2.3, 4.5), world.magnetic_field()) + +# +# ########################/ +# TEST(DOMWorld, SetGui) +# { +# sdf::Gui gui +# gui.set_Fullscreen(true) +# +# world = World() +# self.assertEqual(None, world.Gui()) +# +# world.set_Gui(gui) +# self.assertNotEqual(None, world.Gui()) +# self.assertTrue(world.Gui().Fullscreen()) +# } +# +# ########################/ +# TEST(DOMWorld, SetScene) +# { +# world = World() +# self.assertEqual(None, world.Scene()) +# +# sdf::Scene scene +# scene.set_Ambient(Color::Blue) +# scene.set_Background(Color::Red) +# scene.set_Grid(true) +# scene.set_Shadows(true) +# scene.set_OriginVisual(true) +# world.set_Scene(scene) +# +# self.assertNotEqual(None, world.Scene()) +# self.assertEqual(Color::Blue, world.Scene().Ambient()) +# self.assertEqual(Color::Red, world.Scene().Background()) +# self.assertTrue(world.Scene().Grid()) +# self.assertTrue(world.Scene().Shadows()) +# self.assertTrue(world.Scene().OriginVisual()) +# } + + def test_add_model(self): + world = World() + self.assertEqual(0, world.model_count()) + + model = Model() + model.set_name("model1") + self.assertTrue(world.add_model(model)) + self.assertEqual(1, world.model_count()) + self.assertFalse(world.add_model(model)) + self.assertEqual(1, world.model_count()) + + world.clear_models() + self.assertEqual(0, world.model_count()) + + self.assertTrue(world.add_model(model)) + self.assertEqual(1, world.model_count()) + modelFromWorld = world.model_by_index(0) + self.assertNotEqual(None, modelFromWorld) + self.assertEqual(modelFromWorld.name(), model.name()) + + + def test_add_modify_frame(self): + + world = World() + self.assertEqual(0, world.frame_count()) + + frame = Frame() + frame.set_name("frame1") + self.assertTrue(world.add_frame(frame)) + self.assertEqual(1, world.frame_count()) + self.assertFalse(world.add_frame(frame)) + self.assertEqual(1, world.frame_count()) + + world.clear_frames() + self.assertEqual(0, world.frame_count()) + + self.assertTrue(world.add_frame(frame)) + self.assertEqual(1, world.frame_count()) + frameFromWorld = world.frame_by_index(0) + self.assertNotEqual(None, frameFromWorld) + self.assertEqual(frameFromWorld.name(), frame.name()) + + mutableFrame = world.frame_by_index(0) + mutableFrame.set_name("newName1") + self.assertEqual(mutableFrame.name(), world.frame_by_index(0).name()) + + mutableframe_by_Name = world.frame_by_name("frame1") + self.assertEqual(None, mutableframe_by_Name) + mutableframe_by_Name = world.frame_by_name("newName1") + self.assertNotEqual(None, mutableframe_by_Name) + self.assertEqual("newName1", world.frame_by_name("newName1").name()) + +# ########################/ +# TEST(DOMWorld, AddActor) +# { +# world = World() +# self.assertEqual(0, world.Actor_count()) +# +# sdf::Actor actor +# actor.set_name("actor1") +# self.assertTrue(world.add_Actor(actor)) +# self.assertEqual(1, world.Actor_count()) +# self.assertFalse(world.add_Actor(actor)) +# self.assertEqual(1, world.Actor_count()) +# +# world.clear_Actors() +# self.assertEqual(0, world.Actor_count()) +# +# self.assertTrue(world.add_Actor(actor)) +# self.assertEqual(1, world.Actor_count()) +# const sdf::Actor *actorFromWorld = world.Actor_by_index(0) +# self.assertNotEqual(None, actorFromWorld) +# self.assertEqual(actorFromWorld.name(), actor.name()) +# } +# +# ########################/ +# TEST(DOMWorld, AddLight) +# { +# world = World() +# self.assertEqual(0, world.Light_count()) +# +# sdf::Light light +# light.set_name("light1") +# self.assertTrue(world.add_Light(light)) +# self.assertEqual(1, world.Light_count()) +# self.assertFalse(world.add_Light(light)) +# self.assertEqual(1, world.Light_count()) +# +# world.clear_Lights() +# self.assertEqual(0, world.Light_count()) +# +# self.assertTrue(world.add_Light(light)) +# self.assertEqual(1, world.Light_count()) +# const sdf::Light *lightFromWorld = world.Light_by_index(0) +# self.assertNotEqual(None, lightFromWorld) +# self.assertEqual(lightFromWorld.name(), light.name()) +# } + + + def test_mutable_by_index(self): + world = World() + + model = Model() + model.set_name("model1") + self.assertTrue(world.add_model(model)) + + # sdf::Actor actor + # actor.set_name("actor1") + # self.assertTrue(world.add_Actor(actor)) + # + # sdf::Light light + # light.set_name("light1") + # self.assertTrue(world.add_Light(light)) + # + # sdf::Physics physics + # physics.set_name("physics1") + # self.assertTrue(world.add_Physics(physics)) + + frame = Frame() + frame.set_name("frame1") + self.assertTrue(world.add_frame(frame)) + + # Modify the model + m = world.model_by_index(0) + self.assertNotEqual(None, m) + self.assertEqual("model1", m.name()) + m.set_name("model2") + self.assertEqual("model2", world.model_by_index(0).name()) + + # # Modify the actor + # sdf::Actor *a = world.Actor_by_index(0) + # self.assertNotEqual(None, a) + # self.assertEqual("actor1", a.name()) + # a.set_name("actor2") + # self.assertEqual("actor2", world.Actor_by_index(0).name()) + # + # # Modify the light + # sdf::Light *l = world.Light_by_index(0) + # self.assertNotEqual(None, l) + # self.assertEqual("light1", l.name()) + # l.set_name("light2") + # self.assertEqual("light2", world.Light_by_index(0).name()) + + # # Modify the physics + # sdf::Physics *p = world.physics_by_index(1) + # self.assertNotEqual(None, p) + # self.assertEqual("physics1", p.name()) + # p.set_name("physics2") + # self.assertEqual("physics2", world.physics_by_index(1).name()) + + # Modify the frame + f = world.frame_by_index(0) + self.assertNotEqual(None, f) + self.assertEqual("frame1", f.name()) + f.set_name("frame2") + self.assertEqual("frame2", world.frame_by_index(0).name()) + + + def test_mutable_by_name(self): + world = World() + + model = Model() + model.set_name("model1") + self.assertTrue(world.add_model(model)) + + frame = Frame() + frame.set_name("frame1") + self.assertTrue(world.add_frame(frame)) + + # Modify the model + m = world.model_by_name("model1") + self.assertNotEqual(None, m) + self.assertEqual("model1", m.name()) + m.set_name("model2") + self.assertFalse(world.model_by_name("model1")) + self.assertTrue(world.model_by_name("model2")) + + # Modify the frame + f = world.frame_by_name("frame1") + self.assertNotEqual(None, f) + self.assertEqual("frame1", f.name()) + f.set_name("frame2") + self.assertFalse(world.frame_by_name("frame1")) + self.assertTrue(world.frame_by_name("frame2")) +# +# ########################/ +# TEST(DOMWorld, Plugins) +# { +# world = World() +# self.assertTrue(world.Plugins().empty()) +# +# sdf::Plugin plugin +# plugin.set_name("name1") +# plugin.set_Filename("filename1") +# +# world.add_Plugin(plugin) +# ASSERT_EQ(1, world.Plugins().size()) +# +# plugin.set_name("name2") +# world.add_Plugin(plugin) +# ASSERT_EQ(2, world.Plugins().size()) +# +# self.assertEqual("name1", world.Plugins()[0].name()) +# self.assertEqual("name2", world.Plugins()[1].name()) +# +# world.clear_Plugins() +# self.assertTrue(world.Plugins().empty()) +# } +# + + +if __name__ == '__main__': + unittest.main() diff --git a/sdf/1.7/camera.sdf b/sdf/1.7/camera.sdf index 1e46ff484..ef637048d 100644 --- a/sdf/1.7/camera.sdf +++ b/sdf/1.7/camera.sdf @@ -20,6 +20,9 @@ (L8|L16|R_FLOAT16|R_FLOAT32|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8) + + Value used for anti-aliasing + diff --git a/sdf/1.8/CMakeLists.txt b/sdf/1.8/CMakeLists.txt index d074806f5..30555b39a 100644 --- a/sdf/1.8/CMakeLists.txt +++ b/sdf/1.8/CMakeLists.txt @@ -82,5 +82,5 @@ add_custom_target(schema1_8 ALL DEPENDS ${SDF_SCHEMA}) set_source_files_properties(${SDF_SCHEMA} PROPERTIES GENERATED TRUE) -install(FILES 1_7.convert ${sdfs} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${SDF_MAJOR_VERSION}/1.8) -install(FILES ${SDF_SCHEMA} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${SDF_MAJOR_VERSION}/1.8) +install(FILES 1_7.convert ${sdfs} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/1.8) +install(FILES ${SDF_SCHEMA} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/1.8) diff --git a/sdf/1.8/camera.sdf b/sdf/1.8/camera.sdf index 1e46ff484..ef637048d 100644 --- a/sdf/1.8/camera.sdf +++ b/sdf/1.8/camera.sdf @@ -20,6 +20,9 @@ (L8|L16|R_FLOAT16|R_FLOAT32|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8) + + Value used for anti-aliasing + diff --git a/sdf/1.8/light.sdf b/sdf/1.8/light.sdf index a6939ac48..356bad19a 100644 --- a/sdf/1.8/light.sdf +++ b/sdf/1.8/light.sdf @@ -18,6 +18,10 @@ When true, the light is on. + + If true, the light is visualized in the GUI + + Scale factor to set the relative power of a light. diff --git a/sdf/1.9/CMakeLists.txt b/sdf/1.9/CMakeLists.txt index 209d63043..7778acafd 100644 --- a/sdf/1.9/CMakeLists.txt +++ b/sdf/1.9/CMakeLists.txt @@ -82,5 +82,5 @@ add_custom_target(schema1_9 ALL DEPENDS ${SDF_SCHEMA}) set_source_files_properties(${SDF_SCHEMA} PROPERTIES GENERATED TRUE) -install(FILES 1_8.convert ${sdfs} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${SDF_MAJOR_VERSION}/1.9) -install(FILES ${SDF_SCHEMA} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${SDF_MAJOR_VERSION}/1.9) +install(FILES 1_8.convert ${sdfs} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/1.9) +install(FILES ${SDF_SCHEMA} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/1.9) diff --git a/sdf/1.9/camera.sdf b/sdf/1.9/camera.sdf index c3cf5c5ab..4be16539b 100644 --- a/sdf/1.9/camera.sdf +++ b/sdf/1.9/camera.sdf @@ -28,6 +28,9 @@ (L8|L16|R_FLOAT16|R_FLOAT32|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8) + + Value used for anti-aliasing + diff --git a/sdf/1.9/model.sdf b/sdf/1.9/model.sdf index 2a7aaf4b7..57dc83a01 100644 --- a/sdf/1.9/model.sdf +++ b/sdf/1.9/model.sdf @@ -14,9 +14,11 @@ The name of the model's canonical link, to which the model's implicit - coordinate frame is attached. If unset or set to an empty string, - the first link element listed as a child of this model is chosen - as the canonical link. + coordinate frame is attached. If unset or set to an empty string, the + first `/link` listed as a direct child of this model is chosen as the + canonical link. If the model has no direct `/link` children, it will + instead be attached to the first nested (or included) model's implicit + frame. @@ -24,7 +26,13 @@ - If set to true, the model is immovable. Otherwise the model is simulated in the dynamics engine. + + If set to true, the model is immovable; i.e., a dynamics engine will not + update its position. This will also overwrite this model's `@canonical_link` + and instead attach the model's implicit frame to the world's implicit frame. + This holds even if this model is nested (or included) by another model + instead of being a direct child of `//world`. + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 8770d6d6c..68992a2da 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -51,13 +51,6 @@ if (BUILD_TESTING) ${PROJECT_SOURCE_DIR}/test ) - if (TARGET UNIT_ign_TEST) - # Link the libraries that we always need. - target_link_libraries("UNIT_ign_TEST" - ignition-cmake${IGN_CMAKE_VER}::utilities - ) - endif() - if (TARGET UNIT_Converter_TEST) target_link_libraries(UNIT_Converter_TEST TINYXML2::TINYXML2) @@ -69,6 +62,7 @@ if (BUILD_TESTING) if (TARGET UNIT_FrameSemantics_TEST) target_sources(UNIT_FrameSemantics_TEST PRIVATE FrameSemantics.cc Utils.cc) + target_link_libraries(UNIT_FrameSemantics_TEST TINYXML2::TINYXML2) endif() if (TARGET UNIT_ParamPassing_TEST) @@ -89,6 +83,7 @@ if (BUILD_TESTING) if (TARGET UNIT_Utils_TEST) target_sources(UNIT_Utils_TEST PRIVATE Utils.cc) + target_link_libraries(UNIT_Utils_TEST TINYXML2::TINYXML2) endif() if (TARGET UNIT_XmlUtils_TEST) diff --git a/src/Camera.cc b/src/Camera.cc index 7a606d64f..4fa5e763a 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -73,6 +73,9 @@ class sdf::Camera::Implementation /// \brief Image format. public: PixelFormatType pixelFormat{PixelFormatType::RGB_INT8}; + /// \brief Anti-aliasing value. + public: uint32_t antiAliasingValue{4}; + /// \brief Near clip distance. public: double nearClip{0.1}; @@ -266,6 +269,10 @@ Errors Camera::Load(ElementPtr _sdf) errors.push_back({ErrorCode::ELEMENT_INVALID, "Camera sensor has invalid value of " + format}); } + + this->dataPtr->antiAliasingValue = + elem->Get("anti_aliasing", + this->dataPtr->antiAliasingValue).first; } else { @@ -500,6 +507,18 @@ void Camera::SetPixelFormatStr(const std::string &_fmt) this->dataPtr->pixelFormat = ConvertPixelFormat(_fmt); } +////////////////////////////////////////////////// +uint32_t Camera::AntiAliasingValue() const +{ + return this->dataPtr->antiAliasingValue; +} + +////////////////////////////////////////////////// +void Camera::SetAntiAliasingValue(uint32_t _antiAliasingValue) +{ + this->dataPtr->antiAliasingValue = _antiAliasingValue; +} + ////////////////////////////////////////////////// double Camera::DepthNearClip() const { @@ -1043,6 +1062,9 @@ sdf::ElementPtr Camera::ToElement() const imageElem->GetElement("width")->Set(this->ImageWidth()); imageElem->GetElement("height")->Set(this->ImageHeight()); imageElem->GetElement("format")->Set(this->PixelFormatStr()); + imageElem->GetElement("anti_aliasing")->Set( + this->AntiAliasingValue()); + sdf::ElementPtr clipElem = elem->GetElement("clip"); clipElem->GetElement("near")->Set(this->NearClip()); clipElem->GetElement("far")->Set(this->FarClip()); diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 2e00db31e..8caf37f91 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -50,6 +50,10 @@ TEST(DOMCamera, Construction) cam.SetPixelFormat(sdf::PixelFormatType::L_INT8); EXPECT_EQ(sdf::PixelFormatType::L_INT8 , cam.PixelFormat()); + EXPECT_EQ(4u, cam.AntiAliasingValue()); + cam.SetAntiAliasingValue(8); + EXPECT_EQ(8u, cam.AntiAliasingValue()); + EXPECT_DOUBLE_EQ(0.1, cam.DepthNearClip()); EXPECT_FALSE(cam.HasDepthNearClip()); cam.SetDepthNearClip(0.2); diff --git a/src/Light.cc b/src/Light.cc index 4551beb47..c9a1dfd10 100644 --- a/src/Light.cc +++ b/src/Light.cc @@ -87,6 +87,9 @@ class sdf::Light::Implementation /// \brief Is light on ? public: bool isLightOn = true; + + /// \brief is visual light enabled ? + public: bool visualize = true; }; ///////////////////////////////////////////////// @@ -148,6 +151,9 @@ Errors Light::Load(ElementPtr _sdf) this->dataPtr->isLightOn = _sdf->Get("light_on", this->dataPtr->isLightOn).first; + this->dataPtr->visualize = _sdf->Get("visualize", + this->dataPtr->visualize).first; + this->dataPtr->castShadows = _sdf->Get("cast_shadows", this->dataPtr->castShadows).first; @@ -335,6 +341,18 @@ void Light::SetLightOn(const bool _isLightOn) this->dataPtr->isLightOn = _isLightOn; } +///////////////////////////////////////////////// +bool Light::Visualize() const +{ + return this->dataPtr->visualize; +} + +///////////////////////////////////////////////// +void Light::SetVisualize(const bool _visualize) +{ + this->dataPtr->visualize = _visualize; +} + ///////////////////////////////////////////////// ignition::math::Color Light::Diffuse() const { diff --git a/src/Light_TEST.cc b/src/Light_TEST.cc index b79ab85ac..b751fbd84 100644 --- a/src/Light_TEST.cc +++ b/src/Light_TEST.cc @@ -59,6 +59,10 @@ TEST(DOMLight, DefaultConstruction) light.SetLightOn(false); EXPECT_FALSE(light.LightOn()); + EXPECT_TRUE(light.Visualize()); + light.SetVisualize(false); + EXPECT_FALSE(light.Visualize()); + EXPECT_FALSE(light.CastShadows()); light.SetCastShadows(true); EXPECT_TRUE(light.CastShadows()); @@ -118,6 +122,7 @@ TEST(DOMLight, CopyConstructor) light.SetPoseRelativeTo("ground_plane"); light.SetCastShadows(true); light.SetLightOn(false); + light.SetVisualize(false); light.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0)); light.SetSpecular(ignition::math::Color(0.8f, 0.9f, 0.1f, 1.0)); light.SetAttenuationRange(3.2); @@ -137,6 +142,7 @@ TEST(DOMLight, CopyConstructor) EXPECT_EQ("ground_plane", light2.PoseRelativeTo()); EXPECT_TRUE(light2.CastShadows()); EXPECT_FALSE(light2.LightOn()); + EXPECT_FALSE(light2.Visualize()); EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); EXPECT_EQ(ignition::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); EXPECT_DOUBLE_EQ(3.2, light2.AttenuationRange()); @@ -160,6 +166,7 @@ TEST(DOMLight, CopyAssignmentOperator) light.SetPoseRelativeTo("ground_plane"); light.SetCastShadows(true); light.SetLightOn(false); + light.SetVisualize(false); light.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0)); light.SetSpecular(ignition::math::Color(0.8f, 0.9f, 0.1f, 1.0)); light.SetAttenuationRange(3.2); @@ -180,6 +187,7 @@ TEST(DOMLight, CopyAssignmentOperator) EXPECT_EQ("ground_plane", light2.PoseRelativeTo()); EXPECT_TRUE(light2.CastShadows()); EXPECT_FALSE(light2.LightOn()); + EXPECT_FALSE(light2.Visualize()); EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); EXPECT_EQ(ignition::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); EXPECT_DOUBLE_EQ(3.2, light2.AttenuationRange()); @@ -203,6 +211,7 @@ TEST(DOMLight, MoveConstructor) light.SetPoseRelativeTo("ground_plane"); light.SetCastShadows(true); light.SetLightOn(false); + light.SetVisualize(false); light.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0)); light.SetSpecular(ignition::math::Color(0.8f, 0.9f, 0.1f, 1.0)); light.SetAttenuationRange(3.2); @@ -222,6 +231,7 @@ TEST(DOMLight, MoveConstructor) EXPECT_EQ("ground_plane", light2.PoseRelativeTo()); EXPECT_TRUE(light2.CastShadows()); EXPECT_FALSE(light2.LightOn()); + EXPECT_FALSE(light2.Visualize()); EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); EXPECT_EQ(ignition::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); EXPECT_DOUBLE_EQ(3.2, light2.AttenuationRange()); @@ -245,6 +255,7 @@ TEST(DOMLight, MoveAssignment) light.SetPoseRelativeTo("ground_plane"); light.SetCastShadows(true); light.SetLightOn(false); + light.SetVisualize(false); light.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0)); light.SetSpecular(ignition::math::Color(0.8f, 0.9f, 0.1f, 1.0)); light.SetAttenuationRange(3.2); @@ -265,6 +276,7 @@ TEST(DOMLight, MoveAssignment) EXPECT_EQ("ground_plane", light2.PoseRelativeTo()); EXPECT_TRUE(light2.CastShadows()); EXPECT_FALSE(light2.LightOn()); + EXPECT_FALSE(light2.Visualize()); EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); EXPECT_EQ(ignition::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); EXPECT_DOUBLE_EQ(3.2, light2.AttenuationRange()); diff --git a/src/Model.cc b/src/Model.cc index a6337ba00..50ca3dcdf 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -123,8 +123,8 @@ class sdf::Model::Implementation /// /// /// and the included model could also have a plugin. We want the - /// ToElement(true) function to output only the plugin specified in the - /// tag. + /// ToElement() function to output only the plugin specified in the + /// tag when the ToElementUseIncludeTag policy is true.. public: std::vector includePlugins; }; @@ -974,9 +974,9 @@ void Model::SetUri(const std::string &_uri) } ///////////////////////////////////////////////// -sdf::ElementPtr Model::ToElement(bool _useIncludeTag) const +sdf::ElementPtr Model::ToElement(const OutputConfig &_config) const { - if (_useIncludeTag && !this->dataPtr->uri.empty()) + if (_config.ToElementUseIncludeTag() && !this->dataPtr->uri.empty()) { sdf::ElementPtr worldElem(new sdf::Element); sdf::initFile("world.sdf", worldElem); @@ -1043,7 +1043,7 @@ sdf::ElementPtr Model::ToElement(bool _useIncludeTag) const // Model for (const sdf::Model &model : this->dataPtr->models) - elem->InsertElement(model.ToElement(_useIncludeTag), true); + elem->InsertElement(model.ToElement(_config), true); // Add in the plugins for (const Plugin &plugin : this->dataPtr->plugins) diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index d619212d2..925e6cbbc 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -455,7 +455,9 @@ TEST(DOMModel, Uri) // ToElement NOT using the URI, which should result in a { - sdf::ElementPtr elem = model.ToElement(false); + sdf::OutputConfig config = sdf::OutputConfig::GlobalConfig(); + config.SetToElementUseIncludeTag(false); + sdf::ElementPtr elem = model.ToElement(config); elem->PrintValues(" "); // Should be a diff --git a/src/OutputConfig.cc b/src/OutputConfig.cc new file mode 100644 index 000000000..0cfd8f640 --- /dev/null +++ b/src/OutputConfig.cc @@ -0,0 +1,53 @@ +/* + * Copyright 2022 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 "sdf/OutputConfig.hh" +#include "sdf/Types.hh" + +using namespace sdf; + +class sdf::OutputConfig::Implementation +{ + /// \brief Flag to use tags within ToElement methods instead of + /// the fully included model. + public: bool toElementUseIncludeTag = true; +}; + +///////////////////////////////////////////////// +OutputConfig::OutputConfig() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +OutputConfig &OutputConfig::GlobalConfig() +{ + static auto *defaultConfig = new OutputConfig; + return *defaultConfig; +} + +///////////////////////////////////////////////// +void OutputConfig::SetToElementUseIncludeTag(bool _useIncludeTag) +{ + this->dataPtr->toElementUseIncludeTag = _useIncludeTag; +} + +///////////////////////////////////////////////// +bool OutputConfig::ToElementUseIncludeTag() const +{ + return this->dataPtr->toElementUseIncludeTag; +} diff --git a/src/OutputConfig_TEST.cc b/src/OutputConfig_TEST.cc new file mode 100644 index 000000000..03b2a23ba --- /dev/null +++ b/src/OutputConfig_TEST.cc @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2022 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 + +#include "sdf/OutputConfig.hh" +#include "test_config.h" + +///////////////////////////////////////////////// +/// Test default construction of sdf::OutputConfig. +TEST(OutputConfig, Construction) +{ + sdf::OutputConfig config; + EXPECT_TRUE(config.ToElementUseIncludeTag()); + + config.SetToElementUseIncludeTag(false); + EXPECT_FALSE(config.ToElementUseIncludeTag()); + EXPECT_TRUE(config.GlobalConfig().ToElementUseIncludeTag()); + + config.GlobalConfig().SetToElementUseIncludeTag(false); + EXPECT_FALSE(config.ToElementUseIncludeTag()); + EXPECT_FALSE(config.GlobalConfig().ToElementUseIncludeTag()); +} diff --git a/src/Param.cc b/src/Param.cc index fcf1633f2..15b2ee3f1 100644 --- a/src/Param.cc +++ b/src/Param.cc @@ -313,7 +313,6 @@ std::string Param::GetAsString(const PrintConfig &_config) const this->dataPtr->StringFromValueImpl(_config, this->dataPtr->typeName, this->dataPtr->value, - this->dataPtr->strValue, valueStr)) { return valueStr; @@ -330,7 +329,6 @@ std::string Param::GetDefaultAsString(const PrintConfig &_config) const _config, this->dataPtr->typeName, this->dataPtr->defaultValue, - this->dataPtr->defaultStrValue, defaultStr)) { return defaultStr; @@ -339,7 +337,7 @@ std::string Param::GetDefaultAsString(const PrintConfig &_config) const sdferr << "Unable to get string from default value, " << "using ParamStreamer instead.\n"; StringStreamClassicLocale ss; - ss << ParamStreamer{ this->dataPtr->defaultValue }; + ss << ParamStreamer{ this->dataPtr->defaultValue, _config.OutPrecision() }; return ss.str(); } @@ -797,7 +795,6 @@ bool ParamPrivate::ValueFromStringImpl(const std::string &_typeName, /// \param[in] _config Printing configuration for the output string. /// \param[in] _parentAttributes Parent Element Attributes. /// \param[in] _value The variant value of this pose. -/// \param[in] _originalStr The original string used to set this pose value. /// \param[out] _valueStr The pose as a string. /// \return True if the string was successfully retrieved from the pose, false /// otherwise. @@ -805,11 +802,15 @@ bool ParamPrivate::ValueFromStringImpl(const std::string &_typeName, bool PoseStringFromValue(const PrintConfig &_config, const Param_V &_parentAttributes, const ParamPrivate::ParamVariant &_value, - const std::optional &_originalStr, std::string &_valueStr) { StringStreamClassicLocale ss; + if (_config.OutPrecision() == std::numeric_limits::max()) + ss << std::setprecision(std::numeric_limits::max_digits10); + else + ss << std::setprecision(_config.OutPrecision()); + const ignition::math::Pose3d *pose = std::get_if(&_value); if (!pose) @@ -878,7 +879,7 @@ bool PoseStringFromValue(const PrintConfig &_config, } // Helper function that sanitizes zero values like '-0' - auto sanitizeZero = [](double _number) + auto sanitizeZero = [&_config](double _number) { StringStreamClassicLocale stream; if (std::fpclassify(_number) == FP_ZERO) @@ -887,6 +888,11 @@ bool PoseStringFromValue(const PrintConfig &_config, } else { + if (_config.OutPrecision() == std::numeric_limits::max()) + stream << std::setprecision(std::numeric_limits::max_digits10); + else + stream << std::setprecision(_config.OutPrecision()); + stream << _number; } return stream.str(); @@ -949,17 +955,6 @@ bool PoseStringFromValue(const PrintConfig &_config, return true; } - // If no modification to the value is needed, the original string is returned. - if (!_config.RotationInDegrees() && - !_config.RotationSnapToDegrees().has_value() && - !_config.RotationSnapTolerance().has_value() && - _originalStr.has_value() && - !_originalStr->empty()) - { - _valueStr = _originalStr.value(); - return true; - } - ss << pose->Pos() << posRotDelimiter << sanitizeZero(pose->Rot().Roll()) << " " << sanitizeZero(pose->Rot().Pitch()) << " " @@ -974,22 +969,6 @@ bool ParamPrivate::StringFromValueImpl( const std::string &_typeName, const ParamVariant &_value, std::string &_valueStr) const -{ - return this->StringFromValueImpl( - _config, - _typeName, - _value, - std::nullopt, - _valueStr); -} - -///////////////////////////////////////////////// -bool ParamPrivate::StringFromValueImpl( - const PrintConfig &_config, - const std::string &_typeName, - const ParamVariant &_value, - const std::optional &_originalStr, - std::string &_valueStr) const { // This will be handled in a type specific manner if (_typeName == "bool") @@ -1012,13 +991,13 @@ bool ParamPrivate::StringFromValueImpl( if (!this->ignoreParentAttributes && p) { return PoseStringFromValue( - _config, p->GetAttributes(), _value, _originalStr, _valueStr); + _config, p->GetAttributes(), _value, _valueStr); } - return PoseStringFromValue(_config, {}, _value, _originalStr, _valueStr); + return PoseStringFromValue(_config, {}, _value, _valueStr); } StringStreamClassicLocale ss; - ss << ParamStreamer{ _value }; + ss << ParamStreamer{ _value, _config.OutPrecision() }; _valueStr = ss.str(); return true; } diff --git a/src/ParamPassing.cc b/src/ParamPassing.cc index 82bf01b02..cea6e8b77 100644 --- a/src/ParamPassing.cc +++ b/src/ParamPassing.cc @@ -184,7 +184,8 @@ void updateParams(const ParserConfig &_config, } else if (actionStr == "replace") { - ElementPtr newElem = initElementDescription(childElemXml, _errors); + ElementPtr newElem = + initElementDescription(childElemXml, _config, _errors); if (!newElem) continue; @@ -334,12 +335,13 @@ ElementPtr getElementByName(const ElementPtr _elem, ////////////////////////////////////////////////// ElementPtr initElementDescription(const tinyxml2::XMLElement *_xml, + const ParserConfig &_config, Errors &_errors) { ElementPtr elemDesc = std::make_shared(); std::string filename = std::string(_xml->Name()) + ".sdf"; - if (!initFile(filename, elemDesc)) + if (!initFile(filename, _config, elemDesc)) { // TODO(jenn) not sure if we should load the element anyway // (e.g., user created their own element), maybe future implementation @@ -360,7 +362,7 @@ void handleIndividualChildActions(const ParserConfig &_config, ElementPtr _elem, Errors &_errors) { - ElementPtr elemDesc = initElementDescription(_childrenXml, _errors); + ElementPtr elemDesc = initElementDescription(_childrenXml, _config, _errors); if (!elemDesc) return; @@ -504,7 +506,7 @@ void handleIndividualChildActions(const ParserConfig &_config, void add(const ParserConfig &_config, const std::string &_source, tinyxml2::XMLElement *_childXml, ElementPtr _elem, Errors &_errors) { - ElementPtr newElem = initElementDescription(_childXml, _errors); + ElementPtr newElem = initElementDescription(_childXml, _config, _errors); if (!newElem) return; diff --git a/src/ParamPassing.hh b/src/ParamPassing.hh index 98994aea2..1572f6dda 100644 --- a/src/ParamPassing.hh +++ b/src/ParamPassing.hh @@ -96,10 +96,12 @@ namespace sdf /// \brief Initialize an sdf element description from the xml element /// \param[in] _xml Pointer to xml element + /// \param[in] _config Custom parser configuration /// \param[out] _errors Captures errors found during parsing /// \return ElementPtr to the initialized element description, /// nullptr if undefined/unknown sdf element ElementPtr initElementDescription(const tinyxml2::XMLElement *_xml, + const ParserConfig &_config, Errors &_errors); /// \brief Handles individual actions of children in _childrenXml diff --git a/src/ParserConfig.cc b/src/ParserConfig.cc index f110c63a3..77de4e9e1 100644 --- a/src/ParserConfig.cc +++ b/src/ParserConfig.cc @@ -49,6 +49,10 @@ class sdf::ParserConfig::Implementation /// \brief Flag to explicitly preserve fixed joints when /// reading the SDF/URDF file. public: bool preserveFixedJoint = false; + + /// \brief Flag to use tags within ToElement methods instead of + /// the fully included model. + public: bool toElementUseIncludeTag = true; }; diff --git a/src/Plugin.cc b/src/Plugin.cc index e35a0fee4..a898c1654 100644 --- a/src/Plugin.cc +++ b/src/Plugin.cc @@ -15,6 +15,7 @@ * */ +#include "sdf/Types.hh" #include "sdf/Plugin.hh" #include "sdf/parser.hh" #include "Utils.hh" @@ -42,6 +43,18 @@ Plugin::Plugin() { } +///////////////////////////////////////////////// +Plugin::Plugin(const std::string &_filename, const std::string &_name, + const std::string &_xmlContent) + : dataPtr(std::make_unique()) +{ + this->SetFilename(_filename); + this->SetName(_name); + std::string trimmed = sdf::trim(_xmlContent); + if (!trimmed.empty()) + this->InsertContent(trimmed); +} + ///////////////////////////////////////////////// Plugin::~Plugin() = default; @@ -175,6 +188,50 @@ void Plugin::InsertContent(const sdf::ElementPtr _elem) this->dataPtr->contents.push_back(_elem->Clone()); } +///////////////////////////////////////////////// +bool Plugin::InsertContent(const std::string _content) +{ + // Read the XML content + auto xmlDoc = tinyxml2::XMLDocument(true, tinyxml2::COLLAPSE_WHITESPACE);; + xmlDoc.Parse(_content.c_str()); + if (xmlDoc.Error()) + { + sdferr << "Error parsing XML from string: " << xmlDoc.ErrorStr() << '\n'; + return false; + } + + // Insert each XML element + for (tinyxml2::XMLElement *xml = xmlDoc.FirstChildElement(); xml; + xml = xml->NextSiblingElement()) + { + sdf::ElementPtr element(new sdf::Element); + + // Copy the name + element->SetName(xml->Name()); + + // Copy attributes + for (const tinyxml2::XMLAttribute *attribute = xml->FirstAttribute(); + attribute; attribute = attribute->Next()) + { + element->AddAttribute(attribute->Name(), "string", "", 1, ""); + element->GetAttribute(attribute->Name())->SetFromString( + attribute->Value()); + } + + // Copy the value + if (xml->GetText() != nullptr) + element->AddValue("string", xml->GetText(), true); + + // Copy all children + copyChildren(element, xml, false); + + // Add the element to this plugin + this->InsertContent(element); + } + + return true; +} + ///////////////////////////////////////////////// Plugin &Plugin::operator=(const Plugin &_plugin) { diff --git a/src/Plugin_TEST.cc b/src/Plugin_TEST.cc index 0a0dd6e20..aef8d609c 100644 --- a/src/Plugin_TEST.cc +++ b/src/Plugin_TEST.cc @@ -318,3 +318,70 @@ TEST(DOMPlugin, InputStreamOperator) std::string elemString = elem->ToString(""); EXPECT_EQ(input, elemString); } + +///////////////////////////////////////////////// +TEST(DOMPlugin, InsertStringContent) +{ + sdf::Plugin plugin("my-filename", "my-name", + "ogre2"); + EXPECT_EQ("my-filename", plugin.Filename()); + EXPECT_EQ("my-name", plugin.Name()); + ASSERT_EQ(1u, plugin.Contents().size()); + EXPECT_EQ("render_engine", plugin.Contents()[0]->GetName()); + EXPECT_EQ("ogre2", plugin.Contents()[0]->Get()); + + std::string extraContent = R"foo( + 1.234 + hello + + goodbye + goodbye + +)foo"; + + // Insert more content using a string + EXPECT_TRUE(plugin.InsertContent(extraContent)); + + std::ostringstream completeContent; + completeContent << " ogre2" << extraContent; + + std::ostringstream completePlugin; + completePlugin << "\n" + << completeContent.str() + << "\n"; + EXPECT_EQ(completePlugin.str(), plugin.ToElement()->ToString("")); + + // Try out curly braces intitialization + sdf::Plugin plugin2{plugin.Filename(), plugin.Name(), completeContent.str()}; + EXPECT_EQ(plugin.ToElement()->ToString(""), + plugin2.ToElement()->ToString("")); + + // Try to insert poorly formed XML, which should fail and not modify the + // content. + EXPECT_FALSE(plugin2.InsertContent("")); + EXPECT_EQ(plugin.ToElement()->ToString(""), + plugin2.ToElement()->ToString("")); + + // An empty string will also fail and not modify the content + EXPECT_FALSE(plugin2.InsertContent("")); + EXPECT_EQ(plugin.ToElement()->ToString(""), + plugin2.ToElement()->ToString("")); + + // Contructing a new plugin with no content + sdf::Plugin plugin3{"a filename", "a name"}; + EXPECT_EQ("a filename", plugin3.Filename()); + EXPECT_EQ("a name", plugin3.Name()); + EXPECT_TRUE(plugin3.Contents().empty()); + + // Contructing a new plugin with bad XML content + sdf::Plugin plugin4{"filename", "name", ""}; + EXPECT_EQ("filename", plugin4.Filename()); + EXPECT_EQ("name", plugin4.Name()); + EXPECT_TRUE(plugin4.Contents().empty()); + + // Contructing a new plugin with bad XML content + sdf::Plugin plugin5{"filename", "name", " "}; + EXPECT_EQ("filename", plugin5.Filename()); + EXPECT_EQ("name", plugin5.Name()); + EXPECT_TRUE(plugin5.Contents().empty()); +} diff --git a/src/PrintConfig.cc b/src/PrintConfig.cc index a36c441b5..311c02141 100644 --- a/src/PrintConfig.cc +++ b/src/PrintConfig.cc @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include #include "sdf/PrintConfig.hh" #include "sdf/Console.hh" @@ -36,6 +37,9 @@ class PrintConfig::Implementation /// \brief True to preserve tags, false to expand. public: bool preserveIncludes = false; + + /// \brief The output stream's precision. By default, uses maximum precision. + public: int outPrecision = std::numeric_limits::max(); }; ///////////////////////////////////////////////// @@ -104,13 +108,26 @@ std::optional PrintConfig::RotationSnapTolerance() const return this->dataPtr->rotationSnapTolerance; } +///////////////////////////////////////////////// +void PrintConfig::SetOutPrecision(int _precision) +{ + this->dataPtr->outPrecision = _precision; +} + +///////////////////////////////////////////////// +int PrintConfig::OutPrecision() const +{ + return this->dataPtr->outPrecision; +} + ///////////////////////////////////////////////// bool PrintConfig::operator==(const PrintConfig &_config) const { if (this->RotationInDegrees() == _config.RotationInDegrees() && this->RotationSnapToDegrees() == _config.RotationSnapToDegrees() && this->RotationSnapTolerance() == _config.RotationSnapTolerance() && - this->PreserveIncludes() == _config.PreserveIncludes()) + this->PreserveIncludes() == _config.PreserveIncludes() && + this->OutPrecision() == _config.OutPrecision()) { return true; } diff --git a/src/PrintConfig_TEST.cc b/src/PrintConfig_TEST.cc index 9ac523c9e..dea1e2db3 100644 --- a/src/PrintConfig_TEST.cc +++ b/src/PrintConfig_TEST.cc @@ -105,3 +105,16 @@ TEST(PrintConfig, PreserveIncludes) config.SetPreserveIncludes(false); EXPECT_FALSE(config.PreserveIncludes()); } + +///////////////////////////////////////////////// +TEST(PrintConfig, OutPrecision) +{ + sdf::PrintConfig config; + EXPECT_EQ(std::numeric_limits::max(), config.OutPrecision()); + config.SetOutPrecision(2); + EXPECT_EQ(2, config.OutPrecision()); + config.SetOutPrecision(8); + EXPECT_EQ(8, config.OutPrecision()); + config.SetOutPrecision(std::numeric_limits::max()); + EXPECT_EQ(std::numeric_limits::max(), config.OutPrecision()); +} diff --git a/src/Root.cc b/src/Root.cc index 9b0e4061b..51e92028c 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -525,7 +525,7 @@ void Root::Implementation::UpdateGraphs(sdf::Model &_model, } ///////////////////////////////////////////////// -sdf::ElementPtr Root::ToElement(bool _useIncludeTag) const +sdf::ElementPtr Root::ToElement(const OutputConfig &_config) const { sdf::ElementPtr elem(new sdf::Element); sdf::initFile("root.sdf", elem); @@ -534,7 +534,7 @@ sdf::ElementPtr Root::ToElement(bool _useIncludeTag) const if (this->Model() != nullptr) { - elem->InsertElement(this->Model()->ToElement(_useIncludeTag), true); + elem->InsertElement(this->Model()->ToElement(_config), true); } else if (this->Light() != nullptr) { @@ -548,7 +548,7 @@ sdf::ElementPtr Root::ToElement(bool _useIncludeTag) const { // Worlds for (const sdf::World &world : this->dataPtr->worlds) - elem->InsertElement(world.ToElement(_useIncludeTag), true); + elem->InsertElement(world.ToElement(_config), true); } return elem; diff --git a/src/Surface.cc b/src/Surface.cc index 3977cf3fa..1811280bc 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -33,8 +33,50 @@ class sdf::Contact::Implementation public: sdf::ElementPtr sdf{nullptr}; }; +class sdf::ODE::Implementation +{ + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf{nullptr}; + + /// \brief Coefficient of friction in first friction pyramid direction, + /// the unitless maximum ratio of force in first friction pyramid + /// direction to normal force. + public: double mu = 1.0; + + /// \brief Coefficient of friction in second friction pyramid direction, + /// the unitless maximum ratio of force in second friction pyramid + /// direction to normal force. + public: double mu2 = 1.0; + + /// \brief Unit vector specifying first friction pyramid direction in + /// collision-fixed reference frame. + public: ignition::math::Vector3d fdir1 = {0, 0, 0}; + + /// \brief Force dependent slip in first friction pyramid direction, + /// equivalent to inverse of viscous damping coefficient + /// with units of m/s/N. + public: double slip1 = 0.0; + + /// \brief Force dependent slip in second friction pyramid direction, + /// equivalent to inverse of viscous damping coefficient + /// with units of m/s/N. + public: double slip2 = 0.0; +}; + +class sdf::Friction::Implementation +{ + /// \brief The object storing contact parameters + public: sdf::ODE ode; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf{nullptr}; +}; + class sdf::Surface::Implementation { + /// \brief The object storing contact parameters + public: sdf::Friction friction; + /// \brief The object storing contact parameters public: sdf::Contact contact; @@ -42,6 +84,174 @@ class sdf::Surface::Implementation public: sdf::ElementPtr sdf{nullptr}; }; + +///////////////////////////////////////////////// +ODE::ODE() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors ODE::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that sdf is a valid pointer + if (!_sdf) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Attempting to load a ODE, but the provided SDF " + "element is null."}); + return errors; + } + + // Check that the provided SDF element is a + // This is an error that cannot be recovered, so return an error. + if (_sdf->GetName() != "ode") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a ODE, but the provided SDF element is not a " + "."}); + return errors; + } + + this->dataPtr->mu = _sdf->Get("mu", this->dataPtr->mu).first; + this->dataPtr->mu2 = _sdf->Get("mu2", this->dataPtr->mu2).first; + this->dataPtr->slip1 = _sdf->Get("slip1", this->dataPtr->slip1).first; + this->dataPtr->slip2 = _sdf->Get("slip2", this->dataPtr->slip2).first; + this->dataPtr->fdir1 = _sdf->Get("fdir1", + this->dataPtr->fdir1).first; + + return errors; +} + +///////////////////////////////////////////////// +double ODE::Mu() const +{ + return this->dataPtr->mu; +} + +///////////////////////////////////////////////// +void ODE::SetMu(double _mu) +{ + this->dataPtr->mu = _mu; +} + +///////////////////////////////////////////////// +double ODE::Mu2() const +{ + return this->dataPtr->mu2; +} + +///////////////////////////////////////////////// +void ODE::SetMu2(double _mu2) +{ + this->dataPtr->mu2 = _mu2; +} + +///////////////////////////////////////////////// +const ignition::math::Vector3d &ODE::Fdir1() const +{ + return this->dataPtr->fdir1; +} + +///////////////////////////////////////////////// +void ODE::SetFdir1(const ignition::math::Vector3d &_fdir) +{ + this->dataPtr->fdir1 = _fdir; +} + +///////////////////////////////////////////////// +double ODE::Slip1() const +{ + return this->dataPtr->slip1; +} + +///////////////////////////////////////////////// +void ODE::SetSlip1(double _slip1) +{ + this->dataPtr->slip1 = _slip1; +} + +///////////////////////////////////////////////// +double ODE::Slip2() const +{ + return this->dataPtr->slip2; +} + +///////////////////////////////////////////////// +void ODE::SetSlip2(double _slip2) +{ + this->dataPtr->slip2 = _slip2; +} + +///////////////////////////////////////////////// +sdf::ElementPtr ODE::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +Friction::Friction() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors Friction::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that sdf is a valid pointer + if (!_sdf) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Attempting to load a Friction, but the provided SDF " + "element is null."}); + return errors; + } + + // Check that the provided SDF element is a + // This is an error that cannot be recovered, so return an error. + if (_sdf->GetName() != "friction") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a Friction, but the provided SDF element is not a " + "."}); + return errors; + } + + if (_sdf->HasElement("ode")) + { + Errors err = this->dataPtr->ode.Load(_sdf->GetElement("ode")); + errors.insert(errors.end(), err.begin(), err.end()); + } + + return errors; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Friction::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +void Friction::SetODE(const sdf::ODE &_ode) +{ + this->dataPtr->ode = _ode; +} + +///////////////////////////////////////////////// +const sdf::ODE *Friction::ODE() const +{ + return &this->dataPtr->ode; +} + ///////////////////////////////////////////////// Contact::Contact() : dataPtr(ignition::utils::MakeImpl()) @@ -83,6 +293,7 @@ Errors Contact::Load(ElementPtr _sdf) // \todo(nkoenig) Parse the remaining collide properties. return errors; } + ///////////////////////////////////////////////// sdf::ElementPtr Contact::Element() const { @@ -139,6 +350,12 @@ Errors Surface::Load(ElementPtr _sdf) errors.insert(errors.end(), err.begin(), err.end()); } + if (_sdf->HasElement("friction")) + { + Errors err = this->dataPtr->friction.Load(_sdf->GetElement("friction")); + errors.insert(errors.end(), err.begin(), err.end()); + } + // \todo(nkoenig) Parse the remaining surface properties. return errors; } @@ -154,6 +371,18 @@ const sdf::Contact *Surface::Contact() const return &this->dataPtr->contact; } +///////////////////////////////////////////////// +void Surface::SetFriction(const sdf::Friction &_friction) +{ + this->dataPtr->friction = _friction; +} + +///////////////////////////////////////////////// +const sdf::Friction *Surface::Friction() const +{ + return &this->dataPtr->friction; +} + ///////////////////////////////////////////////// void Surface::SetContact(const sdf::Contact &_contact) { diff --git a/src/Surface_TEST.cc b/src/Surface_TEST.cc index 1e31247d9..c1f475755 100644 --- a/src/Surface_TEST.cc +++ b/src/Surface_TEST.cc @@ -25,6 +25,7 @@ TEST(DOMsurface, DefaultConstruction) EXPECT_EQ(nullptr, surface.Element()); EXPECT_EQ(surface.Contact()->CollideBitmask(), 0xFF); EXPECT_EQ(surface.Contact()->Element(), nullptr); + EXPECT_EQ(surface.Friction()->Element(), nullptr); } ///////////////////////////////////////////////// @@ -32,11 +33,26 @@ TEST(DOMsurface, CopyOperator) { sdf::Surface surface1; sdf::Contact contact; + sdf::ODE ode; + ode.SetMu(0.1); + ode.SetMu2(0.2); + ode.SetSlip1(3); + ode.SetSlip2(4); + ode.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + sdf::Friction friction; + friction.SetODE(ode); contact.SetCollideBitmask(0x12); surface1.SetContact(contact); + surface1.SetFriction(friction); sdf::Surface surface2(surface1); EXPECT_EQ(surface2.Contact()->CollideBitmask(), 0x12); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip2(), 4); + EXPECT_EQ(surface2.Friction()->ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); } ///////////////////////////////////////////////// @@ -44,11 +60,26 @@ TEST(DOMsurface, CopyAssignmentOperator) { sdf::Surface surface1; sdf::Contact contact; + sdf::ODE ode; + ode.SetMu(0.1); + ode.SetMu2(0.2); + ode.SetSlip1(3); + ode.SetSlip2(4); + ode.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + sdf::Friction friction; + friction.SetODE(ode); contact.SetCollideBitmask(0x12); surface1.SetContact(contact); + surface1.SetFriction(friction); sdf::Surface surface2 = surface1; EXPECT_EQ(surface2.Contact()->CollideBitmask(), 0x12); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip2(), 4); + EXPECT_EQ(surface2.Friction()->ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); } ///////////////////////////////////////////////// @@ -64,12 +95,45 @@ TEST(DOMsurface, CopyAssignmentAfterMove) contact2.SetCollideBitmask(0x34); surface2.SetContact(contact2); + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + sdf::Friction friction1; + friction1.SetODE(ode1); + + sdf::ODE ode2; + ode2.SetMu(0.2); + ode2.SetMu2(0.1); + ode2.SetSlip1(7); + ode2.SetSlip2(8); + ode2.SetFdir1(ignition::math::Vector3d(3, 2, 1)); + sdf::Friction friction2; + friction2.SetODE(ode2); + + surface1.SetFriction(friction1); + surface2.SetFriction(friction2); + sdf::Surface tmp = std::move(surface1); surface1 = surface2; surface2 = tmp; EXPECT_EQ(surface2.Contact()->CollideBitmask(), 0x12); EXPECT_EQ(surface1.Contact()->CollideBitmask(), 0x34); + EXPECT_DOUBLE_EQ(surface1.Friction()->ODE()->Mu(), 0.2); + EXPECT_DOUBLE_EQ(surface1.Friction()->ODE()->Mu2(), 0.1); + EXPECT_DOUBLE_EQ(surface1.Friction()->ODE()->Slip1(), 7); + EXPECT_DOUBLE_EQ(surface1.Friction()->ODE()->Slip2(), 8); + EXPECT_EQ(surface1.Friction()->ODE()->Fdir1(), + ignition::math::Vector3d(3, 2, 1)); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip2(), 4); + EXPECT_EQ(surface2.Friction()->ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); } ///////////////////////////////////////////////// @@ -126,3 +190,242 @@ TEST(DOMcontact, CollideBitmask) EXPECT_EQ(contact.CollideBitmask(), 0x67); } +///////////////////////////////////////////////// +TEST(DOMfriction, SetFriction) +{ + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + sdf::Friction friction1; + friction1.SetODE(ode1); + EXPECT_EQ(nullptr, friction1.Element()); + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip2(), 4); + EXPECT_EQ(friction1.ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMfriction, CopyOperator) +{ + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + sdf::Friction friction1; + friction1.SetODE(ode1); + + sdf::Friction friction2(friction1); + EXPECT_DOUBLE_EQ(friction2.ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(friction2.ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(friction2.ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); + EXPECT_EQ(friction2.ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMfriction, CopyAssignmentOperator) +{ + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + sdf::Friction friction1; + friction1.SetODE(ode1); + + sdf::Friction friction2 = friction1; + EXPECT_DOUBLE_EQ(friction2.ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(friction2.ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(friction2.ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); + EXPECT_EQ(friction2.ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMfriction, CopyAssignmentAfterMove) +{ + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + sdf::Friction friction1; + friction1.SetODE(ode1); + + sdf::ODE ode2; + ode2.SetMu(0.2); + ode2.SetMu2(0.1); + ode2.SetSlip1(7); + ode2.SetSlip2(8); + ode2.SetFdir1(ignition::math::Vector3d(3, 2, 1)); + sdf::Friction friction2; + friction2.SetODE(ode2); + + sdf::Friction tmp = std::move(friction1); + friction1 = friction2; + friction2 = tmp; + + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu(), 0.2); + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu2(), 0.1); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip1(), 7); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip2(), 8); + EXPECT_EQ(friction1.ODE()->Fdir1(), + ignition::math::Vector3d(3, 2, 1)); + EXPECT_DOUBLE_EQ(friction2.ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(friction2.ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(friction2.ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); + EXPECT_EQ(friction2.ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMfriction, Set) +{ + sdf::ODE ode1; + sdf::Friction friction1; + + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu(), 1.0); + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu2(), 1.0); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip1(), 0); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip2(), 0); + EXPECT_EQ(friction1.ODE()->Fdir1(), + ignition::math::Vector3d(0, 0, 0)); + + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + friction1.SetODE(ode1); + + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip2(), 4); + EXPECT_EQ(friction1.ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMode, DefaultValues) +{ + sdf::ODE ode; + EXPECT_EQ(nullptr, ode.Element()); + EXPECT_DOUBLE_EQ(ode.Mu(), 1.0); + EXPECT_DOUBLE_EQ(ode.Mu2(), 1.0); + EXPECT_DOUBLE_EQ(ode.Slip1(), 0); + EXPECT_DOUBLE_EQ(ode.Slip2(), 0); + EXPECT_EQ(ode.Fdir1(), + ignition::math::Vector3d(0, 0, 0)); +} + +///////////////////////////////////////////////// +TEST(DOMode, CopyOperator) +{ + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + + sdf::ODE ode2(ode1); + EXPECT_DOUBLE_EQ(ode2.Mu(), 0.1); + EXPECT_DOUBLE_EQ(ode2.Mu2(), 0.2); + EXPECT_DOUBLE_EQ(ode2.Slip1(), 3); + EXPECT_DOUBLE_EQ(ode2.Slip2(), 4); + EXPECT_EQ(ode2.Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMode, CopyAssignmentOperator) +{ + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + + sdf::ODE ode2 = ode1; + EXPECT_DOUBLE_EQ(ode2.Mu(), 0.1); + EXPECT_DOUBLE_EQ(ode2.Mu2(), 0.2); + EXPECT_DOUBLE_EQ(ode2.Slip1(), 3); + EXPECT_DOUBLE_EQ(ode2.Slip2(), 4); + EXPECT_EQ(ode2.Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMode, CopyAssignmentAfterMove) +{ + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + + sdf::ODE ode2; + ode2.SetMu(0.2); + ode2.SetMu2(0.1); + ode2.SetSlip1(7); + ode2.SetSlip2(8); + ode2.SetFdir1(ignition::math::Vector3d(3, 2, 1)); + + sdf::ODE tmp = std::move(ode1); + ode1 = ode2; + ode2 = tmp; + + EXPECT_DOUBLE_EQ(ode1.Mu(), 0.2); + EXPECT_DOUBLE_EQ(ode1.Mu2(), 0.1); + EXPECT_DOUBLE_EQ(ode1.Slip1(), 7); + EXPECT_DOUBLE_EQ(ode1.Slip2(), 8); + EXPECT_EQ(ode1.Fdir1(), + ignition::math::Vector3d(3, 2, 1)); + EXPECT_DOUBLE_EQ(ode2.Mu(), 0.1); + EXPECT_DOUBLE_EQ(ode2.Mu2(), 0.2); + EXPECT_DOUBLE_EQ(ode2.Slip1(), 3); + EXPECT_DOUBLE_EQ(ode2.Slip2(), 4); + EXPECT_EQ(ode2.Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMode, Set) +{ + sdf::ODE ode1; + + EXPECT_DOUBLE_EQ(ode1.Mu(), 1.0); + EXPECT_DOUBLE_EQ(ode1.Mu2(), 1.0); + EXPECT_DOUBLE_EQ(ode1.Slip1(), 0); + EXPECT_DOUBLE_EQ(ode1.Slip2(), 0); + EXPECT_EQ(ode1.Fdir1(), + ignition::math::Vector3d(0, 0, 0)); + + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + EXPECT_DOUBLE_EQ(ode1.Mu(), 0.1); + EXPECT_DOUBLE_EQ(ode1.Mu2(), 0.2); + EXPECT_DOUBLE_EQ(ode1.Slip1(), 3); + EXPECT_DOUBLE_EQ(ode1.Slip2(), 4); + EXPECT_EQ(ode1.Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} diff --git a/src/Utils.cc b/src/Utils.cc index e37cda171..77d29f629 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -304,5 +304,63 @@ sdf::Errors loadIncludedInterfaceModels(sdf::ElementPtr _sdf, return allErrors; } + +///////////////////////////////////////////////// +void copyChildren(ElementPtr _sdf, tinyxml2::XMLElement *_xml, + const bool _onlyUnknown) +{ + // Iterate over all the child elements + tinyxml2::XMLElement *elemXml = nullptr; + for (elemXml = _xml->FirstChildElement(); elemXml; + elemXml = elemXml->NextSiblingElement()) + { + std::string elemName = elemXml->Name(); + + if (_sdf->HasElementDescription(elemName)) + { + if (!_onlyUnknown) + { + sdf::ElementPtr element = _sdf->AddElement(elemName); + + // FIXME: copy attributes + for (const auto *attribute = elemXml->FirstAttribute(); + attribute; attribute = attribute->Next()) + { + element->GetAttribute(attribute->Name())->SetFromString( + attribute->Value()); + } + + // copy value + const char *value = elemXml->GetText(); + if (value) + { + element->GetValue()->SetFromString(value); + } + copyChildren(element, elemXml, _onlyUnknown); + } + } + else + { + sdf::ElementPtr element(new sdf::Element); + element->SetParent(_sdf); + element->SetName(elemName); + for (const tinyxml2::XMLAttribute *attribute = elemXml->FirstAttribute(); + attribute; attribute = attribute->Next()) + { + element->AddAttribute(attribute->Name(), "string", "", 1, ""); + element->GetAttribute(attribute->Name())->SetFromString( + attribute->Value()); + } + + if (elemXml->GetText() != nullptr) + { + element->AddValue("string", elemXml->GetText(), true); + } + + copyChildren(element, elemXml, _onlyUnknown); + _sdf->InsertElement(element); + } + } +} } } diff --git a/src/Utils.hh b/src/Utils.hh index 5efbb928d..f80c84c4c 100644 --- a/src/Utils.hh +++ b/src/Utils.hh @@ -22,6 +22,7 @@ #include #include #include +#include #include "sdf/Error.hh" #include "sdf/Element.hh" #include "sdf/InterfaceElements.hh" @@ -229,6 +230,15 @@ namespace sdf return &_opt.value(); return nullptr; } + + /// \brief Copy all children from the provided tinyxml2 object into the + /// provided sdf element pointer. + /// \param[in, out] _sdf A valid sdf element pointer. + /// \param[in] _xml XML to copy. + /// \param[in] _onlyUnknown Set this to true to only copy XML elements that + /// do not have a matching description in the provided sdf element pointer. + void copyChildren(ElementPtr _sdf, tinyxml2::XMLElement *_xml, + const bool _onlyUnknown); } } #endif diff --git a/src/World.cc b/src/World.cc index f5574669e..ae10f3d27 100644 --- a/src/World.cc +++ b/src/World.cc @@ -856,7 +856,7 @@ Errors World::Implementation::LoadSphericalCoordinates( } ///////////////////////////////////////////////// -sdf::ElementPtr World::ToElement(bool _useIncludeTag) const +sdf::ElementPtr World::ToElement(const OutputConfig &_config) const { sdf::ElementPtr elem(new sdf::Element); sdf::initFile("world.sdf", elem); @@ -874,7 +874,7 @@ sdf::ElementPtr World::ToElement(bool _useIncludeTag) const // Models for (const sdf::Model &model : this->dataPtr->models) - elem->InsertElement(model.ToElement(_useIncludeTag), true); + elem->InsertElement(model.ToElement(_config), true); // Actors for (const sdf::Actor &actor : this->dataPtr->actors) diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 04562f03f..a4074775c 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -42,7 +42,8 @@ COMMANDS = { 'sdf' => " -d [ --describe ] [SPEC VERSION] Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@).\n" + " -g [ --graph ] arg Print the PoseRelativeTo or FrameAttachedTo graph. (WARNING: This is for advanced\n" + " use only and the output may change without any promise of stability)\n" + - " -p [ --print ] arg Print converted arg.\n" + + " -p [ --print ] arg Print converted arg. Note the quaternion representation of the\n" + + " rotational part of poses and unit vectors will be normalized.\n" + " -i [ --preserve-includes ] Preserve included tags when printing converted arg (does not preserve merge-includes).\n" + " --degrees Pose rotation angles are printed in degrees.\n" + " --snap-to-degrees arg Snap pose rotation angles to this specified interval in degrees. This value must be\n" + @@ -50,6 +51,9 @@ COMMANDS = { 'sdf' => " --snap-tolerance arg Used in conjunction with --snap-to-degrees, specifies the tolerance at which snapping\n" + " occurs. This value must be larger than 0, less than 360, and less than the defined\n" + " degrees value to snap to. If unspecified, its default value is 0.01.\n" + + " --inertial-stats arg Prints moment of inertia, centre of mass, and total mass from a model sdf file.\n" + + " --precision arg Set the output stream precision for floating point numbers. The arg must be a positive integer.\n" + + COMMON_OPTIONS } @@ -65,6 +69,7 @@ class Cmd options = {} options['degrees'] = 0 options['snap_tolerance'] = 0.01 + options['preserve_includes'] = 0 usage = COMMANDS[args[0]] @@ -81,6 +86,10 @@ class Cmd 'Check if an SDFormat file is valid.') do |arg| options['check'] = arg end + opts.on('--inertial-stats arg', String, + 'Prints moment of inertia, centre of mass, and total mass from a model sdf file.') do |arg| + options['inertial_stats'] = arg + end opts.on('-d', '--describe [VERSION]', 'Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@)') do |v| options['describe'] = v end @@ -109,6 +118,10 @@ class Cmd end options['snap_tolerance'] = arg end + opts.on('--precision arg', Integer, + 'Set the output stream precision for floating point numbers.') do |arg| + options['precision'] = arg + end opts.on('-g arg', '--graph type', String, 'Print PoseRelativeTo or FrameAttachedTo graph') do |graph_type| options['graph'] = {:type => graph_type} @@ -131,7 +144,8 @@ class Cmd options['command'] = ARGV[0] - if options['preserve_includes'] and not options['print'] + if (options['preserve_includes'] != 0 and not options['print']) || + (options['precision'] and not options['print']) puts usage exit(-1) end @@ -201,26 +215,33 @@ class Cmd if options.key?('check') Importer.extern 'int cmdCheck(const char *)' exit(Importer.cmdCheck(File.expand_path(options['check']))) + elsif options.key?('inertial_stats') + Importer.extern 'int cmdInertialStats(const char *)' + exit(Importer.cmdInertialStats(options['inertial_stats'])) elsif options.key?('describe') Importer.extern 'int cmdDescribe(const char *)' exit(Importer.cmdDescribe(options['describe'])) elsif options.key?('print') snap_to_degrees = 0 - if options['preserve_includes'] - Importer.extern 'int cmdPrintPreserveIncludes(const char *)' - exit(Importer.cmdPrintPreserveIncludes(File.expand_path(options['print']))) - elsif options.key?('snap_to_degrees') + precision = 0 + + if options.key?('snap_to_degrees') if options['snap_to_degrees'] < options['snap_tolerance'] puts "Rotation snapping tolerance must be larger than the snapping tolerance." exit(-1) end snap_to_degrees = options['snap_to_degrees'] end - Importer.extern 'int cmdPrint(const char *, int in_degrees, int snap_to_degrees, float snap_tolerance)' + if options.key?('precision') + precision = options['precision'] + end + Importer.extern 'int cmdPrint(const char *, int in_degrees, int snap_to_degrees, float snap_tolerance, int, int)' exit(Importer.cmdPrint(File.expand_path(options['print']), options['degrees'], snap_to_degrees, - options['snap_tolerance'])) + options['snap_tolerance'], + options['preserve_includes'], + precision)) elsif options.key?('graph') Importer.extern 'int cmdGraph(const char *, const char *)' exit(Importer.cmdGraph(options['graph'][:type], File.expand_path(ARGV[1]))) diff --git a/src/ign.cc b/src/ign.cc index 23f9625e8..fd15caa64 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -20,14 +20,19 @@ #include #include #include +#include #include "sdf/sdf_config.h" #include "sdf/Filesystem.hh" +#include "sdf/Link.hh" +#include "sdf/Model.hh" #include "sdf/Root.hh" #include "sdf/parser.hh" #include "sdf/PrintConfig.hh" #include "sdf/system_util.hh" +#include "ignition/math/Inertial.hh" + #include "FrameSemantics.hh" #include "ScopedGraph.hh" #include "ign.hh" @@ -134,7 +139,8 @@ extern "C" SDFORMAT_VISIBLE int cmdDescribe(const char *_version) ////////////////////////////////////////////////// extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path, - int inDegrees, int snapToDegrees, float snapTolerance) + int _inDegrees, int _snapToDegrees, float _snapTolerance, + int _preserveIncludes, int _outPrecision) { if (!sdf::filesystem::exists(_path)) { @@ -157,47 +163,24 @@ extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path, } sdf::PrintConfig config; - if (inDegrees!= 0) + if (_inDegrees != 0) { config.SetRotationInDegrees(true); } - if (snapToDegrees > 0) - { - config.SetRotationSnapToDegrees(static_cast(snapToDegrees), - static_cast(snapTolerance)); - } - - sdf->PrintValues(config); - return 0; -} -////////////////////////////////////////////////// -extern "C" SDFORMAT_VISIBLE int cmdPrintPreserveIncludes(const char *_path) -{ - if (!sdf::filesystem::exists(_path)) + if (_snapToDegrees > 0) { - std::cerr << "Error: File [" << _path << "] does not exist.\n"; - return -1; + config.SetRotationSnapToDegrees(static_cast(_snapToDegrees), + static_cast(_snapTolerance)); } - sdf::SDFPtr sdf(new sdf::SDF()); - - if (!sdf::init(sdf)) - { - std::cerr << "Error: SDF schema initialization failed.\n"; - return -1; - } + if (_preserveIncludes != 0) + config.SetPreserveIncludes(true); - if (!sdf::readFile(_path, sdf)) - { - std::cerr << "Error: SDF parsing the xml failed.\n"; - return -1; - } + if (_outPrecision > 0) + config.SetOutPrecision(_outPrecision); - sdf::PrintConfig config; - config.SetPreserveIncludes(true); sdf->PrintValues(config); - return 0; } @@ -267,3 +250,110 @@ extern "C" SDFORMAT_VISIBLE int cmdGraph( return 0; } + +////////////////////////////////////////////////// +extern "C" SDFORMAT_VISIBLE int cmdInertialStats( + const char *_path) +{ + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } + + sdf::Root root; + sdf::Errors errors = root.Load(_path); + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } + + if (root.WorldCount() > 0) + { + std::cerr << "Error: Expected a model file but received a world file." + << std::endl; + return -1; + } + + const sdf::Model *model = root.Model(); + if (!model) + { + std::cerr << "Error: Could not find the model." << std::endl; + return -1; + } + + if (model->ModelCount() > 0) + { + std::cout << "Warning: Inertial properties of links in nested" + " models will not be included." << std::endl; + } + + ignition::math::Inertiald totalInertial; + + for (uint64_t i = 0; i < model->LinkCount(); i++) + { + ignition::math::Pose3d linkPoseRelativeToModel; + errors = model->LinkByIndex(i)->SemanticPose(). + Resolve(linkPoseRelativeToModel, "__model__"); + + auto currentLinkInertial = model->LinkByIndex(i)->Inertial(); + currentLinkInertial.SetPose(linkPoseRelativeToModel * + currentLinkInertial.Pose()); + totalInertial += currentLinkInertial; + } + + auto totalMass = totalInertial.MassMatrix().Mass(); + auto xCentreOfMass = totalInertial.Pose().Pos().X(); + auto yCentreOfMass = totalInertial.Pose().Pos().Y(); + auto zCentreOfMass = totalInertial.Pose().Pos().Z(); + + std::cout << "Inertial statistics for model: " << model->Name() << std::endl; + std::cout << "---" << std::endl; + std::cout << "Total mass of the model: " << totalMass << std::endl; + std::cout << "---" << std::endl; + + std::cout << "Centre of mass in model frame: " << std::endl; + std::cout << "X: " << xCentreOfMass << std::endl; + std::cout << "Y: " << yCentreOfMass << std::endl; + std::cout << "Z: " << zCentreOfMass << std::endl; + std::cout << "---" << std::endl; + + std::cout << "Moment of inertia matrix: " << std::endl; + + // Pretty print the MOI matrix + std::stringstream ss; + ss << totalInertial.Moi(); + + std::string s; + size_t maxLength = 0u; + std::vector moiVector; + while ( std::getline(ss, s, ' ' ) ) + { + moiVector.push_back(s); + if (s.size() > maxLength) + { + maxLength = s.size(); + } + } + + for (int i = 0; i < 9; i++) + { + size_t spacePadding = maxLength - moiVector[i].size(); + // Print the matrix element + std::cout << moiVector[i]; + for (size_t j = 0; j < spacePadding; j++) + { + std::cout << " "; + } + // Add space for the next element + std::cout << " "; + // Add '\n' if the next row is about to start + if ((i+1)%3 == 0) + { + std::cout << "\n"; + } + } + std::cout << "---" << std::endl; + + return 0; +} diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index a60c9c525..0d99a3322 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -20,7 +20,7 @@ #include #include -#include +#include #include "sdf/parser.hh" #include "sdf/SDFImpl.hh" @@ -962,14 +962,15 @@ TEST(print_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Default printing std::string output = custom_exec_str( - IgnCommand() + " sdf -p " + path + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "1 2 3 30.009 44.991 -60.009"); // Printing with in_degrees output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --degrees " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -977,8 +978,8 @@ TEST(print_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 5 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 5 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -986,8 +987,8 @@ TEST(print_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 2 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 2 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -995,8 +996,8 @@ TEST(print_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 20 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 20 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1004,8 +1005,8 @@ TEST(print_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 5, snap_tolerance 0.008 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + - "--snap-tolerance 0.008 " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1029,14 +1030,15 @@ TEST(print_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Default printing std::string output = custom_exec_str( - IgnCommand() + " sdf -p " + path + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "1 2 3 0.523756 0.785241 -1.04735"); // Printing with in_degrees output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --degrees " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1044,8 +1046,8 @@ TEST(print_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 5 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 5 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1053,8 +1055,8 @@ TEST(print_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 2 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 2 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1062,8 +1064,8 @@ TEST(print_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 20 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 20 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1071,8 +1073,8 @@ TEST(print_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 5, snap_tolerance 0.008 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + - "--snap-tolerance 0.008 " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1096,7 +1098,7 @@ TEST(print_rotations_in_quaternions, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Default printing std::string output = custom_exec_str( - IgnCommand() + " sdf -p " + path + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1104,7 +1106,8 @@ TEST(print_rotations_in_quaternions, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with in_degrees output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --degrees " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1121,8 +1124,8 @@ TEST(print_rotations_in_quaternions, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 2 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 2 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1130,8 +1133,8 @@ TEST(print_rotations_in_quaternions, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 20 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 20 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1139,8 +1142,8 @@ TEST(print_rotations_in_quaternions, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 5, snap_tolerance 0.008 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + - "--snap-tolerance 0.008 " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1167,14 +1170,15 @@ TEST(print_includes_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Default printing std::string output = custom_exec_str( - IgnCommand() + " sdf -p " + path + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "1 2 3 30.009 44.991 -60.009"); // Printing with in_degrees output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --degrees " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1191,8 +1195,8 @@ TEST(print_includes_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 2 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 2 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1200,8 +1204,8 @@ TEST(print_includes_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 20 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 20 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1209,8 +1213,8 @@ TEST(print_includes_rotations_in_degrees, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 5, snap_tolerance 0.008 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + - "--snap-tolerance 0.008 " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1237,14 +1241,15 @@ TEST(print_includes_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Default printing std::string output = custom_exec_str( - IgnCommand() + " sdf -p " + path + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "1 2 3 0.523756 0.785241 -1.04735"); // Printing with in_degrees output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --degrees " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1261,8 +1266,8 @@ TEST(print_includes_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 2 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 2 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1270,8 +1275,8 @@ TEST(print_includes_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 20 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 20 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1279,8 +1284,8 @@ TEST(print_includes_rotations_in_radians, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Printing with snap_to_degrees 5, snap_tolerance 0.008 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + - "--snap-tolerance 0.008 " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1308,7 +1313,7 @@ TEST(print_includes_rotations_in_quaternions, // Default printing std::string output = custom_exec_str( - IgnCommand() + " sdf -p " + path + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1316,7 +1321,8 @@ TEST(print_includes_rotations_in_quaternions, // Printing with in_degrees output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --degrees " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1333,8 +1339,8 @@ TEST(print_includes_rotations_in_quaternions, // Printing with snap_to_degrees 2 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 2 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1342,8 +1348,8 @@ TEST(print_includes_rotations_in_quaternions, // Printing with snap_to_degrees 20 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 20 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1351,8 +1357,8 @@ TEST(print_includes_rotations_in_quaternions, // Printing with snap_to_degrees 5, snap_tolerance 0.008 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + - "--snap-tolerance 0.008 " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1380,14 +1386,15 @@ TEST(print_rotations_in_unnormalized_degrees, // returned by parsing the pose value, whenever a parent Element Attribute, // or PrintConfig is used. std::string output = custom_exec_str( - IgnCommand() + " sdf -p " + path + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "1 2 3 30.009 44.991 -60.009"); // Printing with in_degrees output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --degrees " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1404,8 +1411,8 @@ TEST(print_rotations_in_unnormalized_degrees, // Printing with snap_to_degrees 2 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 2 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1413,8 +1420,8 @@ TEST(print_rotations_in_unnormalized_degrees, // Printing with snap_to_degrees 20 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 20 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1422,8 +1429,8 @@ TEST(print_rotations_in_unnormalized_degrees, // Printing with snap_to_degrees 5, snap_tolerance 0.008 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + - "--snap-tolerance 0.008 " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1448,14 +1455,15 @@ TEST(print_rotations_in_unnormalized_radians, // Default printing std::string output = custom_exec_str( - IgnCommand() + " sdf -p " + path + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, - "1 2 3 -5.75943 -11.78112 5.23583"); + "1 2 3 0.523755 0.785251 -1.04736"); // Printing with in_degrees output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --degrees " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1472,8 +1480,8 @@ TEST(print_rotations_in_unnormalized_radians, // Printing with snap_to_degrees 2 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 2 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 2 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1481,8 +1489,8 @@ TEST(print_rotations_in_unnormalized_radians, // Printing with snap_to_degrees 20 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 20 " + - SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 20 " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1490,8 +1498,8 @@ TEST(print_rotations_in_unnormalized_radians, // Printing with snap_to_degrees 5, snap_tolerance 0.008 output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --snap-to-degrees 5 " + - "--snap-tolerance 0.008 " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --snap-to-degrees 5 " + + "--snap-tolerance 0.008 " + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1515,7 +1523,8 @@ TEST(shuffled_cmd_flags, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // -p PATH --degrees std::string output = custom_exec_str( - IgnCommand() + " sdf -p " + path + " --degrees " + SdfVersion()); + IgnCommand() + " sdf -p --precision 6 " + path + " --degrees " + + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1523,7 +1532,7 @@ TEST(shuffled_cmd_flags, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // --degrees -p PATH output = custom_exec_str( - IgnCommand() + " sdf --degrees -p " + path + SdfVersion()); + IgnCommand() + " sdf --degrees -p --precision 6 " + path + SdfVersion()); ASSERT_FALSE(output.empty()); EXPECT_PRED2(contains, output, "" @@ -1795,6 +1804,89 @@ TEST(GraphCmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(ModelFrameAttachedTo)) EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); } +///////////////////////////////////////////////// +TEST(inertial_stats, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + std::string pathBase = PROJECT_SOURCE_PATH; + pathBase += "/test/sdf"; + + auto expectedOutput = + "Inertial statistics for model: test_model\n" + "---\n" + "Total mass of the model: 24\n" + "---\n" + "Centre of mass in model frame: \n" + "X: 0\n" + "Y: 0\n" + "Z: 0\n" + "---\n" + "Moment of inertia matrix: \n" + "304 0 0 \n" + "0 304 0 \n" + "0 0 604 \n" + "---\n"; + + // Check a good SDF file by passing the absolute path + { + std::string path = pathBase +"/inertial_stats.sdf"; + + std::string output = + custom_exec_str(IgnCommand() + " sdf --inertial-stats " + + path + SdfVersion()); + EXPECT_EQ(expectedOutput, output); + } + + // Check a good SDF file from the same folder by passing a relative path + { + std::string path = "inertial_stats.sdf"; + + std::string output = + custom_exec_str("cd " + pathBase + " && " + + IgnCommand() + " sdf --inertial-stats " + + path + SdfVersion()); + EXPECT_EQ(expectedOutput, output); + } + + expectedOutput = + "Error Code 18: Msg: A link named link has invalid inertia.\n\n" + "Inertial statistics for model: model\n" + "---\n" + "Total mass of the model: 0\n" + "---\n" + "Centre of mass in model frame: \n" + "X: 0\n" + "Y: 0\n" + "Z: 0\n" + "---\n" + "Moment of inertia matrix: \n" + "0 0 0 \n" + "0 0 0 \n" + "0 0 0 \n" + "---\n"; + + // Check an invalid SDF file by passing the absolute path + { + std::string path = pathBase +"/inertial_invalid.sdf"; + + std::string output = + custom_exec_str(IgnCommand() + " sdf --inertial-stats " + + path + SdfVersion()); + EXPECT_EQ(expectedOutput, output); + } + + expectedOutput = + "Error: Expected a model file but received a world file.\n"; + // Check a valid world file. + { + std::string path = pathBase +"/box_plane_low_friction_test.world"; + + std::string output = + custom_exec_str(IgnCommand() + " sdf --inertial-stats " + + path + SdfVersion()); + EXPECT_EQ(expectedOutput, output); + } +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) diff --git a/src/parser.cc b/src/parser.cc index b7cd3492b..2edde28fa 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -155,7 +155,9 @@ static bool isSdfFile(const std::string &_fileName) ////////////////////////////////////////////////// template -static inline bool _initFile(const std::string &_filename, TPtr _sdf) +static inline bool _initFile(const std::string &_filename, + const ParserConfig &_config, + TPtr _sdf) { auto xmlDoc = makeSdfDoc(); if (tinyxml2::XML_SUCCESS != xmlDoc.LoadFile(_filename.c_str())) @@ -165,7 +167,7 @@ static inline bool _initFile(const std::string &_filename, TPtr _sdf) return false; } - return initDoc(&xmlDoc, _sdf); + return initDoc(&xmlDoc, _config, _sdf); } ////////////////////////////////////////////////// @@ -181,7 +183,9 @@ static inline bool _initFile(const std::string &_filename, TPtr _sdf) /// \param[out] _errors Captures errors encountered during parsing. static void insertIncludedElement(sdf::SDFPtr _includeSDF, const SourceLocation &_sourceLoc, bool _merge, - sdf::ElementPtr _parent, sdf::Errors &_errors) + sdf::ElementPtr _parent, + const ParserConfig &_config, + sdf::Errors &_errors) { Error invalidFileError(ErrorCode::FILE_READ, "Included model is invalid. Skipping model."); @@ -230,7 +234,7 @@ static void insertIncludedElement(sdf::SDFPtr _includeSDF, // We create a throwaway sdf::Root object in order to validate the // included entity. sdf::Root includedRoot; - sdf::Errors includeDOMerrors = includedRoot.Load(_includeSDF); + sdf::Errors includeDOMerrors = includedRoot.Load(_includeSDF, _config); _errors.insert(_errors.end(), includeDOMerrors.begin(), includeDOMerrors.end()); @@ -365,11 +369,17 @@ static void insertIncludedElement(sdf::SDFPtr _includeSDF, ////////////////////////////////////////////////// bool init(SDFPtr _sdf) +{ + return init(_sdf, ParserConfig::GlobalConfig()); +} + +////////////////////////////////////////////////// +bool init(SDFPtr _sdf, const ParserConfig &_config) { std::string xmldata = SDF::EmbeddedSpec("root.sdf", false); auto xmlDoc = makeSdfDoc(); xmlDoc.Parse(xmldata.c_str()); - return initDoc(&xmlDoc, _sdf); + return initDoc(&xmlDoc, _config, _sdf); } ////////////////////////////////////////////////// @@ -387,9 +397,10 @@ bool initFile( { auto xmlDoc = makeSdfDoc(); xmlDoc.Parse(xmldata.c_str()); - return initDoc(&xmlDoc, _sdf); + return initDoc(&xmlDoc, _config, _sdf); } - return _initFile(sdf::findFile(_filename, true, false, _config), _sdf); + return _initFile(sdf::findFile(_filename, true, false, _config), _config, + _sdf); } ////////////////////////////////////////////////// @@ -407,14 +418,15 @@ bool initFile( { auto xmlDoc = makeSdfDoc(); xmlDoc.Parse(xmldata.c_str()); - return initDoc(&xmlDoc, _sdf); + return initDoc(&xmlDoc, _config, _sdf); } - return _initFile(sdf::findFile(_filename, true, false, _config), _sdf); + return _initFile(sdf::findFile(_filename, true, false, _config), _config, + _sdf); } ////////////////////////////////////////////////// bool initString( - const std::string &_xmlString, const ParserConfig &, SDFPtr _sdf) + const std::string &_xmlString, const ParserConfig &_config, SDFPtr _sdf) { auto xmlDoc = makeSdfDoc(); if (xmlDoc.Parse(_xmlString.c_str())) @@ -423,7 +435,7 @@ bool initString( return false; } - return initDoc(&xmlDoc, _sdf); + return initDoc(&xmlDoc, _config, _sdf); } ////////////////////////////////////////////////// @@ -452,7 +464,9 @@ inline tinyxml2::XMLElement *_initDocGetElement(tinyxml2::XMLDocument *_xmlDoc) } ////////////////////////////////////////////////// -bool initDoc(tinyxml2::XMLDocument *_xmlDoc, SDFPtr _sdf) +bool initDoc(tinyxml2::XMLDocument *_xmlDoc, + const ParserConfig &_config, + SDFPtr _sdf) { auto element = _initDocGetElement(_xmlDoc); if (!element) @@ -460,11 +474,13 @@ bool initDoc(tinyxml2::XMLDocument *_xmlDoc, SDFPtr _sdf) return false; } - return initXml(element, _sdf->Root()); + return initXml(element, _config, _sdf->Root()); } ////////////////////////////////////////////////// -bool initDoc(tinyxml2::XMLDocument *_xmlDoc, ElementPtr _sdf) +bool initDoc(tinyxml2::XMLDocument *_xmlDoc, + const ParserConfig &_config, + ElementPtr _sdf) { auto element = _initDocGetElement(_xmlDoc); if (!element) @@ -472,11 +488,13 @@ bool initDoc(tinyxml2::XMLDocument *_xmlDoc, ElementPtr _sdf) return false; } - return initXml(element, _sdf); + return initXml(element, _config, _sdf); } ////////////////////////////////////////////////// -bool initXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf) +bool initXml(tinyxml2::XMLElement *_xml, + const ParserConfig &_config, + ElementPtr _sdf) { const char *refString = _xml->Attribute("ref"); if (refString) @@ -594,7 +612,7 @@ bool initXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf) else { ElementPtr element(new Element); - initXml(child, element); + initXml(child, _config, element); _sdf->AddElementDescription(element); } } @@ -607,7 +625,7 @@ bool initXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf) ElementPtr element(new Element); - initFile(filename, element); + initFile(filename, _config, element); // override description for include elements tinyxml2::XMLElement *description = child->FirstChildElement("description"); @@ -634,7 +652,7 @@ SDFPtr readFile( { // Create and initialize the data structure that will hold the parsed SDF data sdf::SDFPtr sdfParsed(new sdf::SDF()); - sdf::init(sdfParsed); + sdf::init(sdfParsed, _config); // Read an SDF file, and store the result in sdfParsed. if (!sdf::readFile(_filename, _config, sdfParsed, _errors)) @@ -1510,7 +1528,7 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, ElementPtr refSDF; refSDF.reset(new Element); std::string refFilename = refSDFStr + ".sdf"; - initFile(refFilename, refSDF); + initFile(refFilename, _config, refSDF); _sdf->RemoveFromParent(); _sdf->Copy(refSDF); @@ -1575,12 +1593,12 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, // a new sdf pointer is created here by cloning a fresh sdf template // pointer instead of calling init every iteration. // SDFPtr includeSDF(new SDF); - // init(includeSDF); + // init(includeSDF, _config); static SDFPtr includeSDFTemplate; if (!includeSDFTemplate) { includeSDFTemplate.reset(new SDF); - init(includeSDFTemplate); + init(includeSDFTemplate, _config); } SDFPtr includeSDF(new SDF); includeSDF->Root(includeSDFTemplate->Root()->Clone()); @@ -1803,7 +1821,8 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, SourceLocation sourceLoc{includeXmlPath, _source, elemXml->GetLineNum()}; - insertIncludedElement(includeSDF, sourceLoc, toMerge, _sdf, _errors); + insertIncludedElement(includeSDF, sourceLoc, toMerge, _sdf, _config, + _errors); continue; } } @@ -1914,65 +1933,6 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, return true; } -///////////////////////////////////////////////// -void copyChildren(ElementPtr _sdf, - tinyxml2::XMLElement *_xml, - const bool _onlyUnknown) -{ - // Iterate over all the child elements - tinyxml2::XMLElement *elemXml = nullptr; - for (elemXml = _xml->FirstChildElement(); elemXml; - elemXml = elemXml->NextSiblingElement()) - { - std::string elemName = elemXml->Name(); - - if (_sdf->HasElementDescription(elemName)) - { - if (!_onlyUnknown) - { - sdf::ElementPtr element = _sdf->AddElement(elemName); - - // FIXME: copy attributes - for (const auto *attribute = elemXml->FirstAttribute(); - attribute; attribute = attribute->Next()) - { - element->GetAttribute(attribute->Name())->SetFromString( - attribute->Value()); - } - - // copy value - const char *value = elemXml->GetText(); - if (value) - { - element->GetValue()->SetFromString(value); - } - copyChildren(element, elemXml, _onlyUnknown); - } - } - else - { - ElementPtr element(new Element); - element->SetParent(_sdf); - element->SetName(elemName); - for (const tinyxml2::XMLAttribute *attribute = elemXml->FirstAttribute(); - attribute; attribute = attribute->Next()) - { - element->AddAttribute(attribute->Name(), "string", "", 1, ""); - element->GetAttribute(attribute->Name())->SetFromString( - attribute->Value()); - } - - if (elemXml->GetText() != nullptr) - { - element->AddValue("string", elemXml->GetText(), true); - } - - copyChildren(element, elemXml, _onlyUnknown); - _sdf->InsertElement(element); - } - } -} - ///////////////////////////////////////////////// bool convertFile(const std::string &_filename, const std::string &_version, SDFPtr _sdf) diff --git a/src/parser_private.hh b/src/parser_private.hh index c1b8d963f..70a5a5c5d 100644 --- a/src/parser_private.hh +++ b/src/parser_private.hh @@ -46,16 +46,24 @@ namespace sdf /// This actually forwards to initXml after converting the inputs /// \param[in] _xmlDoc TinyXML2 document containing the SDFormat description /// file that corresponds with the input SDFPtr + /// \param[in] _config Custom parser configuration /// \param[out] _sdf SDF interface to be initialized - bool initDoc(tinyxml2::XMLDocument *_xmlDoc, SDFPtr _sdf); + /// \return True on success, false on error. + bool initDoc(tinyxml2::XMLDocument *_xmlDoc, + const ParserConfig &_config, + SDFPtr _sdf); /// \brief Initialize the SDF Element using a TinyXML2 document /// /// This actually forwards to initXml after converting the inputs /// \param[in] _xmlDoc TinyXML2 document containing the SDFormat description /// file that corresponds with the input ElementPtr + /// \param[in] _config Custom parser configuration /// \param[out] _sdf SDF Element to be initialized - bool initDoc(tinyxml2::XMLDocument *_xmlDoc, ElementPtr _sdf); + /// \return True on success, false on error. + bool initDoc(tinyxml2::XMLDocument *_xmlDoc, + const ParserConfig &_config, + ElementPtr _sdf); /// \brief Initialize the SDF Element by parsing the SDFormat description in /// the input TinyXML2 element. This is where SDFormat spec/description files @@ -63,8 +71,12 @@ namespace sdf /// \remark For internal use only. Do not use this function. /// \param[in] _xml TinyXML2 element containing the SDFormat description /// file that corresponds with the input ElementPtr + /// \param[in] _config Custom parser configuration /// \param[out] _sdf SDF ElementPtr to be initialized - bool initXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf); + /// \return True on success, false on error. + bool initXml(tinyxml2::XMLElement *_xml, + const ParserConfig &_config, + ElementPtr _sdf); /// \brief Populate the SDF values from a TinyXML document bool readDoc(tinyxml2::XMLDocument *_xmlDoc, SDFPtr _sdf, @@ -83,6 +95,7 @@ namespace sdf /// \param[in] _xmlRoot Pointer to the root level TinyXML element. /// \param[in] _source Source of the XML document. /// \param[in] _errors Captures errors found during the checks. + /// \return True on success, false on error. bool checkXmlFromRoot(tinyxml2::XMLElement *_xmlRoot, const std::string &_source, Errors &_errors); diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 8a78389e5..d592c6c7b 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -58,7 +58,7 @@ set(tests world_dom.cc ) -if (PYTHONINTERP_FOUND AND PY_PSUTIL) +if (Python3_Interpreter_FOUND AND PY_PSUTIL) set(tests ${tests} element_memory_leak.cc) endif() diff --git a/test/integration/default_elements.cc b/test/integration/default_elements.cc index 7785d5490..b93821ac0 100644 --- a/test/integration/default_elements.cc +++ b/test/integration/default_elements.cc @@ -214,8 +214,10 @@ TEST(ExplicitlySetInFile, ToString) << " \n" << "\n"; - EXPECT_EQ(root.Element()->ToString(""), stream.str()); - EXPECT_EQ(root.Element()->ToString("", true, false), stream.str()); + sdf::PrintConfig config; + config.SetOutPrecision(6); + EXPECT_EQ(root.Element()->ToString("", config), stream.str()); + EXPECT_EQ(root.Element()->ToString("", true, false, config), stream.str()); stream.str(std::string()); stream @@ -249,5 +251,5 @@ TEST(ExplicitlySetInFile, ToString) << " \n" << "\n"; - EXPECT_EQ(root.Element()->ToString("", true, true), stream.str()); + EXPECT_EQ(root.Element()->ToString("", true, true, config), stream.str()); } diff --git a/test/integration/model/test_model_with_frames_no_rotations/model.config b/test/integration/model/test_model_with_frames_no_rotations/model.config new file mode 100644 index 000000000..33e622b95 --- /dev/null +++ b/test/integration/model/test_model_with_frames_no_rotations/model.config @@ -0,0 +1,6 @@ + + + + test_model_with_frames + model.sdf + diff --git a/test/integration/model/test_model_with_frames_no_rotations/model.sdf b/test/integration/model/test_model_with_frames_no_rotations/model.sdf new file mode 100644 index 000000000..769175115 --- /dev/null +++ b/test/integration/model/test_model_with_frames_no_rotations/model.sdf @@ -0,0 +1,69 @@ + + + + + 0.12345 0 0 0 0 0 + + + 0 1.2345 0 0 0 0 + + + + + + + + 1.5707963267948966 + + + + + + + + 0.78539816339744828 + + + + + + 1 0 0 0 0 0 + + + 0 1 0 0 0 0 + + + 0 0 1 0 0 0 + + + + L1 + L2 + + 0 0 1 + + + 1 0 0 + + + + 0 0 1 0 0 0 + L2 + L3 + + 0 0 1 + + + + 1 0 1 0 0 0 + L3 + L4 + + + 1 0 0 0 0 0 + + 1 0 0 0 0 0 + + + + diff --git a/test/integration/model/test_nested_model_with_frames/model.sdf b/test/integration/model/test_nested_model_with_frames/model.sdf index b28a42b4f..7716ebeea 100644 --- a/test/integration/model/test_nested_model_with_frames/model.sdf +++ b/test/integration/model/test_nested_model_with_frames/model.sdf @@ -3,8 +3,8 @@ - test_model_with_frames - 0 10 0 1.570796326794895 0 0 + test_model_with_frames_no_rotations + 0 10 0 0 0 0 diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 88886cb2b..ccb9ce0d5 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -698,9 +698,11 @@ TEST(DOMModel, IncludeModelWithPlugin) const sdf::Model *model = world->ModelByIndex(0); ASSERT_NE(nullptr, model); + sdf::OutputConfig outConfig; // Test ToElement with useInclude == true { - sdf::ElementPtr elem = model->ToElement(true); + outConfig.SetToElementUseIncludeTag(true); + sdf::ElementPtr elem = model->ToElement(outConfig); // There should be a uri ASSERT_TRUE(elem->HasElement("uri")); @@ -723,7 +725,8 @@ TEST(DOMModel, IncludeModelWithPlugin) // model SDF which would have two plugins, one from the tag and // one from the included model { - sdf::ElementPtr elem = model->ToElement(false); + outConfig.SetToElementUseIncludeTag(false); + sdf::ElementPtr elem = model->ToElement(outConfig); // There should NOT be a uri ASSERT_FALSE(elem->HasElement("uri")); diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index d6056274d..935345fc4 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -574,15 +574,15 @@ void prepareForDirectComparison(sdf::ElementPtr _worldElem) } ////////////////////////////////////////////////// -// Test parsing models with child models containg frames nested via +// Test parsing models with child models containing frames nested via // Compare parsed SDF with expected string TEST(NestedModel, NestedModelWithFramesDirectComparison) { - const std::string name = "test_model_with_frames"; + const std::string name = "test_model_with_frames_no_rotations"; const std::string modelPath = sdf::testing::TestFile("integration", "model", name); - const ignition::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); + const ignition::math::Pose3d model1Pose(10, 0, 0, 0, 0, 0); std::ostringstream stream; std::string version = "1.7"; @@ -731,7 +731,7 @@ TEST(NestedModel, TwoLevelNestedModelWithFramesDirectComparison) const std::string modelPath = sdf::testing::TestFile( "integration", "model", name); - const ignition::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); + const ignition::math::Pose3d model1Pose(10, 0, 0, 0, 0, 0); std::ostringstream stream; std::string version = "1.7"; diff --git a/test/integration/nested_model_with_frames_expected.sdf b/test/integration/nested_model_with_frames_expected.sdf index 04cf0cf89..078f6f7db 100644 --- a/test/integration/nested_model_with_frames_expected.sdf +++ b/test/integration/nested_model_with_frames_expected.sdf @@ -4,10 +4,10 @@ - 0 0 0 1.570796 0 0 + 0.12345 0 0 0 0 0 - 0 0 0 0 0.785398 0 + 0 1.2344999999999999 0 0 0 0 0 0 0 0 0 0 @@ -67,7 +67,7 @@ 1 0 0 0 0 0 - 10 0 0 0 0 1.5708 + 10 0 0 0 0 0 diff --git a/test/integration/pose_1_9_sdf.cc b/test/integration/pose_1_9_sdf.cc index d806401d7..c0c16f104 100644 --- a/test/integration/pose_1_9_sdf.cc +++ b/test/integration/pose_1_9_sdf.cc @@ -764,7 +764,9 @@ TEST(Pose1_9, ToStringWithoutAttrib) ASSERT_NE(nullptr, poseValueParam); EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); - std::string elemStr = poseElem->ToString(""); + sdf::PrintConfig config; + config.SetOutPrecision(6); + std::string elemStr = poseElem->ToString("", config); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); } @@ -786,7 +788,9 @@ TEST(Pose1_9, ToStringWithDegreesFalse) ASSERT_NE(nullptr, poseValueParam); EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); - std::string elemStr = poseElem->ToString(""); + sdf::PrintConfig config; + config.SetOutPrecision(6); + std::string elemStr = poseElem->ToString("", config); EXPECT_PRED2(contains, elemStr, "degrees='false'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); } @@ -809,7 +813,9 @@ TEST(Pose1_9, ToStringWithDegreesTrue) ASSERT_NE(nullptr, poseValueParam); EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); - std::string elemStr = poseElem->ToString(""); + sdf::PrintConfig config; + config.SetOutPrecision(6); + std::string elemStr = poseElem->ToString("", config); EXPECT_PRED2(contains, elemStr, "degrees='true'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); } @@ -833,7 +839,9 @@ TEST(Pose1_9, ToStringWithEulerRPY) ASSERT_NE(nullptr, poseValueParam); EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); - std::string elemStr = poseElem->ToString(""); + sdf::PrintConfig config; + config.SetOutPrecision(6); + std::string elemStr = poseElem->ToString("", config); EXPECT_PRED2(contains, elemStr, "rotation_format='euler_rpy'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); } @@ -861,7 +869,9 @@ TEST(Pose1_9, ToStringWithEulerRPYDegreesTrue) ASSERT_NE(nullptr, poseValueParam); EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.4 0.5 0.6")); - std::string elemStr = poseElem->ToString(""); + sdf::PrintConfig config; + config.SetOutPrecision(6); + std::string elemStr = poseElem->ToString("", config); EXPECT_PRED2(contains, elemStr, "degrees='true'"); EXPECT_PRED2(contains, elemStr, "rotation_format='euler_rpy'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); @@ -886,9 +896,11 @@ TEST(Pose1_9, ToStringWithQuatXYZ) ASSERT_NE(nullptr, poseValueParam); EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.7071068 0 0 0.7071068")); + sdf::PrintConfig config; + config.SetOutPrecision(6); // The string output has changed as it was parsed from the value, instead of // the original string. - std::string elemStr = poseElem->ToString(""); + std::string elemStr = poseElem->ToString("", config); EXPECT_PRED2(contains, elemStr, "rotation_format='quat_xyzw'"); EXPECT_PRED2(contains, elemStr, "0.707107 0 0 0.707107"); } @@ -916,9 +928,11 @@ TEST(Pose1_9, ToStringWithQuatXYZWDegreesFalse) ASSERT_NE(nullptr, poseValueParam); EXPECT_TRUE(poseValueParam->SetFromString("1 2 3 0.7071068 0 0 0.7071068")); + sdf::PrintConfig config; + config.SetOutPrecision(6); // The string output has changed as it was parsed from the value, instead of // the original string. - std::string elemStr = poseElem->ToString(""); + std::string elemStr = poseElem->ToString("", config); EXPECT_PRED2(contains, elemStr, "degrees='false'"); EXPECT_PRED2(contains, elemStr, "rotation_format='quat_xyzw'"); EXPECT_PRED2(contains, elemStr, "0.707107 0 0 0.707107"); @@ -939,7 +953,9 @@ TEST(Pose1_9, ToStringAfterChangingDegreeAttribute) ASSERT_NE(nullptr, valParam); ASSERT_TRUE(valParam->SetFromString("1 2 3 0.4 0.5 0.6")); - std::string elemStr = poseElem->ToString(""); + sdf::PrintConfig config; + config.SetOutPrecision(6); + std::string elemStr = poseElem->ToString("", config); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); // Changing to attribute to degrees, however this does not modify the @@ -950,13 +966,14 @@ TEST(Pose1_9, ToStringAfterChangingDegreeAttribute) ASSERT_TRUE(degreesAttrib->Set(true)); EXPECT_TRUE(valParam->Reparse()); - elemStr = poseElem->ToString(""); + elemStr = poseElem->ToString("", config); EXPECT_PRED2(contains, elemStr, "degrees='true'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); // Changing back to radians ASSERT_TRUE(degreesAttrib->Set(false)); - elemStr = poseElem->ToString(""); + EXPECT_TRUE(valParam->Reparse()); + elemStr = poseElem->ToString("", config); EXPECT_PRED2(contains, elemStr, "degrees='false'"); EXPECT_PRED2(contains, elemStr, "0.4 0.5 0.6"); } diff --git a/test/integration/print_config.cc b/test/integration/print_config.cc index 511b6b69e..12c6b8753 100644 --- a/test/integration/print_config.cc +++ b/test/integration/print_config.cc @@ -17,6 +17,8 @@ #include +#include +#include #include #include @@ -136,6 +138,7 @@ R"( )"; sdf::PrintConfig config; + config.SetOutPrecision(6); // by default, included model should be expanded EXPECT_EQ(includedModel->Element()->ToString("", config), expandedIncludeStr); EXPECT_EQ(modelWithInclude->Element()->ToString("", config), @@ -224,3 +227,190 @@ R"( EXPECT_EQ(includeMergedModel->Element()->ToString("", config), expectedIncludeMerge); } + +void PrecisionTest(const sdf::ElementPtr _elem, + const std::map &_expected) +{ + ASSERT_NE(_elem, nullptr); + + EXPECT_EQ(_elem->GetValue()->GetAsString(), _expected.at("default")); + + sdf::PrintConfig config; + config.SetOutPrecision(6); + EXPECT_EQ(_elem->GetValue()->GetAsString(config), _expected.at("6")); + + config.SetOutPrecision(2); + EXPECT_EQ(_elem->GetValue()->GetAsString(config), _expected.at("2")); + + config.SetOutPrecision(std::numeric_limits::max()); + EXPECT_EQ(_elem->GetValue()->GetAsString(config), _expected.at("default")); +} + +///////////////////////////////////////////////// +TEST(PrintConfig, OutPrecision) +{ + const std::string sdfStr = +R"( + + + + + + + 1 1.0 -1.5707963267948966 + 0.707106781 0 0 0.707106781 + + + + 1.23456789123456789 + + + + 0.15 0.6 0.87525741223854215 1 + 0.16 + + 0.15 + + + + 1.4 + + 0.15 + + +
0.1512345678912345678 1.4
+ + + + + + world + L1 + + 1.5707963267948966 0.34 0.56 + + + + + +)"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(sdfStr); + EXPECT_TRUE(errors.empty()) << errors; + + auto *world = root.WorldByIndex(0); + ASSERT_NE(world, nullptr); + auto *model = world->ModelByIndex(0); + ASSERT_NE(model, nullptr); + auto *link = model->LinkByIndex(0); + ASSERT_NE(link, nullptr); + auto *visual = link->VisualByIndex(0); + ASSERT_NE(visual, nullptr); + + + // Below we are comparing default (maximum) then setting the precision + // to 6 (std::ostream's default), 2, and then back to max. + // Used this IEEE 754 floating point converter to determine expected strings: + // https://baseconvert.com/ieee-754-floating-point + // double (64 bit) has max of 17 digits + // float (32 bit) has max of 9 digits + + // key: precision, value: expected string + std::map expected = { + {"default", ""}, + {"6", ""}, + {"2", ""} + }; + sdf::ElementPtr elem; + + // //pose -> type: ignition::math::Pose3d + { + SCOPED_TRACE("PrecisionTest: Pose3d"); + elem = visual->Element()->FindElement("pose"); + expected["default"] = + "1 1 -1.5707963267948966 " + "0.70710678100000002 0 0 0.70710678100000002"; + expected["6"] = "1 1 -1.5708 0.707107 0 0 0.707107"; + expected["2"] = "1 1 -1.6 0.71 0 0 0.71"; + + PrecisionTest(elem, expected); + } + + // //radius -> type: double + { + auto *geom = visual->Geom(); + ASSERT_NE(geom, nullptr); + auto *sphere = geom->SphereShape(); + ASSERT_NE(sphere, nullptr); + + SCOPED_TRACE("PrecisionTest: double"); + elem = sphere->Element()->FindElement("radius"); + expected["default"] = "1.2345678912345679"; + expected["6"] = "1.23457"; + expected["2"] = "1.2"; + + PrecisionTest(elem, expected); + } + + auto *material = visual->Material(); + ASSERT_NE(material, nullptr); + + // //material/diffuse -> type: ignition::math::Color (float values) + { + SCOPED_TRACE("PrecisionTest: Color"); + elem = material->Element()->FindElement("diffuse"); + expected["default"] = + "0.150000006 0.600000024 0.875257432 1"; + expected["6"] = "0.15 0.6 0.875257 1"; + expected["2"] = "0.15 0.6 0.88 1"; + + PrecisionTest(elem, expected); + } + + // //material/render_order -> type: float + { + SCOPED_TRACE("PrecisionTest: float"); + elem = material->Element()->FindElement("render_order"); + expected["default"] = "0.159999996"; + expected["6"] = "0.16"; + expected["2"] = "0.16"; + + PrecisionTest(elem, expected); + } + + // //camera/distortion/center -> type: ignition::math::Vector2d + { + auto *sensor = link->SensorByIndex(0); + ASSERT_NE(sensor, nullptr); + auto *camera = sensor->CameraSensor(); + ASSERT_NE(camera, nullptr); + elem = camera->Element()->FindElement("distortion"); + ASSERT_NE(elem, nullptr); + + SCOPED_TRACE("PrecisionTest: Vector2d"); + elem = elem->FindElement("center"); + expected["default"] = "0.15123456789123457 1.3999999999999999"; + expected["6"] = "0.151235 1.4"; + expected["2"] = "0.15 1.4"; + + PrecisionTest(elem, expected); + } + + // //joint/axis/xyz -> type: ignition::math::Vector3d + { + auto *joint = model->JointByIndex(0); + ASSERT_NE(joint, nullptr); + auto *axis = joint->Axis(0); + ASSERT_NE(axis, nullptr); + + SCOPED_TRACE("PrecisionTest: Vector3d"); + elem = axis->Element()->FindElement("xyz"); + expected["default"] = + "1.5707963267948966 0.34000000000000002 0.56000000000000005"; + expected["6"] = "1.5708 0.34 0.56"; + expected["2"] = "1.6 0.34 0.56"; + + PrecisionTest(elem, expected); + } +} diff --git a/test/integration/surface_dom.cc b/test/integration/surface_dom.cc index 27d9d09a3..22de924ff 100644 --- a/test/integration/surface_dom.cc +++ b/test/integration/surface_dom.cc @@ -50,4 +50,10 @@ TEST(DOMSurface, Shapes) ASSERT_NE(nullptr, boxCol->Surface()); ASSERT_NE(nullptr, boxCol->Surface()->Contact()); EXPECT_EQ(boxCol->Surface()->Contact()->CollideBitmask(), 0xAB); + EXPECT_DOUBLE_EQ(boxCol->Surface()->Friction()->ODE()->Mu(), 0.6); + EXPECT_DOUBLE_EQ(boxCol->Surface()->Friction()->ODE()->Mu2(), 0.7); + EXPECT_DOUBLE_EQ(boxCol->Surface()->Friction()->ODE()->Slip1(), 4); + EXPECT_DOUBLE_EQ(boxCol->Surface()->Friction()->ODE()->Slip2(), 5); + EXPECT_EQ(boxCol->Surface()->Friction()->ODE()->Fdir1(), + ignition::math::Vector3d(1.2, 3.4, 5.6)); } diff --git a/test/integration/two_level_nested_model_with_frames_expected.sdf b/test/integration/two_level_nested_model_with_frames_expected.sdf index 34bc32674..a0b97a805 100644 --- a/test/integration/two_level_nested_model_with_frames_expected.sdf +++ b/test/integration/two_level_nested_model_with_frames_expected.sdf @@ -5,10 +5,10 @@ - 0 0 0 1.570796 0 0 + 0.12345 0 0 0 0 0 - 0 0 0 0 0.785398 0 + 0 1.2344999999999999 0 0 0 0 0 0 0 0 0 0 @@ -68,9 +68,9 @@ 1 0 0 0 0 0 - 0 10 0 1.570796326794895 0 0 + 0 10 0 0 0 0 - 10 0 0 0 0 1.5708 + 10 0 0 0 0 0 diff --git a/test/sdf/ellipsoid_model.sdf b/test/sdf/ellipsoid_model.sdf new file mode 100644 index 000000000..d848d8270 --- /dev/null +++ b/test/sdf/ellipsoid_model.sdf @@ -0,0 +1,38 @@ + + + + 0 3.0 0.5 0 0 0 + + + + 0.068 + 0 + 0 + 0.058 + 0 + 0.026 + + 1.0 + + + + + 0.2 0.3 0.5 + + + + + + + 0.2 0.3 0.5 + + + + 1 0 1 1 + 1 0 1 1 + 1 0 1 1 + + + + + diff --git a/test/sdf/inertial_stats.sdf b/test/sdf/inertial_stats.sdf new file mode 100644 index 000000000..d8d6df8a7 --- /dev/null +++ b/test/sdf/inertial_stats.sdf @@ -0,0 +1,131 @@ + + + + + + 0 0 0 0 0 0 + + + 5 0 0 0 0 0 + + 6.0 + + 1 + 1 + 1 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + -5 0 0 0 0 0 + + 6.0 + + 1 + 1 + 1 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + 0 5 0 0 0 0 + + 6.0 + + 1 + 1 + 1 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + 0 -5 0 0 0 0 + + 6.0 + + 1 + 1 + 1 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index fa21449e6..922b6d747 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -12,6 +12,15 @@ 0xAB + + + 0.6 + 0.7 + 4 + 5 + 1.2 3.4 5.6 + + diff --git a/test/usd/double_pendulum.usda b/test/usd/double_pendulum.usda new file mode 100644 index 000000000..2b175b5ff --- /dev/null +++ b/test/usd/double_pendulum.usda @@ -0,0 +1,688 @@ +#usda 1.0 +( + upAxis = "Z" +) + +def "double_pendulum" +{ + def PhysicsScene "physics" + { + } + + def Xform "ground_plane" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, -0.125) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "link" ( + prepend apiSchemas = ["PhysicsMassAPI"] + ) + { + float physics:mass = 1 + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "visual" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cube "geometry" + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + rel material:binding = + double size = 1 + float3 xformOp:scale = (100, 100, 0.25) + uniform token[] xformOpOrder = ["xformOp:scale"] + } + } + } + } + + def Xform "double_pendulum_with_base" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "base" ( + prepend apiSchemas = ["PhysicsMassAPI"] + ) + { + float physics:mass = 100 + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "vis_plate_on_ground" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0.01) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cylinder "geometry" + { + float3[] extent = [(-0.8, -0.8, -0.01), (0.8, 0.8, 0.01)] + double height = 0.02 + rel material:binding = + double radius = 0.8 + } + } + + def Xform "vis_pole" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (-0.275, 0, 1.1) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cube "geometry" + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + rel material:binding = + double size = 1 + float3 xformOp:scale = (0.2, 0.2, 2.2) + uniform token[] xformOpOrder = ["xformOp:scale"] + } + } + } + + def Xform "upper_link" ( + prepend apiSchemas = ["PhysicsMassAPI"] + ) + { + float physics:mass = 1 + float3 xformOp:rotateXYZ = (-90.00021, 0, 0) + double3 xformOp:translate = (0, 0, 2.1) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "vis_upper_joint" + { + float3 xformOp:rotateXYZ = (180, 89.99979, 180) + double3 xformOp:translate = (-0.05, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cylinder "geometry" + { + float3[] extent = [(-0.1, -0.1, -0.15), (0.1, 0.1, 0.15)] + double height = 0.3 + rel material:binding = + double radius = 0.1 + } + } + + def Xform "vis_lower_joint" + { + float3 xformOp:rotateXYZ = (180, 89.99979, 180) + double3 xformOp:translate = (0, 0, 1) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cylinder "geometry" + { + float3[] extent = [(-0.1, -0.1, -0.1), (0.1, 0.1, 0.1)] + double height = 0.2 + rel material:binding = + double radius = 0.1 + } + } + + def Xform "vis_cylinder" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0.5) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cylinder "geometry" + { + float3[] extent = [(-0.1, -0.1, -0.45), (0.1, 0.1, 0.45)] + double height = 0.9 + rel material:binding = + double radius = 0.1 + } + } + } + + def Xform "lower_link" ( + prepend apiSchemas = ["PhysicsMassAPI"] + ) + { + float physics:mass = 1 + float3 xformOp:rotateXYZ = (-114.59156, 0, 0) + double3 xformOp:translate = (0.25, 1, 2.1) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "vis_lower_joint" + { + float3 xformOp:rotateXYZ = (180, 89.99979, 180) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cylinder "geometry" + { + float3[] extent = [(-0.08, -0.08, -0.15), (0.08, 0.08, 0.15)] + double height = 0.3 + rel material:binding = + double radius = 0.08 + } + } + + def Xform "vis_cylinder" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0.5) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cylinder "geometry" + { + float3[] extent = [(-0.1, -0.1, -0.45), (0.1, 0.1, 0.45)] + double height = 0.9 + rel material:binding = + double radius = 0.1 + } + } + } + + def PhysicsRevoluteJoint "upper_joint" + { + uniform token physics:axis = "X" + prepend rel physics:body0 = + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0, 2.1) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0.70710546, -0.7071081, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -5.729578e17 + float physics:upperLimit = 5.729578e17 + } + + def PhysicsRevoluteJoint "lower" + { + uniform token physics:axis = "X" + prepend rel physics:body0 = + prepend rel physics:body1 = + point3f physics:localPos0 = (0.25, -0.0000036732051, 1) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0.9770616, -0.21295662, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -5.729578e17 + float physics:upperLimit = 5.729578e17 + } + } +} + +def Scope "Looks" +{ + def Material "Material_0" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_1" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_2" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_3" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_4" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_5" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_6" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_7" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } +} diff --git a/test/usd/materials/textures/FANS_Albedo.png b/test/usd/materials/textures/FANS_Albedo.png new file mode 100644 index 000000000..e69de29bb diff --git a/test/usd/materials/textures/FANS_Metalness.png b/test/usd/materials/textures/FANS_Metalness.png new file mode 100644 index 000000000..e69de29bb diff --git a/test/usd/materials/textures/FANS_Normal.png b/test/usd/materials/textures/FANS_Normal.png new file mode 100644 index 000000000..e69de29bb diff --git a/test/usd/materials/textures/FANS_Roughness.png b/test/usd/materials/textures/FANS_Roughness.png new file mode 100644 index 000000000..e69de29bb diff --git a/test/usd/nested_transforms.usda b/test/usd/nested_transforms.usda new file mode 100644 index 000000000..32e46edf4 --- /dev/null +++ b/test/usd/nested_transforms.usda @@ -0,0 +1,36 @@ +#usda 1.0 +( + metersPerUnit = 0.01 + upAxis = "Z" +) + +def "transforms" +{ + def Xform "nested_transforms_XYZ" + { + float3 xformOp:rotateXYZ = (0, 0, 90) + double3 xformOp:translate = (1, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "child_transform" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (1, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + } + } + + def Xform "nested_transforms_ZYX" + { + float3 xformOp:rotateZYX = (90, 0, 0) + double3 xformOp:translate = (1, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] + + def Xform "child_transform" + { + float3 xformOp:rotateZYX = (0, 0, 0) + double3 xformOp:translate = (1, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] + } + } +} diff --git a/test/usd/sensors.usda b/test/usd/sensors.usda new file mode 100644 index 000000000..b9a6f6005 --- /dev/null +++ b/test/usd/sensors.usda @@ -0,0 +1,34 @@ +#usda 1.0 +( + metersPerUnit = 0.01 + upAxis = "Z" +) + +def Lidar "lidar" +{ + bool highLod = 0 + float horizontalFov = 360 + float horizontalResolution = 0.4 + float maxRange = 100 + float minRange = 0.4 + float rotationRate = 0 + float verticalFov = 30 + float verticalResolution = 4 + float3 xformOp:rotateZYX = (0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-6, 0, 44) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX", "xformOp:scale"] + float yawOffset = 0 +} + +def Camera "camera" ( + kind = "model" +) +{ + float focalLength = 24 + float focusDistance = 400 + float3 xformOp:rotateZYX = (360, 285.3, 270) + float3 xformOp:scale = (1, 1.0000027, 1.0000008) + float3 xformOp:translate = (-183.61661, 0, 77.35324) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX", "xformOp:scale"] +} diff --git a/test/usd/upAxisY.usda b/test/usd/upAxisY.usda new file mode 100644 index 000000000..000ea5db7 --- /dev/null +++ b/test/usd/upAxisY.usda @@ -0,0 +1,198 @@ +#usda 1.0 +( + metersPerUnit = 1.0 + upAxis = "Y" +) + +def "shapes" +{ + def Xform "ground_plane" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, -0.125) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "link" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "visual" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cube "geometry" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + ) + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + rel material:binding = + double size = 1 + float3 xformOp:scale = (100, 100, 0.25) + uniform token[] xformOpOrder = ["xformOp:scale"] + } + } + } + } + + def Xform "box" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0.5) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "box_link" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.16666, 0.16666, 0.16666) + float physics:mass = 1 + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "box_visual" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cube "geometry" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + ) + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + rel material:binding = + double size = 1 + float3 xformOp:scale = (1, 1, 1) + uniform token[] xformOpOrder = ["xformOp:scale"] + } + } + } + } + + def Scope "Looks" + { + def Material "Material_0" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_1" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (1, 0, 0) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + } +} diff --git a/test/usd/upAxisZ.usda b/test/usd/upAxisZ.usda new file mode 100644 index 000000000..1474fd7fe --- /dev/null +++ b/test/usd/upAxisZ.usda @@ -0,0 +1,790 @@ +#usda 1.0 +( + metersPerUnit = 0.01 + upAxis = "Z" +) + +def PhysicsScene "physics" +{ + vector3f physics:gravityDirection = (0, 0, -1) + float physics:gravityMagnitude = 9.8 +} + +def DistantLight "defaultLight" ( + prepend apiSchemas = ["ShapingAPI"] + kind = "model" +) +{ + float angle = 1 + float inputs:diffuse = 0.5 + float inputs:intensity = 5000 + float shaping:cone:angle = 180 + float3 xformOp:rotateZYX = (0, 45, 0) + float3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] +} + +def DiskLight "diskLight" ( + prepend apiSchemas = ["ShapingAPI"] + kind = "model" +) +{ + float inputs:intensity = 3000 + float inputs:specular = 0.5 + float3 xformOp:rotateZYX = (0, 0, 45) + float3 xformOp:translate = (0, 0, 1000) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] +} + +def Xform "ground_plane" +{ + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, -0.125) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "link" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "visual" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cube "geometry" + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + rel material:binding = + double size = 1 + float3 xformOp:scale = (100, 100, 0.25) + uniform token[] xformOpOrder = ["xformOp:scale"] + } + def Cube "collision" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + ) + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + rel material:binding = + double size = 1 + float3 xformOp:scale = (100, 100, 0.25) + uniform token[] xformOpOrder = ["xformOp:scale"] + } + } + } +} + +def Xform "box" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI"] +) +{ + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0.5) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "box_link" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.16666, 0.16666, 0.16666) + float physics:mass = 1 + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + float3 xformOp:scale = (1, 0.1, 1) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ", "xformOp:scale"] + + def Xform "box_visual" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cube "geometry" + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + rel material:binding = + double size = 1 + float3 xformOp:scale = (1, 0.1, 0.2) + uniform token[] xformOpOrder = ["xformOp:scale"] + } + def Cube "collision" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + ) + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + rel material:binding = + double size = 1 + float3 xformOp:scale = (1, 1, 1) + uniform token[] xformOpOrder = ["xformOp:scale"] + } + } + } +} + +def Xform "cylinder" +{ + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, -1.5, 0.5) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "cylinder_link" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.1458, 0.1458, 0.125) + float physics:mass = 1.7 + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "cylinder_visual" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cylinder "geometry" + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + double height = 1 + rel material:binding = + double radius = 0.5 + } + def Cylinder "collision" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + ) + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + double height = 1 + rel material:binding = + double radius = 0.5 + } + } + } +} + +def Xform "sphere" +{ + float3 xformOp:rotateZYX = (-69, 31, -62) + double3 xformOp:translate = (0, 1.5, 0.5) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] + + def Xform "sphere_link" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.1, 0.1, 0.1) + float physics:mass = 2 + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "sphere_visual" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Sphere "geometry" + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + rel material:binding = + double radius = 0.5 + } + def Sphere "collision" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + ) + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + rel material:binding = + double radius = 0.5 + } + } + } +} + +def Xform "capsule" +{ + float3 xformOp:rotateZYX = (15, 80, -55) + double3 xformOp:translate = (0, -3, 0.5) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] + + def Xform "capsule_link" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.074154, 0.074154, 0.018769) + float physics:mass = 1 + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "capsule_visual" + { + float3 xformOp:rotateZYX = (0, 0, 90) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] + + def Capsule "geometry" + { + float3[] extent = [(-0.2, -0.2, -0.5), (0.2, 0.2, 0.5)] + double height = 0.6 + rel material:binding = + double radius = 0.2 + } + def Capsule "collision" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + ) + { + float3[] extent = [(-0.2, -0.2, -0.5), (0.2, 0.2, 0.5)] + double height = 0.6 + rel material:binding = + double radius = 0.2 + } + } + } +} + +def Xform "ellipsoid" +{ + float3 xformOp:rotateXYZ = (15, 80, -55) + double3 xformOp:translate = (0, 3, 0.5) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "ellipsoid_link" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.068, 0.058, 0.026) + float physics:mass = 1 + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "ellipsoid_visual" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Sphere "geometry" + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + rel material:binding = + double radius = 0.5 + float3 xformOp:scale = (0.4, 0.6, 1) + uniform token[] xformOpOrder = ["xformOp:scale"] + } + def Sphere "collision" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + ) + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + rel material:binding = + double radius = 0.5 + float3 xformOp:scale = (0.4, 0.6, 1) + uniform token[] xformOpOrder = ["xformOp:scale"] + } + } + } +} + +def DistantLight "sun" +{ + float inputs:intensity = 1000 + float intensity = 100 + float3 xformOp:rotateXYZ = (0, -35, 0) + double3 xformOp:translate = (0, 0, 10) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] +} + +def Scope "Looks" +{ + def Material "Material_0" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_1" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (1, 0, 0) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_2" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0, 1, 0) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_3" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0, 0, 1) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_4" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (1, 1, 0) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_5" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (1, 0, 1) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_textures" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (1, 1, 1) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + asset inputs:diffuse_texture = @materials/textures/FANS_Albedo.png@ ( + colorSpace = "auto" + customData = { + asset default = @@ + } + displayGroup = "Albedo" + displayName = "Base Map" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + float inputs:metallic_constant = 0.5 ( + customData = { + double default = 0.5 + dictionary range = { + int max = 1 + int min = 0 + } + } + displayGroup = "Reflectivity" + displayName = "Metallic Amount" + doc = "Metallic Material" + ) + asset inputs:metallic_texture = @materials/textures/FANS_Metalness.png@ ( + colorSpace = "raw" + customData = { + asset default = @@ + } + displayGroup = "Reflectivity" + displayName = "Metallic Map" + ) + asset inputs:normalmap_texture = @materials/textures/FANS_Normal.png@ ( + colorSpace = "raw" + customData = { + asset default = @@ + } + displayGroup = "Normal" + displayName = "Normal Map" + ) + float inputs:reflection_roughness_constant = 0.5 ( + customData = { + double default = 0.5 + dictionary range = { + int max = 1 + int min = 0 + } + } + displayGroup = "Reflectivity" + displayName = "Roughness Amount" + doc = "Higher roughness values lead to more blurry reflections" + ) + bool inputs:reflection_roughness_texture_influence = 1 ( + colorSpace = "raw" + customData = { + int default = 0 + dictionary range = { + int max = 1 + int min = 0 + } + } + displayGroup = "Reflectivity" + displayName = "Roughness Map Influence" + ) + asset inputs:reflectionroughness_texture = @materials/textures/FANS_Roughness.png@ ( + colorSpace = "raw" + customData = { + asset default = @@ + } + displayGroup = "RoughnessMap" + displayName = "RoughnessMap Map" + ) + token outputs:out + } + } +} diff --git a/test/usd/upAxis_wrong.usda b/test/usd/upAxis_wrong.usda new file mode 100644 index 000000000..f35d392d9 --- /dev/null +++ b/test/usd/upAxis_wrong.usda @@ -0,0 +1,5 @@ +#usda 1.0 +( + metersPerUnit = 0.009999999776482582 + upAxis = "X" +) diff --git a/usd/include/sdf/usd/UsdError.hh b/usd/include/sdf/usd/UsdError.hh index 28742e2d6..763c81040 100644 --- a/usd/include/sdf/usd/UsdError.hh +++ b/usd/include/sdf/usd/UsdError.hh @@ -69,6 +69,24 @@ namespace sdf /// \brief Invalid material INVALID_MATERIAL, + + /// \brief Invalid usd file + INVALID_USD_FILE, + + /// \brief Invalid up axis + INVALID_UP_AXIS, + + /// \brief Prim is missing a particular attribute + PRIM_MISSING_ATTRIBUTE, + + /// \brief Prim is of the incorrect schema type + PRIM_INCORRECT_SCHEMA_TYPE, + + /// \brief Parsing of USD object to a SDF object failed. + USD_TO_SDF_PARSING_ERROR, + + /// \brief Parsing of USD polygin object to a SDF object failed. + USD_TO_SDF_POLYGON_PARSING_ERROR, }; class IGNITION_SDFORMAT_USD_VISIBLE UsdError diff --git a/usd/include/sdf/usd/usd_parser/Parser.hh b/usd/include/sdf/usd/usd_parser/Parser.hh new file mode 100644 index 000000000..6f638cb99 --- /dev/null +++ b/usd/include/sdf/usd/usd_parser/Parser.hh @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#ifndef SDF_USD_USD_PARSER_PARSER_HH +#define SDF_USD_USD_PARSER_PARSER_HH + +#include + +#include "sdf/config.hh" +#include "sdf/usd/Export.hh" +#include "sdf/usd/UsdError.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse a USD file and convert it to a SDF file + /// \param[in] _inputFilenameUsd Path of the USD file to parse + /// \param[in] _outputFilenameSdf Path where the SDF file will be located + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when parsing the USD file to its SDF representation. + UsdErrors IGNITION_SDFORMAT_USD_VISIBLE parseUSDFile( + const std::string &_inputFilenameUsd, + const std::string &_outputFilenameSdf); + } + } +} +#endif diff --git a/usd/include/sdf/usd/usd_parser/USDData.hh b/usd/include/sdf/usd/usd_parser/USDData.hh new file mode 100644 index 000000000..9936858cb --- /dev/null +++ b/usd/include/sdf/usd/usd_parser/USDData.hh @@ -0,0 +1,118 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#ifndef SDF_USD_USD_PARSER_USDDATA_HH_ +#define SDF_USD_USD_PARSER_USDDATA_HH_ + +#include +#include +#include +#include +#include + +#include + +#include "sdf/Material.hh" +#include "sdf/Types.hh" +#include "sdf/sdf_config.h" +#include "sdf/system_util.hh" +#include "sdf/usd/usd_parser/USDStage.hh" +#include "sdf/usd/Export.hh" +#include "sdf/usd/UsdError.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief this class will handle the data of a stage + /// It will parse the materials in the stage + /// If the stage has some references to other stages, this class + /// will read them and make this data available here too. + class IGNITION_SDFORMAT_USD_VISIBLE USDData + { + /// \brief Constructor + public: explicit USDData(const std::string &_filename); + + /// \brief Initialize the data inside the class with the stage + /// defined in the constructor + /// \return A vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: UsdErrors Init(); + + /// \brief If a stage contains substages, this will allow to include + /// them. + /// \return A vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: UsdErrors AddStage(const std::string &_ref); + + /// \brief Read materials + /// \return A vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: UsdErrors ParseMaterials(); + + /// \brief Get all materials readed in the stage + public: const std::unordered_map & + Materials() const; + + /// \brief Get all sdformat models inside the stage + public: const std::set &Models() const; + + /// \brief Get all stages (the main one and all the referenced). + public: const std::unordered_map> & + AllReferences() const; + + /// \brief Find a path and get the data + /// \param[in] _name Name of the path to find + /// \return A pair with the name of the stage and the data + public: const std::pair> + FindStage(const std::string &_name) const; + + public: friend std::ostream& operator<<( + std::ostream& os, const USDData& data) + { + os << "References:" << "\n"; + for (auto &ref : data.AllReferences()) + { + os << "\t" << ref.first << "\n"; + os << "\t\t" << ref.second->UpAxis() << "\n"; + os << "\t\t" << ref.second->MetersPerUnit() << "\n"; + for (auto &path : ref.second->USDPaths()) + { + os << "\t\tPath " << path << '\n'; + } + } + os << "Models:" << "\n"; + auto models = data.Models(); + for (const auto &model : models) + { + os << "\t" << model << "\n"; + } + + return os; + } + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } +} +} + +#endif diff --git a/usd/include/sdf/usd/usd_parser/USDStage.hh b/usd/include/sdf/usd/usd_parser/USDStage.hh new file mode 100644 index 000000000..2bc8f5d25 --- /dev/null +++ b/usd/include/sdf/usd/usd_parser/USDStage.hh @@ -0,0 +1,69 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#ifndef SDF_USD_USD_PARSER_USDSTAGE_HH_ +#define SDF_USD_USD_PARSER_USDSTAGE_HH_ + +#include +#include + +#include + +#include "sdf/Types.hh" +#include "sdf/sdf_config.h" +#include "sdf/system_util.hh" +#include "sdf/usd/Export.hh" +#include "sdf/usd/UsdError.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief this class will keep the data of a stage + /// - UpAxis + /// - MetersPerUnit + /// - All USD paths + class IGNITION_SDFORMAT_USD_VISIBLE USDStage + { + /// \brief Default constructor + /// \param[in] _refFileName File name of the stage in the disk + public: explicit USDStage(const std::string &_refFileName); + + /// \brief Initialize the data structure + /// \return A vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: UsdErrors Init(); + + /// \brief Get stage up axis + public: const std::string &UpAxis() const; + + /// \brief Get stage meter per unit + public: double MetersPerUnit() const; + + /// \brief Get USD paths available in the stage + public: const std::set &USDPaths() const; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif // SDF_USD_PARSER_USDSTAGE_HH diff --git a/usd/include/sdf/usd/usd_parser/USDTransforms.hh b/usd/include/sdf/usd/usd_parser/USDTransforms.hh new file mode 100644 index 000000000..4b0d75da5 --- /dev/null +++ b/usd/include/sdf/usd/usd_parser/USDTransforms.hh @@ -0,0 +1,116 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#ifndef SDF_USD_USD_PARSER_USDTRANSFORMS_HH_ +#define SDF_USD_USD_PARSER_USDTRANSFORMS_HH_ + +#include +#include + +#include +#include +#include + +#include + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/sdf_config.h" +#include "sdf/system_util.hh" +#include "sdf/usd/Export.hh" +#include "sdf/usd/UsdError.hh" +#include "USDData.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief This class stores the transforms of a schema + /// This might contain scale, translate or rotation operations + /// The booleans are used to check if there is a transform defined + /// in the schema + class IGNITION_SDFORMAT_USD_VISIBLE UDSTransforms + { + /// \brief Default constructor + public: UDSTransforms(); + + /// \brief Translate + /// \return A 3D vector with the translation + public: const ignition::math::Vector3d Translation() const; + + /// \brief Scale + /// \return A 3D vector with the scale + public: const ignition::math::Vector3d Scale() const; + + /// \brief Get the Rotation + /// \return Return The rotation, if one exists. If no rotation exists, + /// std::nullopt is returned + public: const std::optional Rotation() const; + + /// \brief Set translate + /// \param[in] _translate Translate to set + public: void SetTranslation(const ignition::math::Vector3d &_translate); + + /// \brief Set scale + /// \param[in] _scale Scale to set + public: void SetScale(const ignition::math::Vector3d &_scale); + + /// \brief Set rotation + /// \param[in] _q Quaternion that defines the rotation + public: void SetRotation(const ignition::math::Quaterniond &_q); + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + + /// \brief This function gets the transform from a prim to the specified + /// _schemaToStop variable + /// \param[in] _prim Initial prim to read the transform + /// \param[in] _usdData USDData structure to get info about the prim, for + /// example: metersperunit + /// \param[out] _pose Pose of the prim. From _prim to _schemaToStop. + /// \param[out] _scale The scale of the prim + /// \param[in] _schemaToStop Name of the prim where the loop will stop + /// reading transforms + void IGNITION_SDFORMAT_USD_VISIBLE GetTransform( + const pxr::UsdPrim &_prim, + const USDData &_usdData, + ignition::math::Pose3d &_pose, + ignition::math::Vector3d &_scale, + const std::string &_schemaToStop); + + /// \brief Read the usd prim transforms. Scale, rotation or transform might + /// be defined as float or doubles + /// \param[in] _prim Prim where the transforms are read + /// \return A USDTransforms class with all the transforms related to + /// the prim + UDSTransforms IGNITION_SDFORMAT_USD_VISIBLE ParseUSDTransform( + const pxr::UsdPrim &_prim); +} +} +} +#endif diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index 29b4ef4b5..95df04d71 100644 --- a/usd/src/CMakeLists.txt +++ b/usd/src/CMakeLists.txt @@ -1,6 +1,7 @@ set(sources Conversions.cc UsdError.cc + sdf_parser/Collision.cc sdf_parser/Geometry.cc sdf_parser/Joint.cc sdf_parser/Light.cc @@ -10,6 +11,19 @@ set(sources sdf_parser/Sensor.cc sdf_parser/Visual.cc sdf_parser/World.cc + usd_parser/Parser.cc + usd_parser/polygon_helper.cc + usd_parser/USD2SDF.cc + usd_parser/USDData.cc + usd_parser/USDJoints.cc + usd_parser/USDLights.cc + usd_parser/USDLinks.cc + usd_parser/USDMaterial.cc + usd_parser/USDPhysics.cc + usd_parser/USDSensors.cc + usd_parser/USDStage.cc + usd_parser/USDTransforms.cc + usd_parser/USDWorld.cc ) ign_add_component(usd SOURCES ${sources} GET_TARGET_NAME usd_target) @@ -28,16 +42,23 @@ target_link_libraries(${usd_target} set(gtest_sources sdf_parser/sdf2usd_TEST.cc + usd_parser/usd2sdf_TEST.cc sdf_parser/Geometry_Sdf2Usd_TEST.cc sdf_parser/Joint_Sdf2Usd_TEST.cc sdf_parser/Light_Sdf2Usd_TEST.cc sdf_parser/Link_Sdf2Usd_TEST.cc sdf_parser/Material_Sdf2Usd_TEST.cc - # TODO(adlarkin) add a test for SDF -> USD models once model parsing - # functionality is complete sdf_parser/Sensors_Sdf2Usd_TEST.cc sdf_parser/Visual_Sdf2Usd_TEST.cc sdf_parser/World_Sdf2Usd_TEST.cc + usd_parser/USDData_TEST.cc + usd_parser/USDPhysics_TEST.cc + usd_parser/USDJoints_TEST.cc + usd_parser/USDLinks_TEST.cc + usd_parser/USDLight_TEST.cc + usd_parser/USDSensors_TEST.cc + usd_parser/USDStage_TEST.cc + usd_parser/USDTransforms_TEST.cc Conversions_TEST.cc UsdError_TEST.cc UsdUtils_TEST.cc @@ -47,8 +68,68 @@ set(gtest_sources ign_build_tests( TYPE UNIT SOURCES ${gtest_sources} - LIB_DEPS ${usd_target} ignition-cmake${IGN_CMAKE_VER}::utilities - INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test + LIB_DEPS ${usd_target} + INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test usd_parser ) +if (TARGET UNIT_USDLinks_TEST) + target_sources(UNIT_USDLinks_TEST PRIVATE + usd_parser/USDLinks.cc + Conversions.cc + usd_parser/polygon_helper.cc) +endif() + +if (TARGET UNIT_USDSensors_TEST) + target_sources(UNIT_USDSensors_TEST PRIVATE + usd_parser/USDSensors.cc) +endif() + +if (TARGET UNIT_USDJoints_TEST) + target_sources(UNIT_USDJoints_TEST PRIVATE + usd_parser/USDJoints.cc) +endif() + +if (TARGET UNIT_USDPhysics_TEST) + target_sources(UNIT_USDPhysics_TEST PRIVATE + usd_parser/USDPhysics.cc) +endif() + +if (TARGET UNIT_USDTransport_TEST) + target_sources(UNIT_USDTransport_TEST PRIVATE + usd_parser/USDTransforms.cc) +endif() + +if (TARGET UNIT_Conversions_TEST) + target_sources(UNIT_Conversions_TEST PRIVATE + Conversions.cc) +endif() + +set(sdf2usd_test_sources + Conversions.cc + sdf_parser/Collision.cc + sdf_parser/Geometry.cc + sdf_parser/Joint.cc + sdf_parser/Light.cc + sdf_parser/Link.cc + sdf_parser/Material.cc + sdf_parser/Model.cc + sdf_parser/Sensor.cc + sdf_parser/Visual.cc +) + +if (TARGET UNIT_Joint_Sdf2Usd_TEST) + target_sources(UNIT_Joint_Sdf2Usd_TEST PRIVATE + ${sdf2usd_test_sources}) +endif() + +if (TARGET UNIT_Light_Sdf2Usd_TEST) + target_sources(UNIT_Light_Sdf2Usd_TEST PRIVATE + ${sdf2usd_test_sources}) +endif() + +if (TARGET UNIT_Material_Sdf2Usd_TEST) + target_sources(UNIT_Material_Sdf2Usd_TEST PRIVATE + ${sdf2usd_test_sources}) +endif() + add_subdirectory(cmd) diff --git a/usd/src/Conversions.cc b/usd/src/Conversions.cc index 5386893b3..6fa9a0ac6 100644 --- a/usd/src/Conversions.cc +++ b/usd/src/Conversions.cc @@ -15,7 +15,7 @@ * */ -#include "sdf/usd/Conversions.hh" +#include "Conversions.hh" #include @@ -43,7 +43,14 @@ namespace sdf out.SetNormalMap(pbr->NormalMap()); sdf::Pbr pbrOut; sdf::PbrWorkflow pbrWorkflow; - pbrWorkflow.SetAlbedoMap(pbr->AlbedoMap()); + if (!pbr->AlbedoMap().empty()) + { + pbrWorkflow.SetAlbedoMap(pbr->AlbedoMap()); + } + else + { + pbrWorkflow.SetAlbedoMap(_in->TextureImage()); + } pbrWorkflow.SetMetalnessMap(pbr->MetalnessMap()); pbrWorkflow.SetEmissiveMap(pbr->EmissiveMap()); pbrWorkflow.SetRoughnessMap(pbr->RoughnessMap()); @@ -66,13 +73,15 @@ namespace sdf pbr->NormalMap(), sdf::NormalMapSpace::OBJECT); } - if (pbr->Type() == ignition::common::PbrType::METAL) + if (pbr->Type() == ignition::common::PbrType::SPECULAR) { - pbrOut.SetWorkflow(sdf::PbrWorkflowType::METAL, pbrWorkflow); + pbrWorkflow.SetType(sdf::PbrWorkflowType::SPECULAR); + pbrOut.SetWorkflow(sdf::PbrWorkflowType::SPECULAR, pbrWorkflow); } - else if (pbr->Type() == ignition::common::PbrType::SPECULAR) + else { - pbrOut.SetWorkflow(sdf::PbrWorkflowType::SPECULAR, pbrWorkflow); + pbrWorkflow.SetType(sdf::PbrWorkflowType::METAL); + pbrOut.SetWorkflow(sdf::PbrWorkflowType::METAL, pbrWorkflow); } out.SetPbrMaterial(pbrOut); } diff --git a/usd/include/sdf/usd/Conversions.hh b/usd/src/Conversions.hh similarity index 93% rename from usd/include/sdf/usd/Conversions.hh rename to usd/src/Conversions.hh index b1409a7af..9905c256a 100644 --- a/usd/include/sdf/usd/Conversions.hh +++ b/usd/src/Conversions.hh @@ -25,7 +25,6 @@ #include "sdf/Material.hh" #include "sdf/sdf_config.h" -#include "sdf/usd/Export.hh" namespace sdf { @@ -38,14 +37,12 @@ namespace sdf /// to a SDF material /// \param[in] _in Ignition Common Material. /// \return SDF material. - IGNITION_SDFORMAT_USD_VISIBLE sdf::Material convert(const ignition::common::Material *_in); /// \brief Specialized conversion from an SDF material to a Ignition Common /// material. /// \param[in] _in SDF material. /// \param[out] _out The Ignition Common Material. - IGNITION_SDFORMAT_USD_VISIBLE void convert(const sdf::Material &_in, ignition::common::Material &_out); } } diff --git a/usd/src/Conversions_TEST.cc b/usd/src/Conversions_TEST.cc index e5c092146..bfcaeb99f 100644 --- a/usd/src/Conversions_TEST.cc +++ b/usd/src/Conversions_TEST.cc @@ -25,7 +25,7 @@ #include "sdf/Material.hh" #include "sdf/Pbr.hh" -#include "sdf/usd/Conversions.hh" +#include "Conversions.hh" ///////////////////////////////////////////////// TEST(Conversions, SdfToCommonMaterial) diff --git a/usd/src/UsdTestUtils.hh b/usd/src/UsdTestUtils.hh index d9156277d..7a8e07af0 100644 --- a/usd/src/UsdTestUtils.hh +++ b/usd/src/UsdTestUtils.hh @@ -224,7 +224,6 @@ void CheckInertial(const pxr::UsdPrim &_usdPrim, } EXPECT_TRUE(checkedCOM); - EXPECT_EQ(_isRigid, _usdPrim.HasAPI()); EXPECT_EQ(_isRigid, _usdPrim.HasAPI()); } } // namespace testing diff --git a/usd/src/UsdUtils.hh b/usd/src/UsdUtils.hh index f989cda67..cbc79d278 100644 --- a/usd/src/UsdUtils.hh +++ b/usd/src/UsdUtils.hh @@ -18,6 +18,8 @@ #ifndef SDF_USD_SDF_PARSER_UTILS_HH_ #define SDF_USD_SDF_PARSER_UTILS_HH_ +#include + #include #include #include @@ -34,6 +36,8 @@ #include #pragma pop_macro ("__DEPRECATED") +#include + #include "sdf/Collision.hh" #include "sdf/Error.hh" #include "sdf/Geometry.hh" @@ -51,6 +55,31 @@ namespace sdf // namespace usd { + /// \brief Return a valid USD path + /// Path must not: + /// - start with a digit + /// - Contain spaces + /// - Contain dots + /// - Contain dashes + /// \param[in] _path Path to check + /// \return A valid path + inline std::string validPath(const std::string &_path) + { + std::string result; + if (_path.empty()) + { + return result; + } + result = ignition::common::replaceAll(_path, " ", ""); + result = ignition::common::replaceAll(result, ".", "_"); + result = ignition::common::replaceAll(result, "-", "_"); + if (std::isdigit(result[0])) + { + result = "_" + result; + } + return result; + } + /// \brief Get an object's pose w.r.t. its parent. /// \param[in] _obj The object whose pose should be computed/retrieved. /// \param[out] _pose The pose of _obj w.r.t. its parent. diff --git a/usd/src/UsdUtils_TEST.cc b/usd/src/UsdUtils_TEST.cc index 3bf8031f7..b52850a7e 100644 --- a/usd/src/UsdUtils_TEST.cc +++ b/usd/src/UsdUtils_TEST.cc @@ -112,3 +112,23 @@ TEST(UsdUtils, IsPlane) mutableModel->SetStatic(false); EXPECT_FALSE(sdf::usd::IsPlane(*mutableModel)); } + +////////////////////////////////////////////////// +TEST(UsdUtils, validPath) +{ + const std::string alreadyValid = "/valid/path"; + EXPECT_EQ(alreadyValid, sdf::usd::validPath(alreadyValid)); + + EXPECT_EQ("", sdf::usd::validPath("")); + + EXPECT_EQ("_0/start/with/digit", sdf::usd::validPath("0/start/with/digit")); + + EXPECT_EQ("/hasSpaces", sdf::usd::validPath("/has Spaces")); + + EXPECT_EQ("/has_period", sdf::usd::validPath("/has.period")); + + EXPECT_EQ("/has_dash", sdf::usd::validPath("/has-dash")); + + EXPECT_EQ("_5/has_period/hasSpace/has_dash", + sdf::usd::validPath("5/has.period/has Space/has-dash")); +} diff --git a/usd/src/cmd/CMakeLists.txt b/usd/src/cmd/CMakeLists.txt index 46b6ca8a3..9b03d0671 100644 --- a/usd/src/cmd/CMakeLists.txt +++ b/usd/src/cmd/CMakeLists.txt @@ -1,17 +1,40 @@ if(TARGET ${usd_target}) add_executable(sdf2usd sdf2usd.cc + ../sdf_parser/Collision.cc + ../sdf_parser/Model.cc + ../sdf_parser/Joint.cc + ../sdf_parser/Link.cc + ../sdf_parser/Geometry.cc + ../sdf_parser/Material.cc + ../sdf_parser/Sensor.cc + ../sdf_parser/Visual.cc + ../sdf_parser/Light.cc + ../Conversions.cc ) target_link_libraries(sdf2usd PUBLIC ignition-utils${IGN_UTILS_VER}::ignition-utils${IGN_UTILS_VER} + ignition-utils${IGN_UTILS_VER}::cli + ${usd_target} + ) + + add_executable(usd2sdf + usd2sdf.cc + ) + + target_link_libraries(usd2sdf + PUBLIC + ignition-utils${IGN_UTILS_VER}::ignition-utils${IGN_UTILS_VER} + ignition-utils${IGN_UTILS_VER}::cli ${usd_target} ) install( TARGETS sdf2usd + usd2sdf DESTINATION ${BIN_INSTALL_DIR} ) diff --git a/usd/src/cmd/sdf2usd.cc b/usd/src/cmd/sdf2usd.cc index 02ab81579..9aa41a0c7 100644 --- a/usd/src/cmd/sdf2usd.cc +++ b/usd/src/cmd/sdf2usd.cc @@ -15,7 +15,8 @@ * */ -#include +#include +#include #include #include @@ -33,6 +34,8 @@ #include "sdf/sdf.hh" #include "sdf/usd/sdf_parser/World.hh" +#include "../sdf_parser/Model.hh" +#include "../UsdUtils.hh" ////////////////////////////////////////////////// /// \brief Enumeration of available commands @@ -223,11 +226,74 @@ void runCommand(const Options &_opt) exit(-2); } - // only support SDF files with exactly 1 world for now + // only support SDF files with exactly 1 world or 1 model for now if (root.WorldCount() != 1u) { - std::cerr << _opt.inputFilename << " does not have exactly 1 world\n"; - exit(-3); + auto model = root.Model(); + if (model != nullptr) + { + std::string pathInputFile = + ignition::common::parentPath(_opt.inputFilename); + if (pathInputFile.empty() || pathInputFile == _opt.inputFilename) + { + pathInputFile = ignition::common::cwd(); + } + auto systemPaths = ignition::common::systemPaths(); + systemPaths->AddFilePaths(pathInputFile); + + // This loop here will add all the directories inside the sdf file. + // For example: If we download a model from fuel, textures might live in + // in the same path but in a different folder: materials/textures, + // this loop will add these two folders to the systempaths allowing the + // cmd to find the resources. + std::vector pathList = {pathInputFile}; + while (!pathList.empty()) + { + std::string pathToAdd = pathList.back(); + pathList.pop_back(); + for (ignition::common::DirIter file(pathToAdd); + file != ignition::common::DirIter(); ++file) + { + std::string current(*file); + if (ignition::common::isDirectory(current)) + { + systemPaths->AddFilePaths(current); + pathList.push_back(current); + } + } + } + + auto stage = pxr::UsdStage::CreateInMemory(); + std::string modelName = model->Name(); + modelName = sdf::usd::validPath(modelName); + auto modelPath = std::string("/" + modelName); + auto usdErrors = sdf::usd::ParseSdfModel( + *model, + stage, + modelPath, + pxr::SdfPath(modelPath)); + if (!usdErrors.empty()) + { + std::cerr << "The following errors occurred when parsing model [" + << modelName << "]:" << std::endl; + for (const auto &e : usdErrors) + std::cout << e << "\n"; + exit(-5); + } + + if (!stage->GetRootLayer()->Export(_opt.outputFilename)) + { + std::cerr << "Issue saving USD to " << _opt.outputFilename << "\n"; + exit(-6); + } + return; + } + else + { + std::cerr << _opt.inputFilename << " does not have exactly 1 world " + << "or 1 model\n"; + exit(-3); + } } auto world = root.WorldByIndex(0u); @@ -240,7 +306,8 @@ void runCommand(const Options &_opt) auto stage = pxr::UsdStage::CreateInMemory(); - const auto worldPath = std::string("/" + world->Name()); + auto worldPath = std::string("/" + world->Name()); + worldPath = sdf::usd::validPath(worldPath); auto usdErrors = sdf::usd::ParseSdfWorld(*world, stage, worldPath); if (!usdErrors.empty()) { @@ -264,7 +331,8 @@ void addFlags(CLI::App &_app) _app.add_option("input", opt->inputFilename, - "Input filename. Defaults to input.sdf unless otherwise specified."); + "Input filename. Defaults to input.sdf unless otherwise specified." + "Input file might be a world or a model"); _app.add_option("output", opt->outputFilename, diff --git a/usd/src/cmd/usd2sdf.cc b/usd/src/cmd/usd2sdf.cc new file mode 100644 index 000000000..dc0c3106d --- /dev/null +++ b/usd/src/cmd/usd2sdf.cc @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2022 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 +#include + +#include + +#include "sdf/usd/usd_parser/Parser.hh" +#include "sdf/usd/UsdError.hh" +#include "sdf/config.hh" + +////////////////////////////////////////////////// +/// \brief Enumeration of available commands +enum class Command +{ + kNone, +}; + +////////////////////////////////////////////////// +/// \brief Structure to hold all filenames +struct Options +{ + /// \brief Command to execute + Command command{Command::kNone}; + + /// \brief input filename + std::string inputFilename{"input.usd"}; + + /// \brief output filename + std::string outputFilename{"output.sdf"}; +}; + +void runCommand(const Options &_opt) +{ + const auto errors = + sdf::usd::parseUSDFile(_opt.inputFilename, _opt.outputFilename); + if (!errors.empty()) + { + std::cerr << "Errors occurred when generating [" << _opt.outputFilename + << "] from [" << _opt.inputFilename << "]:\n"; + for (const auto &e : errors) + std::cerr << "\t" << e << "\n"; + } +} + +void addFlags(CLI::App &_app) +{ + auto opt = std::make_shared(); + + _app.add_option("input", + opt->inputFilename, + "Input filename. Defaults to input.usd unless otherwise specified."); + + _app.add_option("output", + opt->outputFilename, + "Output filename. Defaults to output.sdf unless otherwise specified."); + + _app.callback([&_app, opt](){ + runCommand(*opt); + }); +} + +////////////////////////////////////////////////// +int main(int argc, char** argv) +{ + CLI::App app{"USD to SDF converter"}; + + app.set_help_all_flag("--help-all", "Show all help"); + + app.add_flag_callback("--version", [](){ + std::cout << SDF_VERSION_FULL << std::endl; + throw CLI::Success(); + }); + + addFlags(app); + CLI11_PARSE(app, argc, argv); + + return 0; +} diff --git a/usd/src/sdf_parser/Collision.cc b/usd/src/sdf_parser/Collision.cc new file mode 100644 index 000000000..38bcf4d45 --- /dev/null +++ b/usd/src/sdf_parser/Collision.cc @@ -0,0 +1,129 @@ +/* + * Copyright (C) 2022 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 "Collision.hh" + +#include + +#include + +// TODO(adlarkin) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Collision.hh" +#include "../UsdUtils.hh" +#include "Geometry.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ + UsdErrors ParseSdfCollision(const sdf::Collision &_collision, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + const pxr::SdfPath sdfCollisionPath(_path); + auto usdCollisionXform = pxr::UsdGeomXform::Define( + _stage, sdfCollisionPath); + if (!usdCollisionXform) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_USD_DEFINITION, + "Not able to define a Geom Xform at path [" + _path + "]")); + return errors; + } + + // Purpose is a builtin attribute with 4 posible values. + // - *default* indicates that a prim has “no special purpose” and should + // generally be included in all traversals + // - *render* should generally only be included when performing a + // “final quality” render + // - *proxy* should generally only be included when performing a lightweight + // proxy render (such as OpenGL) + // - *guide* should generally only be included when an interactive + // application has been explicitly asked to “show guides” + // For collisions we need to define this as guide because we don't need + // to render anything + auto collisionPrim = _stage->GetPrimAtPath(sdfCollisionPath); + collisionPrim.CreateAttribute(pxr::TfToken("purpose"), + pxr::SdfValueTypeNames->Token, false).Set(pxr::TfToken("guide")); + + ignition::math::Pose3d pose; + auto poseErrors = usd::PoseWrtParent(_collision, pose); + if (!poseErrors.empty()) + { + for (const auto &e : poseErrors) + errors.push_back(e); + return errors; + } + + poseErrors = usd::SetPose(pose, _stage, sdfCollisionPath); + if (!poseErrors.empty()) + { + for (const auto &e : poseErrors) + errors.push_back(e); + errors.push_back(UsdError(UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Unable to set the pose of the prim corresponding to the " + "SDF collision named [" + _collision.Name() + "]")); + return errors; + } + + const auto geometry = *(_collision.Geom()); + const auto geometryPath = std::string(_path + "/geometry"); + auto geomErrors = ParseSdfGeometry(geometry, _stage, geometryPath); + if (!geomErrors.empty()) + { + errors.insert(errors.end(), geomErrors.begin(), geomErrors.end()); + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing geometry attached to _collision [" + + _collision.Name() + "]")); + return errors; + } + + auto geomPrim = _stage->GetPrimAtPath(pxr::SdfPath(geometryPath)); + if (!geomPrim) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Internal error: unable to get prim at path [" + + geometryPath + "], but a geom prim should exist at this path")); + return errors; + } + + if (!pxr::UsdPhysicsCollisionAPI::Apply(geomPrim)) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_PRIM_API_APPLY, + "Internal error: unable to apply a collision to the prim at path [" + + geometryPath + "]")); + } + + return errors; + } +} +} +} diff --git a/usd/src/sdf_parser/Collision.hh b/usd/src/sdf_parser/Collision.hh new file mode 100644 index 000000000..e4641ed5b --- /dev/null +++ b/usd/src/sdf_parser/Collision.hh @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#ifndef SDF_USD_SDF_PARSER_COLLISION_HH_ +#define SDF_USD_SDF_PARSER_COLLISION_HH_ + +#include + +// TODO(adlarkin) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Collision.hh" +#include "sdf/config.hh" +#include "sdf/usd/UsdError.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse an SDF collision into a USD stage. + /// \param[in] _collision The SDF collision to parse. + /// \param[in] _stage The stage that should contain the USD representation + /// of _collision. + /// \param[in] _path The USD path of the parsed collision in _stage, + /// which must be a valid USD path. + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error. + UsdErrors ParseSdfCollision( + const sdf::Collision &_collision, + pxr::UsdStageRefPtr &_stage, + const std::string &_path); + } + } +} + +#endif diff --git a/usd/src/sdf_parser/Geometry.cc b/usd/src/sdf_parser/Geometry.cc index 775a1d592..47f7ee201 100644 --- a/usd/src/sdf_parser/Geometry.cc +++ b/usd/src/sdf_parser/Geometry.cc @@ -15,7 +15,7 @@ * */ -#include "sdf/usd/sdf_parser/Geometry.hh" +#include "Geometry.hh" #include @@ -45,7 +45,6 @@ #include #include #include -#include #include #include #pragma pop_macro ("__DEPRECATED") @@ -58,9 +57,9 @@ #include "sdf/Mesh.hh" #include "sdf/Plane.hh" #include "sdf/Sphere.hh" -#include "sdf/usd/Conversions.hh" -#include "sdf/usd/sdf_parser/Material.hh" +#include "Material.hh" +#include "../Conversions.hh" #include "../UsdUtils.hh" namespace sdf @@ -351,7 +350,7 @@ namespace usd else primName = _path + "/submesh_" + std::to_string(i); - primName = ignition::common::replaceAll(primName, "-", "_"); + primName = sdf::usd::validPath(primName); auto usdMesh = pxr::UsdGeomMesh::Define(_stage, pxr::SdfPath(primName)); if (!usdMesh) @@ -600,40 +599,6 @@ namespace usd "Geometry type is either invalid or not supported"))); } - // Set the collision for this geometry. - // In SDF, the collisions of a link are defined separately from - // the link's visual geometries (in case a user wants to define a simpler - // collision, for example). In USD, the collision can be attached to the - // geometry so that the collision shape is the geometry shape. - // The current approach that's taken here (attaching a collision to the - // geometry) assumes that for every visual in a link, the visual should have - // a collision that matches its geometry. This approach currently ignores - // collisions defined in a link since it instead creates collisions for a - // link that match a link's visual geometries. - // TODO(adlarkin) support the option of a different collision shape (i.e., - // don't ignore collisions defined in a link), - // especially for meshes - here's more information about how to do this: - // https://graphics.pixar.com/usd/release/wp_rigid_body_physics.html?highlight=collision#turning-meshes-into-shapes - if (errors.empty()) - { - auto geomPrim = _stage->GetPrimAtPath(pxr::SdfPath(_path)); - if (!geomPrim) - { - errors.push_back(UsdError(sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, - "Internal error: unable to get prim at path [" - + _path + "], but a geom prim should exist at this path")); - return errors; - } - - if (!pxr::UsdPhysicsCollisionAPI::Apply(geomPrim)) - { - errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_PRIM_API_APPLY, - "Internal error: unable to apply a collision to the prim at path [" - + _path + "]")); - return errors; - } - } - return errors; } } diff --git a/usd/include/sdf/usd/sdf_parser/Geometry.hh b/usd/src/sdf_parser/Geometry.hh similarity index 95% rename from usd/include/sdf/usd/sdf_parser/Geometry.hh rename to usd/src/sdf_parser/Geometry.hh index 524da8fa3..c2fc7f728 100644 --- a/usd/include/sdf/usd/sdf_parser/Geometry.hh +++ b/usd/src/sdf_parser/Geometry.hh @@ -31,7 +31,6 @@ #include "sdf/Geometry.hh" #include "sdf/config.hh" -#include "sdf/usd/Export.hh" #include "sdf/usd/UsdError.hh" namespace sdf @@ -49,7 +48,7 @@ namespace sdf /// must be a valid USD path. /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError /// includes an error code and message. An empty vector indicates no error. - UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfGeometry( + UsdErrors ParseSdfGeometry( const sdf::Geometry &_geometry, pxr::UsdStageRefPtr &_stage, const std::string &_path); } diff --git a/usd/src/sdf_parser/Geometry_Sdf2Usd_TEST.cc b/usd/src/sdf_parser/Geometry_Sdf2Usd_TEST.cc index e167491e9..3dd7d5f5d 100644 --- a/usd/src/sdf_parser/Geometry_Sdf2Usd_TEST.cc +++ b/usd/src/sdf_parser/Geometry_Sdf2Usd_TEST.cc @@ -153,15 +153,23 @@ TEST_F(UsdStageFixture, Geometry) const std::string groundPlaneVisualPath = groundPlaneLinkPath + "/visual"; const std::string groundPlaneGeometryPath = groundPlaneVisualPath + "/geometry"; + const std::string groundPlaneCollisionPath = + groundPlaneLinkPath + "/collision"; + const std::string groundPlaneCollisionGeometryPath = + groundPlaneCollisionPath + "/geometry"; pxr::GfVec3f scale; pxr::VtArray extent; double size, height, radius, length; + const auto groundPlaneCollisionGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(groundPlaneCollisionGeometryPath)); + ASSERT_TRUE(groundPlaneCollisionGeometry); + EXPECT_TRUE( + groundPlaneCollisionGeometry.HasAPI()); const auto groundPlaneGeometry = this->stage->GetPrimAtPath( pxr::SdfPath(groundPlaneGeometryPath)); ASSERT_TRUE(groundPlaneGeometry); - EXPECT_TRUE(groundPlaneGeometry.HasAPI()); const auto usdGroundPlane = pxr::UsdGeomCube(groundPlaneGeometry); ASSERT_TRUE(usdGroundPlane); usdGroundPlane.GetSizeAttr().Get(&size); @@ -182,8 +190,14 @@ TEST_F(UsdStageFixture, Geometry) const std::string boxGeometryPath = boxVisualPath + "/geometry"; const auto boxGeometry = this->stage->GetPrimAtPath( pxr::SdfPath(boxGeometryPath)); + const std::string boxCollisionPath = boxLinkPath + "/collision"; + const std::string boxCollisionGeometryPath = boxCollisionPath + "/geometry"; + const auto boxCollisionGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(boxCollisionGeometryPath)); + ASSERT_TRUE(boxCollisionGeometry); + EXPECT_TRUE(boxCollisionGeometry.HasAPI()); + ASSERT_TRUE(boxGeometry); - EXPECT_TRUE(boxGeometry.HasAPI()); const auto usdCube = pxr::UsdGeomCube(boxGeometry); ASSERT_TRUE(usdCube); usdCube.GetSizeAttr().Get(&size); @@ -201,10 +215,16 @@ TEST_F(UsdStageFixture, Geometry) const std::string cylinderLinkPath = cylinderPath + "/link"; const std::string cylinderVisualPath = cylinderLinkPath + "/visual"; const std::string cylinderGeometryPath = cylinderVisualPath + "/geometry"; + const std::string cylinderCollisionPath = cylinderLinkPath + "/collision"; + const std::string cylinderCollisionGeometryPath = + cylinderCollisionPath + "/geometry"; + const auto cylinderCollisionGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(cylinderCollisionGeometryPath)); + ASSERT_TRUE(cylinderCollisionGeometry); + EXPECT_TRUE(cylinderCollisionGeometry.HasAPI()); const auto cylinderGeometry = this->stage->GetPrimAtPath( pxr::SdfPath(cylinderGeometryPath)); ASSERT_TRUE(cylinderGeometry); - EXPECT_TRUE(cylinderGeometry.HasAPI()); const auto usdCylinder = pxr::UsdGeomCylinder(cylinderGeometry); ASSERT_TRUE(usdCylinder); usdCylinder.GetRadiusAttr().Get(&radius); @@ -220,10 +240,16 @@ TEST_F(UsdStageFixture, Geometry) const std::string sphereLinkPath = spherePath + "/link"; const std::string sphereVisualPath = sphereLinkPath + "/sphere_vis"; const std::string sphereGeometryPath = sphereVisualPath + "/geometry"; + const std::string sphereCollisionPath = sphereLinkPath + "/collision"; + const std::string sphereCollisionGeometryPath = + sphereCollisionPath + "/geometry"; + const auto sphereCollisionGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(sphereCollisionGeometryPath)); + ASSERT_TRUE(sphereCollisionGeometry); + EXPECT_TRUE(sphereCollisionGeometry.HasAPI()); const auto sphereGeometry = this->stage->GetPrimAtPath( pxr::SdfPath(sphereGeometryPath)); ASSERT_TRUE(sphereGeometry); - EXPECT_TRUE(sphereGeometry.HasAPI()); const auto usdSphere = pxr::UsdGeomSphere(sphereGeometry); ASSERT_TRUE(usdSphere); usdSphere.GetRadiusAttr().Get(&radius); @@ -239,7 +265,6 @@ TEST_F(UsdStageFixture, Geometry) const auto capsuleGeometry = this->stage->GetPrimAtPath( pxr::SdfPath(capsuleGeometryPath)); ASSERT_TRUE(capsuleGeometry); - EXPECT_TRUE(capsuleGeometry.HasAPI()); const auto usdCapsule = pxr::UsdGeomCapsule(capsuleGeometry); ASSERT_TRUE(usdCapsule); usdCapsule.GetRadiusAttr().Get(&radius); @@ -256,10 +281,16 @@ TEST_F(UsdStageFixture, Geometry) const std::string meshVisualPath = meshLinkPath + "/visual"; const std::string meshGeometryPath = meshVisualPath + "/geometry"; const std::string meshGeometryMeshPath = meshGeometryPath + "/Cube"; + const std::string capsuleCollisionPath = capsuleLinkPath + "/collision"; + const std::string capsuleCollisionGeometryPath = + capsuleCollisionPath + "/geometry"; + const auto capsuleCollisionGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(capsuleCollisionGeometryPath)); + ASSERT_TRUE(capsuleCollisionGeometry); + EXPECT_TRUE(capsuleCollisionGeometry.HasAPI()); const auto meshGeometryParentPrim = this->stage->GetPrimAtPath( pxr::SdfPath(meshGeometryPath)); ASSERT_TRUE(meshGeometryParentPrim); - EXPECT_TRUE(meshGeometryParentPrim.HasAPI()); const auto meshGeometry = this->stage->GetPrimAtPath( pxr::SdfPath(meshGeometryMeshPath)); ASSERT_TRUE(meshGeometry); diff --git a/usd/src/sdf_parser/Joint.cc b/usd/src/sdf_parser/Joint.cc index 62a6b0ef6..909a4c172 100644 --- a/usd/src/sdf_parser/Joint.cc +++ b/usd/src/sdf_parser/Joint.cc @@ -15,7 +15,7 @@ * */ -#include "sdf/usd/sdf_parser/Joint.hh" +#include "Joint.hh" #include diff --git a/usd/include/sdf/usd/sdf_parser/Joint.hh b/usd/src/sdf_parser/Joint.hh similarity index 96% rename from usd/include/sdf/usd/sdf_parser/Joint.hh rename to usd/src/sdf_parser/Joint.hh index e452d6c51..2e91031d3 100644 --- a/usd/include/sdf/usd/sdf_parser/Joint.hh +++ b/usd/src/sdf_parser/Joint.hh @@ -34,7 +34,6 @@ #include "sdf/Joint.hh" #include "sdf/Model.hh" #include "sdf/config.hh" -#include "sdf/usd/Export.hh" #include "sdf/usd/UsdError.hh" namespace sdf @@ -59,7 +58,7 @@ namespace sdf /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError /// includes an error code and message. An empty vector indicates no errors /// occurred when parsing _joint to its USD representation. - UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfJoint( + UsdErrors ParseSdfJoint( const sdf::Joint &_joint, pxr::UsdStageRefPtr &_stage, const std::string &_path, const sdf::Model &_parentModel, diff --git a/usd/src/sdf_parser/Joint_Sdf2Usd_TEST.cc b/usd/src/sdf_parser/Joint_Sdf2Usd_TEST.cc index 8f44ccfcd..853c99f8e 100644 --- a/usd/src/sdf_parser/Joint_Sdf2Usd_TEST.cc +++ b/usd/src/sdf_parser/Joint_Sdf2Usd_TEST.cc @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -49,9 +50,9 @@ #include "sdf/JointAxis.hh" #include "sdf/Model.hh" #include "sdf/Root.hh" -#include "sdf/usd/sdf_parser/Model.hh" #include "test_config.h" #include "test_utils.hh" +#include "Model.hh" ///////////////////////////////////////////////// // Fixture that creates a USD stage for each test case. @@ -332,6 +333,13 @@ TEST_F(UsdJointStageFixture, RevoluteJoints) checkedJoints++; } EXPECT_EQ(checkedJoints, 2); + + // the model prim that has revolute joints should be marked as a + // pxr::UsdPhysicsAtriculationRootAPI + const auto modelPrim = + this->stage->GetPrimAtPath(pxr::SdfPath(this->modelPath)); + ASSERT_TRUE(modelPrim); + EXPECT_TRUE(modelPrim.HasAPI()); } ///////////////////////////////////////////////// @@ -383,6 +391,13 @@ TEST_F(UsdJointStageFixture, JointParentIsWorld) checkedJoints++; } EXPECT_EQ(checkedJoints, 1); + + // the model prim doesn't have revolute joints, so it shouldn't be marked as a + // pxr::UsdPhysicsAtriculationRootAPI + const auto modelPrim = + this->stage->GetPrimAtPath(pxr::SdfPath(this->modelPath)); + ASSERT_TRUE(modelPrim); + EXPECT_FALSE(modelPrim.HasAPI()); } ///////////////////////////////////////////////// diff --git a/usd/src/sdf_parser/Light.cc b/usd/src/sdf_parser/Light.cc index 35bd26b3f..7b2f702f9 100644 --- a/usd/src/sdf_parser/Light.cc +++ b/usd/src/sdf_parser/Light.cc @@ -15,7 +15,7 @@ * */ -#include "sdf/usd/sdf_parser/Light.hh" +#include "Light.hh" #include diff --git a/usd/include/sdf/usd/sdf_parser/Light.hh b/usd/src/sdf_parser/Light.hh similarity index 95% rename from usd/include/sdf/usd/sdf_parser/Light.hh rename to usd/src/sdf_parser/Light.hh index 697116032..e1ed51c21 100644 --- a/usd/include/sdf/usd/sdf_parser/Light.hh +++ b/usd/src/sdf_parser/Light.hh @@ -30,7 +30,6 @@ #pragma pop_macro ("__DEPRECATED") #include "sdf/config.hh" -#include "sdf/usd/Export.hh" #include "sdf/usd/UsdError.hh" #include "sdf/Light.hh" @@ -49,7 +48,7 @@ namespace sdf /// be a valid USD path. /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError /// includes an error code and message. An empty vector indicates no error. - UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfLight( + UsdErrors ParseSdfLight( const sdf::Light &_light, pxr::UsdStageRefPtr &_stage, const std::string &_path); diff --git a/usd/src/sdf_parser/Light_Sdf2Usd_TEST.cc b/usd/src/sdf_parser/Light_Sdf2Usd_TEST.cc index 297ef4f8a..929a29cb4 100644 --- a/usd/src/sdf_parser/Light_Sdf2Usd_TEST.cc +++ b/usd/src/sdf_parser/Light_Sdf2Usd_TEST.cc @@ -37,10 +37,10 @@ #include "sdf/Light.hh" #include "sdf/Root.hh" #include "sdf/World.hh" -#include "sdf/usd/sdf_parser/Light.hh" -#include "sdf/usd/sdf_parser/Model.hh" #include "test_config.h" #include "test_utils.hh" +#include "Light.hh" +#include "Model.hh" #include "../UsdTestUtils.hh" #include "../UsdUtils.hh" diff --git a/usd/src/sdf_parser/Link.cc b/usd/src/sdf_parser/Link.cc index 973cd5bf1..fed5c332a 100644 --- a/usd/src/sdf_parser/Link.cc +++ b/usd/src/sdf_parser/Link.cc @@ -15,7 +15,7 @@ * */ -#include "sdf/usd/sdf_parser/Link.hh" +#include "Link.hh" #include @@ -36,10 +36,11 @@ #pragma pop_macro ("__DEPRECATED") #include "sdf/Link.hh" -#include "sdf/usd/sdf_parser/Light.hh" -#include "sdf/usd/sdf_parser/Sensor.hh" -#include "sdf/usd/sdf_parser/Visual.hh" #include "../UsdUtils.hh" +#include "Collision.hh" +#include "Light.hh" +#include "Sensor.hh" +#include "Visual.hh" namespace sdf { @@ -88,8 +89,9 @@ namespace usd if (!pxr::UsdPhysicsRigidBodyAPI::Apply(linkPrim)) { errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_PRIM_API_APPLY, - "Internal error: unable to mark link at path [" + _path - + "] as a rigid body, so mass properties won't be attached")); + "Internal error: unable to mark model at path [" + + linkPrim.GetPath().GetString() + "] as a rigid body, " + "so mass properties won't be attached")); return errors; } @@ -129,7 +131,8 @@ namespace usd for (uint64_t i = 0; i < _link.VisualCount(); ++i) { const auto visual = *(_link.VisualByIndex(i)); - const auto visualPath = std::string(_path + "/" + visual.Name()); + auto visualPath = std::string(_path + "/" + visual.Name()); + visualPath = sdf::usd::validPath(visualPath); auto errorsLink = ParseSdfVisual(visual, _stage, visualPath); if (!errorsLink.empty()) { @@ -141,11 +144,32 @@ namespace usd } } + // parse all of the link's collisions and convert them to USD + for (uint64_t i = 0; i < _link.CollisionCount(); ++i) + { + const auto collision = *(_link.CollisionByIndex(i)); + auto collisionPath = std::string(_path + "/" + collision.Name()); + collisionPath = sdf::usd::validPath(collisionPath); + auto errorsCollision = ParseSdfCollision(collision, _stage, + collisionPath); + if (!errorsCollision.empty()) + { + errors.insert(errors.end(), errorsCollision.begin(), + errorsCollision.end()); + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing collision [" + collision.Name() + + "] attached to link [" + _link.Name() + "]")); + return errors; + } + } + // convert the link's sensors for (uint64_t i = 0; i < _link.SensorCount(); ++i) { const auto sensor = *(_link.SensorByIndex(i)); - const auto sensorPath = std::string(_path + "/" + sensor.Name()); + auto sensorPath = std::string(_path + "/" + sensor.Name()); + sensorPath = sdf::usd::validPath(sensorPath); UsdErrors errorsSensor = ParseSdfSensor(sensor, _stage, sensorPath); if (!errorsSensor.empty()) { @@ -162,6 +186,7 @@ namespace usd { const auto light = *(_link.LightByIndex(i)); auto lightPath = std::string(_path + "/" + light.Name()); + lightPath = sdf::usd::validPath(lightPath); UsdErrors lightErrors = ParseSdfLight(light, _stage, lightPath); if (!lightErrors.empty()) { diff --git a/usd/include/sdf/usd/sdf_parser/Link.hh b/usd/src/sdf_parser/Link.hh similarity index 95% rename from usd/include/sdf/usd/sdf_parser/Link.hh rename to usd/src/sdf_parser/Link.hh index 369600e83..578528d77 100644 --- a/usd/include/sdf/usd/sdf_parser/Link.hh +++ b/usd/src/sdf_parser/Link.hh @@ -30,7 +30,6 @@ #pragma pop_macro ("__DEPRECATED") #include "sdf/Link.hh" -#include "sdf/usd/Export.hh" #include "sdf/usd/UsdError.hh" #include "sdf/sdf_config.h" @@ -52,7 +51,7 @@ namespace sdf /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError /// includes an error code and message. An empty vector indicates no errors /// occurred when parsing _link to its USD representation. - UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfLink(const sdf::Link &_link, + UsdErrors ParseSdfLink(const sdf::Link &_link, pxr::UsdStageRefPtr &_stage, const std::string &_path, bool _rigidBody); } diff --git a/usd/src/sdf_parser/Link_Sdf2Usd_TEST.cc b/usd/src/sdf_parser/Link_Sdf2Usd_TEST.cc index 2d0d995cb..b939553d4 100644 --- a/usd/src/sdf_parser/Link_Sdf2Usd_TEST.cc +++ b/usd/src/sdf_parser/Link_Sdf2Usd_TEST.cc @@ -82,6 +82,7 @@ TEST_F(UsdStageFixture, Link) std::string groundPlanePath = worldPath + "/" + "ground_plane"; auto groundPlane = this->stage->GetPrimAtPath(pxr::SdfPath(groundPlanePath)); ASSERT_TRUE(groundPlane); + EXPECT_FALSE(groundPlane.HasAPI()); sdf::usd::testing::CheckPrimPose(groundPlane, ignition::math::Pose3d( ignition::math::Vector3d(0, 0, -0.125), @@ -98,6 +99,7 @@ TEST_F(UsdStageFixture, Link) std::string boxPath = worldPath + "/" + "box"; auto box = this->stage->GetPrimAtPath(pxr::SdfPath(boxPath)); ASSERT_TRUE(box); + EXPECT_TRUE(box.HasAPI()); sdf::usd::testing::CheckPrimPose(box, ignition::math::Pose3d( ignition::math::Vector3d(0, 0, 2.5), @@ -116,6 +118,7 @@ TEST_F(UsdStageFixture, Link) std::string cylinderPath = worldPath + "/" + "cylinder"; auto cylinder = this->stage->GetPrimAtPath(pxr::SdfPath(cylinderPath)); ASSERT_TRUE(cylinder); + EXPECT_TRUE(cylinder.HasAPI()); sdf::usd::testing::CheckPrimPose(cylinder, ignition::math::Pose3d( ignition::math::Vector3d(2, 0, 2.5), @@ -135,6 +138,7 @@ TEST_F(UsdStageFixture, Link) std::string spherePath = worldPath + "/" + "sphere"; auto sphere = this->stage->GetPrimAtPath(pxr::SdfPath(spherePath)); ASSERT_TRUE(sphere); + EXPECT_TRUE(sphere.HasAPI()); sdf::usd::testing::CheckPrimPose(sphere, ignition::math::Pose3d( ignition::math::Vector3d(4, 0, 2.5), @@ -153,6 +157,7 @@ TEST_F(UsdStageFixture, Link) std::string capsulePath = worldPath + "/" + "capsule"; auto capsule = this->stage->GetPrimAtPath(pxr::SdfPath(capsulePath)); ASSERT_TRUE(capsule); + EXPECT_TRUE(capsule.HasAPI()); sdf::usd::testing::CheckPrimPose(capsule, ignition::math::Pose3d( ignition::math::Vector3d(6, 0, 2.5), @@ -171,6 +176,7 @@ TEST_F(UsdStageFixture, Link) std::string meshPath = worldPath + "/" + "mesh"; auto mesh = this->stage->GetPrimAtPath(pxr::SdfPath(meshPath)); ASSERT_TRUE(mesh); + EXPECT_TRUE(mesh.HasAPI()); sdf::usd::testing::CheckPrimPose(mesh, ignition::math::Pose3d( ignition::math::Vector3d(8, 0, 2.5), diff --git a/usd/src/sdf_parser/Material.cc b/usd/src/sdf_parser/Material.cc index 0a1c789a8..4e9c6a31d 100644 --- a/usd/src/sdf_parser/Material.cc +++ b/usd/src/sdf_parser/Material.cc @@ -15,7 +15,7 @@ * */ -#include "sdf/usd/sdf_parser/Material.hh" +#include "Material.hh" #include #include @@ -192,6 +192,7 @@ namespace usd sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, "Not able to cast the UsdShadeShader at path [" + shaderPath.GetString() + "] to a Prim")); + return errors; } auto shaderOut = pxr::UsdShadeConnectableAPI(shaderPrim).CreateOutput( diff --git a/usd/include/sdf/usd/sdf_parser/Material.hh b/usd/src/sdf_parser/Material.hh similarity index 95% rename from usd/include/sdf/usd/sdf_parser/Material.hh rename to usd/src/sdf_parser/Material.hh index 429dd7b8e..f6c40aca2 100644 --- a/usd/include/sdf/usd/sdf_parser/Material.hh +++ b/usd/src/sdf_parser/Material.hh @@ -30,7 +30,6 @@ #pragma pop_macro ("__DEPRECATED") #include "sdf/Material.hh" -#include "sdf/usd/Export.hh" #include "sdf/usd/UsdError.hh" #include "sdf/sdf_config.h" @@ -49,7 +48,7 @@ namespace sdf /// \return UsdErrors, which is a list of UsdError objects. This list is /// empty if no errors occurred when parsing _materialSdf its USD /// representation - UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfMaterial( + UsdErrors ParseSdfMaterial( const sdf::Material *_materialSdf, pxr::UsdStageRefPtr &_stage, pxr::SdfPath &_materialPath); diff --git a/usd/src/sdf_parser/Material_Sdf2Usd_TEST.cc b/usd/src/sdf_parser/Material_Sdf2Usd_TEST.cc index 657c5360e..54b21d0c8 100644 --- a/usd/src/sdf_parser/Material_Sdf2Usd_TEST.cc +++ b/usd/src/sdf_parser/Material_Sdf2Usd_TEST.cc @@ -17,6 +17,7 @@ #include +#include #include // TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings @@ -29,11 +30,16 @@ #include #pragma pop_macro ("__DEPRECATED") +#include "Material.hh" + +#include "sdf/usd/UsdError.hh" #include "sdf/usd/sdf_parser/World.hh" +#include "sdf/Material.hh" #include "sdf/Root.hh" #include "test_config.h" #include "test_utils.hh" #include "../UsdTestUtils.hh" +#include "../Conversions.hh" void CheckMaterial( const pxr::UsdPrim &_prim, @@ -183,9 +189,74 @@ class UsdStageFixture : public::testing::Test public: pxr::UsdStageRefPtr stage; }; +TEST_F(UsdStageFixture, MaterialTextureName) +{ + sdf::testing::ScopeExit removeCopiedMaterials( + [] + { + ignition::common::removeAll( + ignition::common::joinPaths(ignition::common::cwd(), "materials")); + }); + + sdf::setFindCallback(sdf::usd::testing::findFileCb); + ignition::common::addFindFileURICallback( + std::bind(&sdf::usd::testing::FindResourceUri, std::placeholders::_1)); + const auto path = sdf::testing::TestFile("sdf", "basic_shapes.sdf"); + + ignition::common::Material materialCommon; + materialCommon.SetTextureImage("materials/textures/albedo_map.png"); + + const sdf::Material materialSdf = sdf::usd::convert(&materialCommon); + + const auto materialPathStr = std::string("/Looks/Material_0"); + auto materialPath = pxr::SdfPath(materialPathStr); + + sdf::usd::UsdErrors errors = sdf::usd::ParseSdfMaterial(&materialSdf, + stage, materialPath); + EXPECT_TRUE(errors.empty()); + + { + ASSERT_TRUE(this->stage->GetPrimAtPath(materialPath)); + + const std::string materialshaderPath = materialPathStr + "/Shader"; + const auto materialShaderPrim = this->stage->GetPrimAtPath( + pxr::SdfPath(materialshaderPath)); + ASSERT_TRUE(materialShaderPrim); + + auto variantshader = pxr::UsdShadeShader(materialShaderPrim); + ASSERT_TRUE(variantshader); + + std::vector inputs = variantshader.GetInputs(); + + bool checkedTextureInput = false; + for (const auto &input : inputs) + { + if (input.GetBaseName() == "diffuse_texture") + { + pxr::SdfAssetPath materialPathUSD; + pxr::UsdShadeInput diffuseTextureShaderInput = + variantshader.GetInput(pxr::TfToken("diffuse_texture")); + diffuseTextureShaderInput.Get(&materialPathUSD); + EXPECT_EQ("materials/textures/albedo_map.png", + materialPathUSD.GetAssetPath()); + checkedTextureInput = true; + break; + } + } + EXPECT_TRUE(checkedTextureInput); + } +} + ///////////////////////////////////////////////// TEST_F(UsdStageFixture, Material) { + sdf::testing::ScopeExit removeCopiedMaterials( + [] + { + ignition::common::removeAll( + ignition::common::joinPaths(ignition::common::cwd(), "materials")); + }); + sdf::setFindCallback(sdf::usd::testing::findFileCb); ignition::common::addFindFileURICallback( std::bind(&sdf::usd::testing::FindResourceUri, std::placeholders::_1)); diff --git a/usd/src/sdf_parser/Model.cc b/usd/src/sdf_parser/Model.cc index 9130921bf..1fda4512c 100644 --- a/usd/src/sdf_parser/Model.cc +++ b/usd/src/sdf_parser/Model.cc @@ -15,7 +15,7 @@ * */ -#include "sdf/usd/sdf_parser/Model.hh" +#include "Model.hh" #include #include @@ -32,12 +32,14 @@ #include #include #include +#include +#include #pragma pop_macro ("__DEPRECATED") #include "sdf/Model.hh" -#include "sdf/usd/sdf_parser/Joint.hh" -#include "sdf/usd/sdf_parser/Link.hh" #include "../UsdUtils.hh" +#include "Joint.hh" +#include "Link.hh" namespace sdf { @@ -116,7 +118,8 @@ namespace usd for (uint64_t i = 0; i < _model.LinkCount(); ++i) { const auto link = *(_model.LinkByIndex(i)); - const auto linkPath = std::string(_path + "/" + link.Name()); + auto linkPath = std::string(_path + "/" + link.Name()); + linkPath = sdf::usd::validPath(linkPath); sdfLinkToUSDPath[link.Name()] = pxr::SdfPath(linkPath); UsdErrors linkErrors = ParseSdfLink( link, _stage, linkPath, !_model.Static()); @@ -131,6 +134,16 @@ namespace usd } // Parse all of the model's joints and convert them to USD. + auto modelPrim = _stage->GetPrimAtPath(sdfModelPath); + if (!modelPrim) + { + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Internal error: unable to find prim at path [" + _path + + "], but a prim should exist at this path.")); + return errors; + } + bool markedArticulationRoot = false; for (uint64_t i = 0; i < _model.JointCount(); ++i) { const auto joint = *(_model.JointByIndex(i)); @@ -145,9 +158,34 @@ namespace usd errors.insert(errors.end(), jointErrors.begin(), jointErrors.end()); return errors; } + + if (!markedArticulationRoot && joint.Type() == sdf::JointType::REVOLUTE) + { + if (!pxr::UsdPhysicsArticulationRootAPI::Apply(modelPrim)) + { + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::FAILED_PRIM_API_APPLY, + "Unable to mark Xform at path [" + _path + + "] as a pxr::UsdPhysicsArticulationRootAPI. " + "Some features might not work.")); + } + else + { + markedArticulationRoot = true; + } + } } - // TODO(adlarkin) finish parsing model + if (!markedArticulationRoot && !_model.Static()) + { + if (!pxr::UsdPhysicsRigidBodyAPI::Apply(modelPrim)) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_PRIM_API_APPLY, + "Internal error: unable to mark model at path [" + + modelPrim.GetPath().GetString() + "] as a rigid body.")); + return errors; + } + } return errors; } diff --git a/usd/include/sdf/usd/sdf_parser/Model.hh b/usd/src/sdf_parser/Model.hh similarity index 96% rename from usd/include/sdf/usd/sdf_parser/Model.hh rename to usd/src/sdf_parser/Model.hh index dcb69a348..6dc4b167d 100644 --- a/usd/include/sdf/usd/sdf_parser/Model.hh +++ b/usd/src/sdf_parser/Model.hh @@ -31,7 +31,6 @@ #pragma pop_macro ("__DEPRECATED") #include "sdf/Model.hh" -#include "sdf/usd/Export.hh" #include "sdf/usd/UsdError.hh" #include "sdf/sdf_config.h" @@ -53,7 +52,7 @@ namespace sdf /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError /// includes an error code and message. An empty vector indicates no error /// occurred when parsing _model to its USD representation. - UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfModel( + UsdErrors ParseSdfModel( const sdf::Model &_model, pxr::UsdStageRefPtr &_stage, const std::string &_path, const pxr::SdfPath &_worldPath); } diff --git a/usd/src/sdf_parser/Sensor.cc b/usd/src/sdf_parser/Sensor.cc index 728b503d4..e7112e99e 100644 --- a/usd/src/sdf_parser/Sensor.cc +++ b/usd/src/sdf_parser/Sensor.cc @@ -15,7 +15,7 @@ * */ -#include "sdf/usd/sdf_parser/Sensor.hh" +#include "Sensor.hh" #include diff --git a/usd/include/sdf/usd/sdf_parser/Sensor.hh b/usd/src/sdf_parser/Sensor.hh similarity index 95% rename from usd/include/sdf/usd/sdf_parser/Sensor.hh rename to usd/src/sdf_parser/Sensor.hh index 97529c173..3211f56d9 100644 --- a/usd/include/sdf/usd/sdf_parser/Sensor.hh +++ b/usd/src/sdf_parser/Sensor.hh @@ -30,7 +30,6 @@ #pragma pop_macro ("__DEPRECATED") #include "sdf/Sensor.hh" -#include "sdf/usd/Export.hh" #include "sdf/usd/UsdError.hh" #include "sdf/sdf_config.h" @@ -49,7 +48,7 @@ namespace sdf /// be a valid USD path. /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError /// includes an error code and message. An empty vector indicates no error. - UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfSensor( + UsdErrors ParseSdfSensor( const sdf::Sensor &_sensor, pxr::UsdStageRefPtr &_stage, const std::string &_path); diff --git a/usd/src/sdf_parser/Visual.cc b/usd/src/sdf_parser/Visual.cc index 3dbbb889c..710b42fe3 100644 --- a/usd/src/sdf_parser/Visual.cc +++ b/usd/src/sdf_parser/Visual.cc @@ -15,7 +15,7 @@ * */ -#include "sdf/usd/sdf_parser/Visual.hh" +#include "Visual.hh" #include @@ -35,9 +35,9 @@ #pragma pop_macro ("__DEPRECATED") #include "sdf/Visual.hh" -#include "sdf/usd/sdf_parser/Geometry.hh" -#include "sdf/usd/sdf_parser/Material.hh" #include "../UsdUtils.hh" +#include "Geometry.hh" +#include "Material.hh" namespace sdf { @@ -80,7 +80,8 @@ namespace usd } const auto geometry = *(_visual.Geom()); - const auto geometryPath = std::string(_path + "/geometry"); + auto geometryPath = std::string(_path + "/geometry"); + geometryPath = sdf::usd::validPath(geometryPath); auto geomErrors = ParseSdfGeometry(geometry, _stage, geometryPath); if (!geomErrors.empty()) { diff --git a/usd/include/sdf/usd/sdf_parser/Visual.hh b/usd/src/sdf_parser/Visual.hh similarity index 95% rename from usd/include/sdf/usd/sdf_parser/Visual.hh rename to usd/src/sdf_parser/Visual.hh index 3c5196ac1..b901ea803 100644 --- a/usd/include/sdf/usd/sdf_parser/Visual.hh +++ b/usd/src/sdf_parser/Visual.hh @@ -31,7 +31,6 @@ #include "sdf/Visual.hh" #include "sdf/config.hh" -#include "sdf/usd/Export.hh" #include "sdf/usd/UsdError.hh" namespace sdf @@ -49,7 +48,7 @@ namespace sdf /// be a valid USD path. /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError /// includes an error code and message. An empty vector indicates no error. - UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfVisual( + UsdErrors ParseSdfVisual( const sdf::Visual &_visual, pxr::UsdStageRefPtr &_stage, const std::string &_path); } diff --git a/usd/src/sdf_parser/World.cc b/usd/src/sdf_parser/World.cc index 0231e9035..c1fc4a3ef 100644 --- a/usd/src/sdf_parser/World.cc +++ b/usd/src/sdf_parser/World.cc @@ -17,6 +17,7 @@ #include "sdf/usd/sdf_parser/World.hh" +#include #include #include @@ -37,8 +38,9 @@ #pragma pop_macro ("__DEPRECATED") #include "sdf/World.hh" -#include "sdf/usd/sdf_parser/Light.hh" -#include "sdf/usd/sdf_parser/Model.hh" +#include "Light.hh" +#include "Model.hh" +#include "../UsdUtils.hh" namespace sdf { @@ -73,15 +75,16 @@ namespace usd for (uint64_t i = 0; i < _world.ModelCount(); ++i) { const auto model = *(_world.ModelByIndex(i)); - auto modelPath = std::string(_path + "/" + model.Name()); - modelPath = ignition::common::replaceAll(modelPath, " ", ""); + std::string modelName = model.Name(); + modelName = sdf::usd::validPath(modelName); + auto modelPath = std::string(_path + "/" + modelName); UsdErrors modelErrors = ParseSdfModel(model, _stage, modelPath, worldPrimPath); if (!modelErrors.empty()) { errors.push_back(UsdError( sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, - "Error parsing model [" + model.Name() + "]")); + "Error parsing model [" + modelName + "]")); errors.insert(errors.end(), modelErrors.begin(), modelErrors.end()); } } @@ -90,7 +93,7 @@ namespace usd { const auto light = *(_world.LightByIndex(i)); auto lightPath = std::string(_path + "/" + light.Name()); - lightPath = ignition::common::replaceAll(lightPath, " ", ""); + lightPath = sdf::usd::validPath(lightPath); UsdErrors lightErrors = ParseSdfLight(light, _stage, lightPath); if (!lightErrors.empty()) { diff --git a/usd/src/sdf_parser/sdf2usd_TEST.cc b/usd/src/sdf_parser/sdf2usd_TEST.cc index a50654396..7103ab262 100644 --- a/usd/src/sdf_parser/sdf2usd_TEST.cc +++ b/usd/src/sdf_parser/sdf2usd_TEST.cc @@ -20,11 +20,21 @@ #include #include -#include +#include #include "test_config.h" #include "test_utils.hh" +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#pragma pop_macro ("__DEPRECATED") + #ifdef _WIN32 #define popen _popen #define pclose _pclose @@ -88,10 +98,36 @@ TEST(check_cmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } } -///////////////////////////////////////////////// -/// Main -int main(int argc, char **argv) +TEST(check_cmd_model, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + std::string pathBase = PROJECT_SOURCE_PATH; + pathBase = ignition::common::joinPaths(pathBase, "test", "sdf"); + + auto tmpDir = ignition::common::tempDirectoryPath(); + auto tmp = ignition::common::createTempDirectory("usd", tmpDir); + // Check a good SDF file + { + std::string path = ignition::common::joinPaths(pathBase, + "ellipsoid_model.sdf"); + const auto outputUsdFilePath = + ignition::common::joinPaths(tmp, "ellipsoid.usd"); + EXPECT_FALSE(ignition::common::isFile(outputUsdFilePath)); + std::string output = + custom_exec_str(sdf2usdCommand() + " " + path + " " + outputUsdFilePath); + EXPECT_TRUE(output.empty()); + + // make sure that a ellipsoid.usd file was generated + EXPECT_TRUE(ignition::common::isFile(outputUsdFilePath)) << output; + + const auto stage = pxr::UsdStage::Open(outputUsdFilePath); + ASSERT_TRUE(stage); + + ASSERT_TRUE(stage->GetPrimAtPath(pxr::SdfPath("/ellipsoid"))); + ASSERT_TRUE(stage->GetPrimAtPath( + pxr::SdfPath("/ellipsoid/ellipsoid_link"))); + ASSERT_TRUE(stage->GetPrimAtPath( + pxr::SdfPath("/ellipsoid/ellipsoid_link/ellipsoid_visual"))); + ASSERT_TRUE(stage->GetPrimAtPath( + pxr::SdfPath("/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry"))); + } } diff --git a/usd/src/usd_parser/Parser.cc b/usd/src/usd_parser/Parser.cc new file mode 100644 index 000000000..3fe584744 --- /dev/null +++ b/usd/src/usd_parser/Parser.cc @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2022 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 "sdf/usd/usd_parser/Parser.hh" +#include "USD2SDF.hh" + +#include "sdf/Root.hh" + +namespace sdf +{ +inline namespace SDF_VERSION_NAMESPACE { +namespace usd +{ + UsdErrors parseUSDFile( + const std::string &_inputFilenameUsd, + const std::string &_outputFilenameSdf) + { + UsdErrors errors; + USD2SDF usd2sdf; + sdf::Root root; + errors = usd2sdf.Read(_inputFilenameUsd, root); + if (!errors.empty()) + { + return errors; + } + + std::ofstream out(_outputFilenameSdf.c_str(), std::ios::out); + if (!out) + { + errors.emplace_back(UsdError( + UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Unable to open file [" + _outputFilenameSdf + "] for writing")); + return errors; + } + out << root.ToElement()->ToString(""); + out.close(); + return errors; + } +} +} +} diff --git a/usd/src/usd_parser/USD2SDF.cc b/usd/src/usd_parser/USD2SDF.cc new file mode 100644 index 000000000..ef68c19f5 --- /dev/null +++ b/usd/src/usd_parser/USD2SDF.cc @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2022 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 "USD2SDF.hh" + +#include "sdf/Console.hh" +#include "sdf/Types.hh" +#include "USDWorld.hh" + +#include "sdf/Error.hh" +#include "sdf/Root.hh" +#include "sdf/World.hh" + +#include "sdf/usd/UsdError.hh" + +namespace sdf { +inline namespace SDF_VERSION_NAMESPACE { +namespace usd { +//////////////////////////////////////////////// +UsdErrors USD2SDF::Read(const std::string &_fileName, + sdf::Root &_root) +{ + UsdErrors errors; + + sdf::World sdfWorld; + + errors = parseUSDWorld(_fileName, sdfWorld); + if (!errors.empty()) + { + errors.emplace_back(UsdError( + UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing usd file [" + _fileName + "]")); + return errors; + } + + const auto addWorldErrors = _root.AddWorld(sdfWorld); + if (!addWorldErrors.empty()) + { + for (const auto & error : addWorldErrors) + { + errors.emplace_back(error); + } + errors.emplace_back(UsdError(sdf::Error( + sdf::ErrorCode::ELEMENT_INVALID, + "Error adding the world [" + sdfWorld.Name() + "] to the SDF DOM"))); + } + + return errors; +} +} +} +} diff --git a/usd/src/usd_parser/USD2SDF.hh b/usd/src/usd_parser/USD2SDF.hh new file mode 100644 index 000000000..9844ff043 --- /dev/null +++ b/usd/src/usd_parser/USD2SDF.hh @@ -0,0 +1,56 @@ +/* + * Copyright 2022 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. + * +*/ + +#ifndef USD_PARSER_USD2SDF_HH_ +#define USD_PARSER_USD2SDF_HH_ + +#include + +#include "sdf/sdf_config.h" +#include "sdf/usd/UsdError.hh" + +#include "sdf/Root.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { + namespace usd + { + /// \brief USD to SDF converter + /// This class helps generate the SDF file + class USD2SDF + { + /// \brief constructor + public: USD2SDF() = default; + + /// \brief convert USD file to a sdf::Root object + /// \param[in] _fileName string containing USD filename. + /// \param[out] _root Root element to populate with the equivalent sdf + /// information from _fileName. + /// \return UsdErrors, which is a list of UsdError objects. An empty list + /// means no errors occurred when populating _root with the contents + /// of _fileName + public: UsdErrors Read( + const std::string &_fileName, + sdf::Root &_root); + }; + } +} +} + +#endif diff --git a/usd/src/usd_parser/USDData.cc b/usd/src/usd_parser/USDData.cc new file mode 100644 index 000000000..7b89616c8 --- /dev/null +++ b/usd/src/usd_parser/USDData.cc @@ -0,0 +1,302 @@ +/* + * Copyright (C) 2022 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 "sdf/usd/usd_parser/USDData.hh" + +#include +#include +#include + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include +#include + +#include "sdf/Material.hh" +#include "USDMaterial.hh" + +namespace sdf { +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd { + /// \brief USDStage private data. + class USDData::Implementation + { + public: + /// File name of the main stage + std::string filename; + + /// Directory where the main stage is located + std::string directoryPath; + + /// map with the filename and its own Stage data + std::unordered_map> references; + + /// Models + std::set models; + + /// Materials availables in the stage and substages + std::unordered_map materials; + + /// Add all subdirectories that are inside the stage folder + /// This will help us to find other stages and/or textures. + /// \param[in] _path Path of the subdirectory to add + void AddSubdirectories(const std::string &_path) + { + for (ignition::common::DirIter file(_path); + file != ignition::common::DirIter(); ++file) + { + std::string current(*file); + + if (ignition::common::isDirectory(current)) + { + auto systemPaths = ignition::common::systemPaths(); + systemPaths->AddFilePaths(current); + this->AddSubdirectories(current); + } + } + } + }; + + ///////////////////////////////////////////////// + USDData::USDData(const std::string &_filename) + : dataPtr(ignition::utils::MakeImpl()) + { + this->dataPtr->filename = _filename; + } + + ///////////////////////////////////////////////// + const std::unordered_map & + USDData::Materials() const + { + return this->dataPtr->materials; + } + + ///////////////////////////////////////////////// + const std::set &USDData::Models() const + { + return this->dataPtr->models; + } + + ///////////////////////////////////////////////// + const std::unordered_map> & + USDData::AllReferences() const + { + return this->dataPtr->references; + } + + ///////////////////////////////////////////////// + UsdErrors USDData::Init() + { + UsdErrors errors; + + auto usdStage = std::make_shared(this->dataPtr->filename); + UsdErrors errorsInit = usdStage->Init(); + if(!errorsInit.empty()) + { + errors.insert(errors.end(), errorsInit.begin(), errorsInit.end()); + return errors; + } + + // Add the base stage to the structure + this->dataPtr->references.insert( + {this->dataPtr->filename, + usdStage + }); + + // it's the stage available + auto referencee = pxr::UsdStage::Open(this->dataPtr->filename); + if (!referencee) + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::INVALID_USD_FILE, + "Failed to load usd file")); + return errors; + } + + this->dataPtr->directoryPath = ignition::common::absPath( + this->dataPtr->filename); + + std::string basename = ignition::common::basename( + this->dataPtr->directoryPath); + this->dataPtr->directoryPath = ignition::common::replaceAll( + this->dataPtr->directoryPath, basename, ""); + + this->dataPtr->AddSubdirectories(this->dataPtr->directoryPath); + + auto range = pxr::UsdPrimRange::Stage(referencee); + + for (auto const &_prim : range) + { + if (_prim.IsA()) + { + continue; + } + + if (_prim.IsA()) + { + continue; + } + + pxr::UsdPrimCompositionQuery query = + pxr::UsdPrimCompositionQuery::GetDirectReferences(_prim); + + std::vector arcs = + query.GetCompositionArcs(); + for (auto & a : arcs ) + { + pxr::SdfLayerHandle handler = a.GetIntroducingLayer(); + + for (auto & ref : handler->GetCompositionAssetDependencies()) + { + this->AddStage(ref); + } + } + } + + return errors; + } + + ///////////////////////////////////////////////// + UsdErrors USDData::ParseMaterials() + { + UsdErrors errors; + auto referencee = pxr::UsdStage::Open(this->dataPtr->filename); + if (!referencee) + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::INVALID_USD_FILE, + "Failed to load usd file")); + return errors; + } + + auto range = pxr::UsdPrimRange::Stage(referencee); + + for (auto const &prim : range) + { + std::string primName = pxr::TfStringify(prim.GetPath()); + if (prim.IsA()) + { + std::string materialName = prim.GetName(); + + if (this->dataPtr->materials.find(materialName) + != this->dataPtr->materials.end()) + { + continue; + } + + sdf::Material material; + UsdErrors errrosMaterial = ParseMaterial(prim, material); + if (!errrosMaterial.empty()) + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing material")); + errors.insert( + errors.end(), errrosMaterial.begin(), errrosMaterial.end()); + return errors; + } + this->dataPtr->materials.insert(std::pair( + materialName, material)); + } + } + return errors; + } + + ///////////////////////////////////////////////// + const std::pair> + USDData::FindStage(const std::string &_name) const + { + for (auto &ref : this->dataPtr->references) + { + if (ref.second != nullptr) + { + for (auto &path : ref.second->USDPaths()) + { + if (path == _name) + { + return ref; + } + } + } + } + return std::make_pair("", nullptr); + } + + ///////////////////////////////////////////////// + UsdErrors USDData::AddStage(const std::string &_ref) + { + UsdErrors errors; + std::string key = _ref; + + auto search = this->dataPtr->references.find(_ref); + if (search == this->dataPtr->references.end()) + { + std::string basename = ignition::common::basename(key); + auto subDirectory = ignition::common::replaceAll(key, basename, ""); + + auto systemPaths = ignition::common::systemPaths(); + systemPaths->AddFilePaths(ignition::common::joinPaths( + this->dataPtr->directoryPath, subDirectory)); + + this->dataPtr->AddSubdirectories(ignition::common::joinPaths( + this->dataPtr->directoryPath, subDirectory)); + + std::string fileNameRef = ignition::common::findFile(basename); + if (fileNameRef.empty()) + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Not able to find asset [" + _ref + "]")); + return errors; + } + + auto usdStage = std::make_shared(fileNameRef); + UsdErrors errorsInit = usdStage->Init(); + if(!errorsInit.empty()) + { + errors.insert(errors.end(), errorsInit.begin(), errorsInit.end()); + return errors; + } + + this->dataPtr->references.insert( + {key, + std::make_shared(fileNameRef) + }); + return errors; + } + else + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Element already exists")); + return errors; + } + } +} +} +} diff --git a/usd/src/usd_parser/USDData_TEST.cc b/usd/src/usd_parser/USDData_TEST.cc new file mode 100644 index 000000000..cbd3d6916 --- /dev/null +++ b/usd/src/usd_parser/USDData_TEST.cc @@ -0,0 +1,123 @@ +/* + * Copyright (C) 2022 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 + +#include +#include + +#include + +#include +#include + +#include "test_config.h" +#include "test_utils.hh" + +#include +#include + +///////////////////////////////////////////////// +TEST(USDData, Constructor) +{ + // Open a invalid USD file + sdf::usd::USDData data(sdf::testing::TestFile("usd", "invalid_name")); + sdf::usd::UsdErrors errors = data.Init(); + EXPECT_EQ(1u, errors.size()); + + // Add test/usd directory to find some resources + auto systemPaths = ignition::common::systemPaths(); + systemPaths->AddFilePaths(sdf::testing::TestFile("usd")); + + { + sdf::testing::ScopeExit removeCopiedMaterials( + [] + { + ignition::common::removeAll( + ignition::common::joinPaths(ignition::common::cwd(), "materials")); + }); + + // Open a valid USD file + std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); + sdf::usd::USDData usdData(filename); + EXPECT_EQ(0u, usdData.Init().size()); + EXPECT_EQ(0u, usdData.ParseMaterials().size()); + + EXPECT_EQ(1u, usdData.AllReferences().size()); + EXPECT_EQ(0u, usdData.Models().size()); + EXPECT_EQ(7u, usdData.Materials().size()); + + auto materials = usdData.Materials(); + + auto material0 = materials["Material_0"]; + EXPECT_EQ(ignition::math::Color(0.8, 0.8, 0.8), material0.Diffuse()); + EXPECT_EQ(ignition::math::Color(0, 0, 0), material0.Emissive()); + + auto material1 = materials["Material_1"]; + EXPECT_EQ(ignition::math::Color(1, 0, 0), material1.Diffuse()); + EXPECT_EQ(ignition::math::Color(0, 0, 0), material1.Emissive()); + + auto material2 = materials["Material_2"]; + EXPECT_EQ(ignition::math::Color(0, 1, 0), material2.Diffuse()); + EXPECT_EQ(ignition::math::Color(0, 0, 0), material2.Emissive()); + + auto material3 = materials["Material_3"]; + EXPECT_EQ(ignition::math::Color(0, 0, 1), material3.Diffuse()); + EXPECT_EQ(ignition::math::Color(0, 0, 0), material3.Emissive()); + + auto material4 = materials["Material_4"]; + EXPECT_EQ(ignition::math::Color(1, 1, 0), material4.Diffuse()); + EXPECT_EQ(ignition::math::Color(0, 0, 0), material4.Emissive()); + + auto material5 = materials["Material_5"]; + EXPECT_EQ(ignition::math::Color(1, 0, 1), material5.Diffuse()); + EXPECT_EQ(ignition::math::Color(0, 0, 0), material5.Emissive()); + + auto materialTextures = materials["Material_textures"]; + const auto * pbr = materialTextures.PbrMaterial(); + ASSERT_TRUE(pbr); + const auto * workflow = pbr->Workflow(sdf::PbrWorkflowType::METAL); + ASSERT_TRUE(workflow); + EXPECT_EQ(ignition::math::Color(1, 1, 1), materialTextures.Diffuse()); + EXPECT_EQ( + "materials/textures/FANS_Albedo.png", + workflow->AlbedoMap()); + EXPECT_EQ(ignition::math::Color(0, 0, 0), materialTextures.Emissive()); + EXPECT_DOUBLE_EQ(0.5, workflow->Metalness()); + EXPECT_EQ( + "materials/textures/FANS_Metalness.png", + workflow->MetalnessMap()); + EXPECT_DOUBLE_EQ(0.5, workflow->Roughness()); + EXPECT_EQ( + "materials/textures/FANS_Roughness.png", + workflow->RoughnessMap()); + EXPECT_EQ( + "materials/textures/FANS_Normal.png", + workflow->NormalMap()); + + // Find a path inside the stage + auto boxStage = usdData.FindStage("box"); + EXPECT_EQ(filename, boxStage.first); + EXPECT_EQ("Z", boxStage.second->UpAxis()); + EXPECT_DOUBLE_EQ(0.01, boxStage.second->MetersPerUnit()); + + // Try to find a invalid path in the stage data + auto invalidStage = usdData.FindStage("invalid"); + EXPECT_EQ("", invalidStage.first); + EXPECT_EQ(nullptr, invalidStage.second); + } +} diff --git a/usd/src/usd_parser/USDJoints.cc b/usd/src/usd_parser/USDJoints.cc new file mode 100644 index 000000000..84b3a7818 --- /dev/null +++ b/usd/src/usd_parser/USDJoints.cc @@ -0,0 +1,283 @@ +/* + * Copyright (C) 2022 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 "USDJoints.hh" + +#include +#include +#include + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include + +#include "sdf/usd/UsdError.hh" +#include "sdf/usd/usd_parser/USDData.hh" +#include "sdf/Joint.hh" +#include "sdf/JointAxis.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + UsdErrors ParseJoints( + const pxr::UsdPrim &_prim, + const USDData &_usdData, + sdf::Joint &_joint) + { + UsdErrors errors; + + std::pair> usdData = + _usdData.FindStage(_prim.GetPath().GetName()); + double metersPerUnit = usdData.second->MetersPerUnit(); + + pxr::SdfPathVector body0, body1; + + auto variant_physics_joint = pxr::UsdPhysicsJoint(_prim); + + if (variant_physics_joint.GetBody0Rel()) + variant_physics_joint.GetBody0Rel().GetTargets(&body0); + if (variant_physics_joint.GetBody1Rel()) + variant_physics_joint.GetBody1Rel().GetTargets(&body1); + + if (body1.size() > 0) + { + _joint.SetChildLinkName(ignition::common::basename( + body1[0].GetString())); + } + else if (body0.size() > 0) + { + _joint.SetParentLinkName("world"); + _joint.SetChildLinkName(ignition::common::basename( + body0[0].GetString())); + } + + if (body0.size() > 0 && _joint.ParentLinkName().empty()) + { + _joint.SetParentLinkName(ignition::common::basename( + body0[0].GetString())); + } + else + { + _joint.SetParentLinkName("world"); + } + + std::string primName = _prim.GetName(); + if (primName.find("_joint") == std::string::npos) + { + _joint.SetName(std::string(_prim.GetName()) + "_joint"); + } + else + { + _joint.SetName(std::string(_prim.GetName())); + } + + float lowerLimit; + float upperLimit; + float stiffness; + float damping; + float maxForce; + float jointFriction; + float vel; + ignition::math::Quaterniond q1; + ignition::math::Quaterniond q2; + pxr::GfVec3f trans; + ignition::math::Vector3d axisVector; + sdf::JointAxis jointAxis; + + if (_prim.IsA() || + _prim.IsA()) + { + _joint.SetPoseRelativeTo(_joint.ParentLinkName()); + + pxr::TfToken axis; + if (_prim.IsA()) + { + pxr::UsdPhysicsPrismaticJoint(_prim).GetAxisAttr().Get(&axis); + } + else + { + pxr::UsdPhysicsRevoluteJoint(_prim).GetAxisAttr().Get(&axis); + } + + if (axis == pxr::UsdGeomTokens->x) + { + axisVector = ignition::math::Vector3d(1, 0, 0); + } + else if (axis == pxr::UsdGeomTokens->y) + { + axisVector = ignition::math::Vector3d(0, 1, 0); + } + else if (axis == pxr::UsdGeomTokens->z) + { + axisVector = ignition::math::Vector3d(0, 0, 1); + } + + pxr::GfVec3f localPose0, localPose1; + pxr::GfQuatf localRot0, localRot1; + + const auto usdPhysicsJoint = pxr::UsdPhysicsJoint(_prim); + usdPhysicsJoint.GetLocalPos0Attr().Get(&localPose0); + usdPhysicsJoint.GetLocalPos1Attr().Get(&localPose1); + usdPhysicsJoint.GetLocalRot0Attr().Get(&localRot0); + usdPhysicsJoint.GetLocalRot1Attr().Get(&localRot1); + + trans = (localPose0 + localPose1) * metersPerUnit; + + q1 = ignition::math::Quaterniond( + localRot0.GetReal(), + localRot0.GetImaginary()[0], + localRot0.GetImaginary()[1], + localRot0.GetImaginary()[2]); + q2 = ignition::math::Quaterniond( + localRot1.GetReal(), + localRot1.GetImaginary()[0], + localRot1.GetImaginary()[1], + localRot1.GetImaginary()[2]); + + _prim.GetAttribute( + pxr::TfToken("physics:lowerLimit")).Get(&lowerLimit); + _prim.GetAttribute( + pxr::TfToken("physics:upperLimit")).Get(&upperLimit); + if (_prim.IsA()) + { + _prim.GetAttribute( + pxr::TfToken("drive:linear:physics:stiffness")).Get(&stiffness); + _prim.GetAttribute( + pxr::TfToken("drive:linear:physics:damping")).Get(&damping); + _prim.GetAttribute( + pxr::TfToken("drive:linear:physics:maxForce")).Get(&maxForce); + } + else + { + _prim.GetAttribute( + pxr::TfToken("drive:angular:physics:stiffness")).Get(&stiffness); + _prim.GetAttribute( + pxr::TfToken("drive:angular:physics:damping")).Get(&damping); + _prim.GetAttribute( + pxr::TfToken("drive:angular:physics:maxForce")).Get(&maxForce); + } + _prim.GetAttribute( + pxr::TfToken("physxJoint:maxJointVelocity")).Get(&vel); + + pxr::UsdAttribute jointFrictionAttribute; + if (jointFrictionAttribute = _prim.GetAttribute( + pxr::TfToken("physxJoint:jointFriction"))) + { + jointFrictionAttribute.Get(&jointFriction); + } + else if (jointFrictionAttribute = _prim.GetAttribute( + pxr::TfToken("jointFriction"))) + { + jointFrictionAttribute.Get(&jointFriction); + } + + jointAxis.SetDamping(damping); + jointAxis.SetEffort(maxForce); + jointAxis.SetSpringStiffness(stiffness); + jointAxis.SetFriction(jointFriction); + jointAxis.SetMaxVelocity(vel); + } + + if (_prim.IsA()) + { + auto variant_physics_prismatic_joint = + pxr::UsdPhysicsPrismaticJoint(_prim); + + _joint.SetType(sdf::JointType::PRISMATIC); + + auto errorsAxis = jointAxis.SetXyz(-(q2 * axisVector).Round()); + if (!errorsAxis.empty()) + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::USD_TO_SDF_PARSING_ERROR, + "Errors encountered when setting xyz of prismatic " + "joint axis: [" + std::string(_prim.GetName()) + "]")); + for (const auto & error : errorsAxis) + errors.emplace_back(error); + return errors; + } + + _joint.SetRawPose( + ignition::math::Pose3d( + ignition::math::Vector3d(trans[0], trans[1], trans[2]), + ignition::math::Quaterniond(q1 * q2))); + + jointAxis.SetLower(lowerLimit * metersPerUnit); + jointAxis.SetUpper(upperLimit * metersPerUnit); + _joint.SetAxis(0, jointAxis); + + return errors; + } + else if (_prim.IsA()) + { + auto variant_physics_revolute_joint = + pxr::UsdPhysicsRevoluteJoint(_prim); + + _joint.SetType(sdf::JointType::REVOLUTE); + + auto errorsAxis = jointAxis.SetXyz(axisVector); + if (!errorsAxis.empty()) + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::USD_TO_SDF_PARSING_ERROR, + "Errors encountered when setting xyz of revolute " + "joint axis: [" + std::string(_prim.GetName()) + "]")); + for (const auto & error : errorsAxis) + errors.emplace_back(error); + return errors; + } + + _joint.SetRawPose(ignition::math::Pose3d( + ignition::math::Vector3d(trans[0], trans[1], trans[2]), + q1)); + + jointAxis.SetLower(IGN_DTOR(lowerLimit)); + jointAxis.SetUpper(IGN_DTOR(upperLimit)); + _joint.SetAxis(0, jointAxis); + + return errors; + } + else if (_prim.IsA() || + _prim.IsA()) + { + _joint.SetType(sdf::JointType::FIXED); + } + else + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::USD_TO_SDF_PARSING_ERROR, + "Unable to create a SDF joint from USD prim [" + + std::string(_prim.GetName()) + + "] because the prim is not a USD joint.")); + } + + return errors; + } + } + } +} diff --git a/usd/src/usd_parser/USDJoints.hh b/usd/src/usd_parser/USDJoints.hh new file mode 100644 index 000000000..9a635700a --- /dev/null +++ b/usd/src/usd_parser/USDJoints.hh @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#ifndef USD_PARSER_JOINTS_HH +#define USD_PARSER_JOINTS_HH + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Joint.hh" +#include "sdf/usd/usd_parser/USDData.hh" +#include "sdf/config.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse a USD joint to its SDF representation + /// This method will parse revolute and prismatic joints, if the Joint type + /// is not one of these two, then the joint is fixed. This will help to + /// avoid issues with when two or more links links in a model doesn't have + /// a joint + /// + /// \param[in] _prim The USD prim that holds the USD joint + /// \param[in] _usdData Object that holds data about the USD stage + /// \param[out] _joint SDF joint to return + /// \return UsdErrors, which is a list of UsdError objects. An empty list + /// means there were no errors parsing joint + UsdErrors ParseJoints( + const pxr::UsdPrim &_prim, + const USDData &_usdData, + sdf::Joint &_joint); + } + } +} + +#endif diff --git a/usd/src/usd_parser/USDJoints_TEST.cc b/usd/src/usd_parser/USDJoints_TEST.cc new file mode 100644 index 000000000..1d79e78f6 --- /dev/null +++ b/usd/src/usd_parser/USDJoints_TEST.cc @@ -0,0 +1,91 @@ +/* + * Copyright (C) 2022 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 + +#include + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include + +#include "test_config.h" +#include "test_utils.hh" + +#include "USDJoints.hh" + +#include "sdf/Joint.hh" +#include "sdf/JointAxis.hh" +#include "sdf/usd/usd_parser/USDData.hh" + +///////////////////////////////////////////////// +TEST(USDJointTest, JointTest) +{ + const std::string filename = + sdf::testing::TestFile("usd", "double_pendulum.usda"); + const auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + sdf::usd::USDData usdData(filename); + usdData.Init(); + + const auto upperJoint = stage->GetPrimAtPath(pxr::SdfPath( + "/double_pendulum/double_pendulum_with_base/upper_joint")); + ASSERT_TRUE(upperJoint); + + sdf::Joint joint1; + auto errors = sdf::usd::ParseJoints( + upperJoint, usdData, joint1); + + EXPECT_EQ(0u, errors.size()); + EXPECT_EQ(sdf::JointType::REVOLUTE, joint1.Type()); + EXPECT_EQ("upper_joint", joint1.Name()); + EXPECT_EQ("upper_link", joint1.ChildLinkName()); + EXPECT_EQ("base", joint1.ParentLinkName()); + EXPECT_EQ(ignition::math::Pose3d(0, 0, 0.021, -1.5708, 0, 0), + joint1.RawPose()); + + auto axis = joint1.Axis(0); + ASSERT_NE(nullptr, axis); + EXPECT_EQ(ignition::math::Vector3d(1, 0, 0), axis->Xyz()); + + const auto lowerJoint = stage->GetPrimAtPath(pxr::SdfPath( + "/double_pendulum/double_pendulum_with_base/lower")); + ASSERT_TRUE(lowerJoint); + + sdf::Joint joint2; + errors = sdf::usd::ParseJoints( + lowerJoint, usdData, joint2); + + EXPECT_EQ(0u, errors.size()); + EXPECT_EQ(sdf::JointType::REVOLUTE, joint2.Type()); + EXPECT_EQ("lower_joint", joint2.Name()); + EXPECT_EQ("lower_link", joint2.ChildLinkName()); + EXPECT_EQ("upper_link", joint2.ParentLinkName()); + EXPECT_EQ(ignition::math::Pose3d(0.0025, -0, 0.01, -0.4292, 0, 0), + joint2.RawPose()); + axis = joint2.Axis(0); + ASSERT_NE(nullptr, axis); + EXPECT_EQ(ignition::math::Vector3d(1, 0, 0), axis->Xyz()); +} diff --git a/usd/src/usd_parser/USDLight_TEST.cc b/usd/src/usd_parser/USDLight_TEST.cc new file mode 100644 index 000000000..2da9ffcc0 --- /dev/null +++ b/usd/src/usd_parser/USDLight_TEST.cc @@ -0,0 +1,91 @@ +/* + * Copyright (C) 2022 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 + +#include +#include + +// TODO(ahcorde) This is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include +#include + +#include +#include + +#include "test_config.h" +#include "test_utils.hh" + +#include "sdf/Light.hh" + +#include "USDLights.hh" + +///////////////////////////////////////////////// +TEST(USDLightsTest, DistanceLight) +{ + std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); + sdf::usd::USDData usdData(filename); + EXPECT_EQ(0u, usdData.Init().size()); + + auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + pxr::UsdPrim prim = stage->GetPrimAtPath( + pxr::SdfPath("/defaultLight")); + ASSERT_TRUE(prim); + + auto light = sdf::usd::ParseUSDLights( + prim, usdData, ""); + ASSERT_TRUE(light); + + EXPECT_EQ("defaultLight", light->Name()); + EXPECT_EQ(sdf::LightType::DIRECTIONAL, light->Type()); + EXPECT_TRUE(light->CastShadows()); + EXPECT_EQ(ignition::math::Color(0.5, 0.5, 0.5, 1), light->Diffuse()); + EXPECT_EQ(ignition::math::Color(1, 1, 1, 1), light->Specular()); + EXPECT_FLOAT_EQ(0.5, light->Intensity()); + EXPECT_EQ(ignition::math::Pose3d(0, 0, 10, 0, IGN_DTOR(45), 0), + light->RawPose()); + + prim = stage->GetPrimAtPath(pxr::SdfPath("/diskLight")); + ASSERT_TRUE(prim); + + auto diskLight = sdf::usd::ParseUSDLights( + prim, usdData, ""); + ASSERT_TRUE(diskLight); + + EXPECT_EQ("diskLight", diskLight->Name()); + EXPECT_EQ(sdf::LightType::SPOT, diskLight->Type()); + EXPECT_TRUE(diskLight->CastShadows()); + EXPECT_EQ(ignition::math::Color(1, 1, 1, 1), diskLight->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.5, 0.5, 0.5, 1), diskLight->Specular()); + EXPECT_FLOAT_EQ(0.3, diskLight->Intensity()); + EXPECT_EQ( + ignition::math::Pose3d(0, 0, 10, 0, 0, IGN_DTOR(45)), diskLight->RawPose()); + EXPECT_DOUBLE_EQ(0.1, diskLight->SpotInnerAngle().Radian()); + EXPECT_DOUBLE_EQ(0.5, diskLight->SpotOuterAngle().Radian()); + EXPECT_DOUBLE_EQ(0.8, diskLight->SpotFalloff()); +} diff --git a/usd/src/usd_parser/USDLights.cc b/usd/src/usd_parser/USDLights.cc new file mode 100644 index 000000000..529739ebc --- /dev/null +++ b/usd/src/usd_parser/USDLights.cc @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2022 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 "USDLights.hh" + +#include + +// TODO(ahcorde) This is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include "pxr/usd/usdLux/lightAPI.h" +#include "pxr/usd/usdLux/boundableLightBase.h" +#include "pxr/usd/usdLux/distantLight.h" +#include "pxr/usd/usdLux/diskLight.h" +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/usd/usd_parser/USDData.hh" +#include "sdf/usd/usd_parser/USDTransforms.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ + std::optional ParseUSDLights( + const pxr::UsdPrim &_prim, + const USDData &_usdData, + const std::string &_linkName) + { + sdf::Light lightSdf; + std::optional light = lightSdf; + + auto variantLight = pxr::UsdLuxBoundableLightBase(_prim); + + ignition::math::Pose3d pose; + ignition::math::Vector3d scale(1, 1, 1); + GetTransform(_prim, _usdData, pose, scale, _linkName); + + light->SetName(_prim.GetPath().GetName()); + float intensity; + variantLight.GetIntensityAttr().Get(&intensity); + // This value was found trying to find a similar light intensity + // between isaac sim and gazebo. Might be wrong + // TODO(ahcorde): Convert the light intensity with an equation or unit + // conversions + light->SetIntensity(intensity / 10000); + float diffuse; + variantLight.GetDiffuseAttr().Get(&diffuse); + light->SetDiffuse(ignition::math::Color(diffuse, diffuse, diffuse, 1)); + float specular; + variantLight.GetSpecularAttr().Get(&specular); + light->SetSpecular(ignition::math::Color(specular, specular, specular, 1)); + light->SetCastShadows(true); + + if (_prim.IsA()) + { + light->SetType(sdf::LightType::DIRECTIONAL); + + // DistantLight in USD does not define height. Added some height to the + // light. The default sun light in ign-gazebo sdf world is 10. + pose += ignition::math::Pose3d(0, 0, 10, 0, 0, 0); + // Light emitted from a distant source along the -Z axis + // The pose should set the direction + light->SetDirection(ignition::math::Vector3d(0, 0, -1)); + } + else if (_prim.IsA()) + { + light->SetType(sdf::LightType::SPOT); + // These parameters are not defined in USD. Added some generic values. + light->SetSpotInnerAngle(0.1); + light->SetSpotOuterAngle(0.5); + light->SetSpotFalloff(0.8); + } + else + { + return std::nullopt; + } + light->SetRawPose(pose); + return light; + } +} +} +} diff --git a/usd/src/usd_parser/USDLights.hh b/usd/src/usd_parser/USDLights.hh new file mode 100644 index 000000000..ebe7dd019 --- /dev/null +++ b/usd/src/usd_parser/USDLights.hh @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2021 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. + * +*/ + +#ifndef USD_USD_PARSER_LIGHTS_HH +#define USD_USD_PARSER_LIGHTS_HH + +#include +#include +#include + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Light.hh" + +#include "sdf/config.hh" +#include "sdf/usd/Export.hh" + +#include "sdf/usd/usd_parser/USDData.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse lights + /// Supported lights: + /// - UsdLuxDistantLight + /// - UsdLuxDiskLight + /// \param[in] _prim Prim to extract the light data + /// \param[in] _usdData Object to get transform data + /// \param[in] _linkName Name of the link to find the transform + /// \return Shared point with the sdf Light object or std::nullopt if the + /// light is not supported. + std::optional IGNITION_SDFORMAT_USD_VISIBLE ParseUSDLights( + const pxr::UsdPrim &_prim, + const USDData &_usdData, + const std::string &_linkName); + } +} +} + +#endif diff --git a/usd/src/usd_parser/USDLinks.cc b/usd/src/usd_parser/USDLinks.cc new file mode 100644 index 000000000..a923db320 --- /dev/null +++ b/usd/src/usd_parser/USDLinks.cc @@ -0,0 +1,760 @@ +/* + * Copyright (C) 2022 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 "USDLinks.hh" + +#include +#include +#include +#include +#include + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#include +#include +#include +#include +#include "pxr/usd/usdPhysics/collisionAPI.h" +#include "pxr/usd/usdPhysics/massAPI.h" +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include +#include +#include +#include +#include +#include + +#include + +#include "sdf/Box.hh" +#include "sdf/Collision.hh" +#include "sdf/Cylinder.hh" +#include "sdf/Geometry.hh" +#include "sdf/Link.hh" +#include "sdf/Mesh.hh" +#include "sdf/Sphere.hh" +#include "sdf/Visual.hh" + +#include "polygon_helper.hh" + +#include "sdf/usd/usd_parser/USDTransforms.hh" +#include "sdf/usd/UsdError.hh" +#include "../Conversions.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ +////////////////////////////////////////////////// +void GetInertial( + const pxr::UsdPrim &_prim, + ignition::math::Inertiald &_inertial) +{ + float mass; + pxr::GfVec3f centerOfMass; + pxr::GfVec3f diagonalInertia; + pxr::GfQuatf principalAxes; + + ignition::math::MassMatrix3d massMatrix; + + if (_prim.HasAPI()) + { + if (pxr::UsdAttribute massAttribute = + _prim.GetAttribute(pxr::TfToken("physics:mass"))) + { + massAttribute.Get(&mass); + + if (pxr::UsdAttribute centerOfMassAttribute = + _prim.GetAttribute(pxr::TfToken("physics:centerOfMass"))) + { + centerOfMassAttribute.Get(¢erOfMass); + } + + if (pxr::UsdAttribute diagonalInertiaAttribute = + _prim.GetAttribute(pxr::TfToken("physics:diagonalInertia"))) + { + diagonalInertiaAttribute.Get(&diagonalInertia); + } + + if (pxr::UsdAttribute principalAxesAttribute = + _prim.GetAttribute(pxr::TfToken("physics:principalAxes"))) + { + principalAxesAttribute.Get(&principalAxes); + } + + // Added a diagonal inertia to avoid crash with the physics engine + if (diagonalInertia == pxr::GfVec3f(0, 0, 0)) + { + diagonalInertia = pxr::GfVec3f(0.0001, 0.0001, 0.0001); + } + + // Added a mass to avoid crash with the physics engine + if (mass < 0.0001) + { + mass = 0.1; + } + + massMatrix.SetMass(mass); + + // TODO(ahcorde) Figure out how to use PrincipalMoments and + // PrincipalAxesOffset: see + // https://github.com/ignitionrobotics/sdformat/pull/902#discussion_r840905534 + massMatrix.SetDiagonalMoments( + ignition::math::Vector3d( + diagonalInertia[0], + diagonalInertia[1], + diagonalInertia[2])); + + _inertial.SetPose(ignition::math::Pose3d( + ignition::math::Vector3d( + centerOfMass[0], centerOfMass[1], centerOfMass[2]), + ignition::math::Quaterniond(1.0, 0, 0, 0))); + + _inertial.SetMassMatrix(massMatrix); + } + } + else + { + auto parent = _prim.GetParent(); + if (parent) + { + GetInertial(parent, _inertial); + } + } +} + +////////////////////////////////////////////////// +/// \brief Create the directory based on the USD path +/// \param[in] _primPath Reference to the USD prim +/// \return A string with the path +std::string directoryFromUSDPath(const std::string &_primPath) +{ + std::vector tokensChild = + ignition::common::split(_primPath, "/"); + std::string directoryMesh; + if (tokensChild.size() > 1) + { + directoryMesh = tokensChild[0]; + for (unsigned int i = 1; i < tokensChild.size() - 1; ++i) + { + directoryMesh = ignition::common::joinPaths( + directoryMesh, tokensChild[i+1]); + } + } + return directoryMesh; +} + +////////////////////////////////////////////////// +/// \brief Get the material name of the prim +/// \param[in] _prim Reference to the USD prim +/// \return A string with the material name +std::string ParseMaterialName(const pxr::UsdPrim &_prim) +{ + // Materials + std::string nameMaterial; + + auto bindMaterial = pxr::UsdShadeMaterialBindingAPI(_prim); + pxr::UsdRelationship directBindingRel = + bindMaterial.GetDirectBindingRel(); + + pxr::SdfPathVector paths; + directBindingRel.GetTargets(&paths); + for (const auto & p : paths) + { + std::vector tokensMaterial = + ignition::common::split(pxr::TfStringify(p), "/"); + + if(tokensMaterial.size() > 0) + { + nameMaterial = tokensMaterial[tokensMaterial.size() - 1]; + } + } + return nameMaterial; +} + +////////////////////////////////////////////////// +/// \brief Parse USD mesh subsets +/// \param[in] _prim Reference to the USD prim +/// \param[in] _link Current link +/// \param[in] _subMesh submesh to add the subsets +/// \param[in] _meshGeom Mesh geom +/// \param[inout] _scale scale mesh +/// \param[in] _usdData metadata of the USD file +/// \return Number of mesh subsets +int ParseMeshSubGeom(const pxr::UsdPrim &_prim, + sdf::Link *_link, + ignition::common::SubMesh &_subMesh, + sdf::Mesh &_meshGeom, + ignition::math::Vector3d &_scale, + const USDData &_usdData) +{ + int numSubMeshes = 0; + + for (const auto & child : _prim.GetChildren()) + { + if (child.IsA()) + { + sdf::Visual visSubset; + std::string nameMaterial = ParseMaterialName(child); + auto it = _usdData.Materials().find(nameMaterial); + if (it != _usdData.Materials().end()) + { + visSubset.SetMaterial(it->second); + } + + ignition::common::Mesh meshSubset; + + numSubMeshes++; + + ignition::common::SubMesh subMeshSubset; + subMeshSubset.SetPrimitiveType(ignition::common::SubMesh::TRISTRIPS); + subMeshSubset.SetName("subgeommesh"); + + if (it != _usdData.Materials().end()) + { + std::shared_ptr matCommon = + std::make_shared(); + convert(it->second, *matCommon.get()); + meshSubset.AddMaterial(matCommon); + subMeshSubset.SetMaterialIndex(meshSubset.MaterialCount() - 1); + } + pxr::VtIntArray faceVertexIndices; + child.GetAttribute(pxr::TfToken("indices")).Get(&faceVertexIndices); + + for (unsigned int i = 0; i < faceVertexIndices.size(); ++i) + { + subMeshSubset.AddIndex(_subMesh.Index(faceVertexIndices[i] * 3)); + subMeshSubset.AddIndex(_subMesh.Index(faceVertexIndices[i] * 3 + 1)); + subMeshSubset.AddIndex(_subMesh.Index(faceVertexIndices[i] * 3 + 2)); + } + + for (unsigned int i = 0; i < _subMesh.VertexCount(); ++i) + { + subMeshSubset.AddVertex(_subMesh.Vertex(i)); + } + for (unsigned int i = 0; i < _subMesh.NormalCount(); ++i) + { + subMeshSubset.AddNormal(_subMesh.Normal(i)); + } + for (unsigned int i = 0; i < _subMesh.TexCoordCount(); ++i) + { + subMeshSubset.AddTexCoord(_subMesh.TexCoord(i)); + } + + meshSubset.AddSubMesh(subMeshSubset); + sdf::Mesh meshGeomSubset; + sdf::Geometry geomSubset; + geomSubset.SetType(sdf::GeometryType::MESH); + + std::string childPathName = pxr::TfStringify(child.GetPath()); + std::string directoryMesh = directoryFromUSDPath(childPathName) + + childPathName; + + if (ignition::common::createDirectories(directoryMesh)) + { + directoryMesh = ignition::common::joinPaths(directoryMesh, + ignition::common::basename(directoryMesh)); + + // Export with extension + ignition::common::ColladaExporter exporter; + exporter.Export(&meshSubset, directoryMesh, false); + } + _meshGeom.SetFilePath(directoryMesh + ".dae"); + _meshGeom.SetUri(directoryMesh + ".dae"); + + geomSubset.SetMeshShape(_meshGeom); + visSubset.SetName("mesh_subset"); + visSubset.SetGeom(geomSubset); + + ignition::math::Pose3d pose; + ignition::math::Vector3d scale(1, 1, 1); + GetTransform(child, _usdData, pose, scale, _link->Name()); + _scale *= scale; + visSubset.SetRawPose(pose); + _link->AddVisual(visSubset); + } + } + return numSubMeshes; +} + +////////////////////////////////////////////////// +/// \brief Parse USD mesh +/// \param[in] _prim Reference to the USD prim +/// \param[in] _link Current link +/// \param[in] _vis sdf visual +/// \param[in] _geom sdf geom +/// \param[in] _scale scale mesh +/// \param[in] _usdData metadata of the USD file +/// \param[out] _pose The pose of the parsed mesh +/// \return UsdErrors, which is a list of UsdError objects. An empty list means +/// that no errors occurred when parsing the USD mesh +UsdErrors ParseMesh( + const pxr::UsdPrim &_prim, + sdf::Link *_link, + sdf::Visual &_vis, + sdf::Geometry &_geom, + ignition::math::Vector3d &_scale, + const USDData &_usdData, + ignition::math::Pose3d &_pose) +{ + UsdErrors errors; + + const std::pair> data = + _usdData.FindStage(_prim.GetPath().GetName()); + + double metersPerUnit = data.second->MetersPerUnit(); + + ignition::common::Mesh mesh; + ignition::common::SubMesh subMesh; + subMesh.SetPrimitiveType(ignition::common::SubMesh::TRISTRIPS); + pxr::VtIntArray faceVertexIndices; + pxr::VtIntArray faceVertexCounts; + pxr::VtArray normals; + pxr::VtArray points; + pxr::VtArray textCoords; + _prim.GetAttribute(pxr::TfToken("faceVertexCounts")).Get(&faceVertexCounts); + _prim.GetAttribute(pxr::TfToken("faceVertexIndices")).Get(&faceVertexIndices); + _prim.GetAttribute(pxr::TfToken("normals")).Get(&normals); + _prim.GetAttribute(pxr::TfToken("points")).Get(&points); + _prim.GetAttribute(pxr::TfToken("primvars:st")).Get(&textCoords); + + if (textCoords.size() == 0) + { + _prim.GetAttribute(pxr::TfToken("primvars:st_0")).Get(&textCoords); + } + + std::vector indexes; + errors = PolygonToTriangles( + faceVertexIndices, faceVertexCounts, indexes); + if (!errors.empty()) + { + errors.emplace_back(UsdErrorCode::USD_TO_SDF_POLYGON_PARSING_ERROR, + "Unable to parse polygon in path [" + pxr::TfStringify(_prim.GetPath()) + + "]"); + return errors; + } + + for (unsigned int i = 0; i < indexes.size(); ++i) + { + subMesh.AddIndex(indexes[i]); + } + + for (const auto & textCoord : textCoords) + { + subMesh.AddTexCoord(textCoord[0], (1 - textCoord[1])); + } + + for (const auto & point : points) + { + ignition::math::Vector3d v = + ignition::math::Vector3d(point[0], point[1], point[2]) * metersPerUnit; + subMesh.AddVertex(v); + } + + for (const auto & normal : normals) + { + subMesh.AddNormal(normal[0], normal[1], normal[2]); + } + + sdf::Mesh meshGeom; + _geom.SetType(sdf::GeometryType::MESH); + + ignition::math::Pose3d pose; + ignition::math::Vector3d scale(1, 1, 1); + std::string linkName = _link->Name(); + size_t nSlash = std::count(linkName.begin(), linkName.end(), '/'); + if (nSlash == 1) + { + GetTransform(_prim, _usdData, pose, scale, "/"); + } + else + { + GetTransform(_prim, _usdData, pose, scale, _link->Name()); + } + + _pose = pose; + + meshGeom.SetScale(scale * _scale); + + std::string primName = pxr::TfStringify(_prim.GetPath()); + + std::string directoryMesh = directoryFromUSDPath(primName) + primName; + + meshGeom.SetFilePath( + ignition::common::joinPaths( + directoryMesh, ignition::common::basename(directoryMesh)) + ".dae"); + + meshGeom.SetUri(meshGeom.FilePath()); + + int numSubMeshes = ParseMeshSubGeom( + _prim, _link, subMesh, meshGeom, _scale, _usdData); + + _geom.SetMeshShape(meshGeom); + + if (numSubMeshes == 0) + { + std::string nameMaterial = ParseMaterialName(_prim); + auto it = _usdData.Materials().find(nameMaterial); + if (it != _usdData.Materials().end()) + { + _vis.SetMaterial(it->second); + std::shared_ptr matCommon = + std::make_shared(); + convert(it->second, *matCommon.get()); + mesh.AddMaterial(matCommon); + subMesh.SetMaterialIndex(mesh.MaterialCount() - 1); + } + + std::string primNameStr = _prim.GetPath().GetName(); + _vis.SetName(primNameStr + "_visual"); + _vis.SetRawPose(pose); + _vis.SetGeom(_geom); + + _link->AddVisual(_vis); + + mesh.AddSubMesh(subMesh); + + if (ignition::common::createDirectories(directoryMesh)) + { + directoryMesh = ignition::common::joinPaths( + directoryMesh, ignition::common::basename(directoryMesh)); + // Export with extension + ignition::common::ColladaExporter exporter; + exporter.Export(&mesh, directoryMesh, false); + } + } + + return errors; +} + +////////////////////////////////////////////////// +/// \brief Parse USD cube +/// \param[in] _prim Reference to the USD prim +/// \param[in] _geom sdf geom +/// \param[in] _scale scale mesh +/// \param[in] _metersPerUnit meter per unit of the stage +void ParseCube(const pxr::UsdPrim &_prim, + sdf::Geometry &_geom, + const ignition::math::Vector3d &_scale, + double _metersPerUnit) +{ + double size; + auto variant_cube = pxr::UsdGeomCube(_prim); + variant_cube.GetSizeAttr().Get(&size); + + size = size * _metersPerUnit; + + sdf::Box box; + _geom.SetType(sdf::GeometryType::BOX); + box.SetSize(ignition::math::Vector3d( + size * _scale.X(), + size * _scale.Y(), + size * _scale.Z())); + + _geom.SetBoxShape(box); +} + +////////////////////////////////////////////////// +/// \brief Parse USD sphere +/// \param[in] _prim Reference to the USD prim +/// \param[in] _geom sdf geom +/// \param[in] _scale scale mesh +/// \param[in] _metersPerUnit meter per unit of the stage +void ParseSphere(const pxr::UsdPrim &_prim, + sdf::Geometry &_geom, + const ignition::math::Vector3d &_scale, + double _metersPerUnit) +{ + double radius; + auto variant_sphere = pxr::UsdGeomSphere(_prim); + variant_sphere.GetRadiusAttr().Get(&radius); + + sdf::Sphere s; + _geom.SetType(sdf::GeometryType::SPHERE); + s.SetRadius(radius * _metersPerUnit * _scale.X()); + _geom.SetSphereShape(s); +} + +////////////////////////////////////////////////// +/// \brief Parse USD cylinder +/// \param[in] _prim Reference to the USD prim +/// \param[in] _geom sdf geom +/// \param[in] _scale scale mesh +/// \param[in] _metersPerUnit meter per unit of the stage +void ParseCylinder( + const pxr::UsdPrim &_prim, + sdf::Geometry &_geom, + const ignition::math::Vector3d &_scale, + double _metersPerUnit) +{ + auto variant_cylinder = pxr::UsdGeomCylinder(_prim); + double radius; + double height; + variant_cylinder.GetRadiusAttr().Get(&radius); + variant_cylinder.GetHeightAttr().Get(&height); + + sdf::Cylinder c; + _geom.SetType(sdf::GeometryType::CYLINDER); + + c.SetRadius(radius * _metersPerUnit * _scale.X()); + c.SetLength(height * _metersPerUnit * _scale.Y()); + + _geom.SetCylinderShape(c); +} + +////////////////////////////////////////////////// +UsdErrors ParseUSDLinks( + const pxr::UsdPrim &_prim, + const std::string &_nameLink, + std::optional &_link, + const USDData &_usdData, + ignition::math::Vector3d &_scale) +{ + UsdErrors errors; + const std::string primNameStr = _prim.GetPath().GetName(); + const std::string primPathStr = pxr::TfStringify(_prim.GetPath()); + const std::string primType = _prim.GetPrimTypeInfo().GetTypeName().GetText(); + + const std::pair> data = + _usdData.FindStage(primNameStr); + + // Is this a new link ? + if (!_link) + { + _link = sdf::Link(); + _link->SetName(ignition::common::basename(_nameLink)); + + // USD define visual inside other visuals or links + // This loop allow to find the link for a specific visual + // For example + // This visual /ur10_long_suction/shoulder_link/cylinder + // correponds to the link /ur10_long_suction/wrist_3_link + // Then we can find the right transformations + std::string originalPrimName = pxr::TfStringify(_prim.GetPath()); + size_t pos = std::string::npos; + std::string nameOfLink; + if ((pos = originalPrimName.find(_link->Name()))!= std::string::npos) + { + nameOfLink = originalPrimName.erase( + pos + _link->Name().length(), + originalPrimName.length() - (pos + _link->Name().length())); + } + pxr::UsdPrim tmpPrim = _prim; + if (!nameOfLink.empty()) + { + while (tmpPrim) + { + if (pxr::TfStringify(tmpPrim.GetPath()) == nameOfLink) + { + break; + } + tmpPrim = tmpPrim.GetParent(); + } + } + + ignition::math::Pose3d pose; + ignition::math::Vector3d scale(1, 1, 1); + GetTransform(tmpPrim, _usdData, pose, scale, ""); + // This is a special case when a geometry is defined in the higher level + // of the path. we should only set the position if the path at least has + // more than 1 level. + // TODO(ahcorde) Review this code and improve this logic + size_t nSlash = std::count(_nameLink.begin(), _nameLink.end(), '/'); + if (nSlash > 1) + _link->SetRawPose(pose); + _scale *= scale; + } + + // If the schema is a rigid body use this name instead. + if (_prim.HasAPI()) + { + _link->SetName(ignition::common::basename(primPathStr)); + } + + ignition::math::Inertiald noneInertial = {{1.0, + ignition::math::Vector3d::One, ignition::math::Vector3d::Zero}, + ignition::math::Pose3d::Zero}; + const auto inertial = _link->Inertial(); + if (inertial == noneInertial) + { + ignition::math::Inertiald newInertial; + GetInertial(_prim, newInertial); + _link->SetInertial(newInertial); + } + + sdf::Geometry geom; + if (_prim.IsA() || + _prim.IsA() || + _prim.IsA() || + _prim.IsA() || + primType == "Plane") + { + sdf::Visual vis; + + auto variant_geom = pxr::UsdGeomGprim(_prim); + + bool collisionEnabled = false; + if (_prim.HasAPI()) + { + _prim.GetAttribute( + pxr::TfToken("physics:collisionEnabled")).Get(&collisionEnabled); + } + + pxr::TfToken kindOfSchema; + if(!pxr::UsdModelAPI(_prim).GetKind(&kindOfSchema)) + { + auto parent = _prim.GetParent(); + pxr::UsdModelAPI(parent).GetKind(&kindOfSchema); + } + + if (_prim.HasAPI() + || pxr::KindRegistry::IsA(kindOfSchema, pxr::KindTokens->model) + || !collisionEnabled) + { + double metersPerUnit = data.second->MetersPerUnit(); + + if (_prim.IsA()) + { + ParseSphere(_prim, geom, _scale, metersPerUnit); + vis.SetName("visual_sphere"); + vis.SetGeom(geom); + _link->AddVisual(vis); + } + else if (_prim.IsA()) + { + ParseCylinder(_prim, geom, _scale, metersPerUnit); + vis.SetName("visual_cylinder"); + vis.SetGeom(geom); + _link->AddVisual(vis); + } + else if (_prim.IsA()) + { + ParseCube(_prim, geom, _scale, metersPerUnit); + vis.SetName("visual_box"); + vis.SetGeom(geom); + _link->AddVisual(vis); + } + else if (_prim.IsA()) + { + ignition::math::Pose3d poseTmp; + errors = ParseMesh( + _prim, &_link.value(), vis, geom, _scale, _usdData, poseTmp); + if (!errors.empty()) + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing mesh")); + return errors; + } + } + } + + pxr::TfTokenVector schemasCollision = _prim.GetAppliedSchemas(); + bool physxCollisionAPIenable = false; + for (const auto & token : schemasCollision) + { + if (std::string(token.GetText()) == "PhysxCollisionAPI") + { + physxCollisionAPIenable = true; + break; + } + } + + if (collisionEnabled || physxCollisionAPIenable) + { + sdf::Collision col; + + // add _collision extension + std::string collisionName = _prim.GetPath().GetName() + "_collision"; + col.SetName(collisionName); + sdf::Geometry colGeom; + + ignition::math::Pose3d poseCol; + ignition::math::Vector3d scaleCol(1, 1, 1); + GetTransform(_prim, _usdData, poseCol, scaleCol, _link->Name()); + + double metersPerUnit = data.second->MetersPerUnit(); + + if (_prim.IsA()) + { + ParseSphere(_prim, colGeom, scaleCol, metersPerUnit); + col.SetGeom(colGeom); + col.SetRawPose(poseCol); + } + else if (_prim.IsA()) + { + ParseCylinder(_prim, colGeom, scaleCol, metersPerUnit); + col.SetGeom(colGeom); + col.SetRawPose(poseCol); + } + else if (_prim.IsA()) + { + ParseCube(_prim, colGeom, scaleCol, metersPerUnit); + col.SetGeom(colGeom); + col.SetRawPose(poseCol); + } + else if (_prim.IsA()) + { + sdf::Visual visTmp; + ignition::math::Pose3d poseTmp; + errors = ParseMesh( + _prim, &_link.value(), visTmp, colGeom, scaleCol, _usdData, poseTmp); + if (!errors.empty()) + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing mesh")); + return errors; + } + col.SetRawPose(poseTmp); + col.SetGeom(colGeom); + } + else if (primType == "Plane") + { + sdf::Plane plane; + colGeom.SetType(sdf::GeometryType::PLANE); + plane.SetSize(ignition::math::Vector2d(100, 100)); + colGeom.SetPlaneShape(plane); + + ignition::math::Pose3d pose; + ignition::math::Vector3d scale(1, 1, 1); + GetTransform( + _prim, _usdData, pose, scale, pxr::TfStringify(_prim.GetPath())); + col.SetRawPose(pose); + col.SetGeom(colGeom); + } + + _link->AddCollision(col); + } + + } + return errors; +} +} +} +} diff --git a/usd/src/usd_parser/USDLinks.hh b/usd/src/usd_parser/USDLinks.hh new file mode 100644 index 000000000..41876b6c8 --- /dev/null +++ b/usd/src/usd_parser/USDLinks.hh @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#ifndef SDF_USD_USD_PARSER_USD_LINKS_HH +#define SDF_USD_USD_PARSER_USD_LINKS_HH + +#include +#include + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/config.hh" +#include "sdf/usd/usd_parser/USDData.hh" +#include "sdf/usd/UsdError.hh" +#include "sdf/Link.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse a USD link into its SDF representation + /// \param[in] _prim The USD prim that contains the link + /// \param[in] _nameLink The name of the link + /// \param[out] _link The SDF link to populate. If no value is held, a new + /// link will be created. If the optional does hold a value, the existing + /// link will have additional information added to it + /// \param[in] _usdData USDData object that holds data about the USD stage + /// \param[in] _scale scale of current Link, sdf::Link does not have a scale + /// attribute we need to keeep the scale in a extenal variable + /// \return UsdErrors, which is a list of UsdError objects. An empty list + /// means there were no errors parsing the USD link + UsdErrors ParseUSDLinks( + const pxr::UsdPrim &_prim, + const std::string &_nameLink, + std::optional &_link, + const USDData &_usdData, + ignition::math::Vector3d &_scale); + } + } +} + +#endif diff --git a/usd/src/usd_parser/USDLinks_TEST.cc b/usd/src/usd_parser/USDLinks_TEST.cc new file mode 100644 index 000000000..35fb2465d --- /dev/null +++ b/usd/src/usd_parser/USDLinks_TEST.cc @@ -0,0 +1,207 @@ +/* + * Copyright (C) 2022 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 + +#include +#include + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "test_config.h" +#include "test_utils.hh" + +#include "USDLinks.hh" + +#include "sdf/Collision.hh" +#include "sdf/Geometry.hh" +#include "sdf/Visual.hh" + +///////////////////////////////////////////////// +TEST(USDLinksTest, LinksNameMassAndDiagonalMoments) +{ + const std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); + const auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + sdf::usd::USDData usdData(filename); + usdData.Init(); + + ignition::math::Vector3d scale(1, 1, 1); + + auto checkLink = + [](const sdf::Link &_link, + const std::string &_name, + double mass, + const ignition::math::Vector3d &_diagonalMoment, + unsigned int _nvisual, + sdf::GeometryType _type) + { + EXPECT_EQ(_name, _link.Name()); + EXPECT_NEAR(mass, _link.Inertial().MassMatrix().Mass(), 1e-4); + EXPECT_EQ( + _diagonalMoment, + _link.Inertial().MassMatrix().PrincipalMoments()); + EXPECT_EQ(_nvisual, _link.VisualCount()); + if (_nvisual) + { + auto visual = _link.VisualByIndex(0); + ASSERT_NE(nullptr, visual); + auto geom = visual->Geom(); + ASSERT_NE(nullptr, geom); + EXPECT_EQ(_type, geom->Type()); + auto collision = _link.CollisionByIndex(0); + ASSERT_NE(nullptr, collision); + auto geomCol = collision->Geom(); + ASSERT_NE(nullptr, geomCol); + } + }; + + const auto boxLink = stage->GetPrimAtPath(pxr::SdfPath("/box/box_link")); + ASSERT_TRUE(boxLink); + + std::optional linkSDF; + sdf::usd::ParseUSDLinks( + boxLink, "/box/box_link", linkSDF, usdData, scale); + + EXPECT_EQ(ignition::math::Vector3d(1, 0.1, 1), scale); + + const auto boxLinkGeometry = stage->GetPrimAtPath( + pxr::SdfPath("/box/box_link/box_visual/geometry")); + ASSERT_TRUE(boxLinkGeometry); + + sdf::usd::ParseUSDLinks( + boxLinkGeometry, "/box/box_link", linkSDF, usdData, scale); + + const auto boxLinkCollision = stage->GetPrimAtPath( + pxr::SdfPath("/box/box_link/box_visual/collision")); + ASSERT_TRUE(boxLinkCollision); + + sdf::usd::ParseUSDLinks( + boxLinkCollision, "/box/box_link", linkSDF, usdData, scale); + + ASSERT_TRUE(linkSDF); + checkLink(linkSDF.value(), "box_link", 1.0, + ignition::math::Vector3d(0.1666, 0.1666, 0.1666), + 1u, sdf::GeometryType::BOX); + + const auto cylinderLink = stage->GetPrimAtPath( + pxr::SdfPath("/cylinder/cylinder_link")); + ASSERT_TRUE(cylinderLink); + + std::optional linkCylinderSDF; + sdf::usd::ParseUSDLinks( + cylinderLink, "/cylinder/cylinder_link", + linkCylinderSDF, usdData, scale); + + const auto cylinderLinkGeometry = stage->GetPrimAtPath( + pxr::SdfPath("/cylinder/cylinder_link/cylinder_visual/geometry")); + EXPECT_TRUE(cylinderLinkGeometry); + + sdf::usd::ParseUSDLinks( + cylinderLinkGeometry, "/cylinder/cylinder_link", + linkCylinderSDF, usdData, scale); + + const auto cylinderLinkCollision = stage->GetPrimAtPath( + pxr::SdfPath("/cylinder/cylinder_link/cylinder_visual/collision")); + EXPECT_TRUE(cylinderLinkCollision); + + sdf::usd::ParseUSDLinks( + cylinderLinkCollision, "/cylinder/cylinder_link", + linkCylinderSDF, usdData, scale); + + checkLink(linkCylinderSDF.value(), "cylinder_link", 1.7, + ignition::math::Vector3d(0.1458, 0.1458, 0.125), + 1u, sdf::GeometryType::CYLINDER); + + const auto sphereLink = stage->GetPrimAtPath( + pxr::SdfPath("/sphere/sphere_link")); + ASSERT_TRUE(sphereLink); + + std::optional linkSphereSDF; + sdf::usd::ParseUSDLinks( + sphereLink, "/sphere/sphere_link", linkSphereSDF, usdData, scale); + + const auto sphereLinkGeometry = stage->GetPrimAtPath( + pxr::SdfPath("/sphere/sphere_link/sphere_visual/geometry")); + EXPECT_TRUE(sphereLinkGeometry); + + sdf::usd::ParseUSDLinks( + sphereLinkGeometry, "/sphere/sphere_link", + linkSphereSDF, usdData, scale); + + const auto sphereLinkCollision = stage->GetPrimAtPath( + pxr::SdfPath("/sphere/sphere_link/sphere_visual/collision")); + EXPECT_TRUE(sphereLinkCollision); + + sdf::usd::ParseUSDLinks( + sphereLinkCollision, "/sphere/sphere_link", + linkSphereSDF, usdData, scale); + + ASSERT_TRUE(linkSphereSDF); + checkLink(linkSphereSDF.value(), "sphere_link", 2, + ignition::math::Vector3d(0.1, 0.1, 0.1), + 1u, sdf::GeometryType::SPHERE); + + const auto capsuleLink = stage->GetPrimAtPath( + pxr::SdfPath("/capsule/capsule_link")); + ASSERT_TRUE(capsuleLink); + + std::optional linkCapsuleSDF; + sdf::usd::ParseUSDLinks( + capsuleLink, "/capsule/capsule_link", linkCapsuleSDF, usdData, scale); + + const auto capsuleLinkCollision = stage->GetPrimAtPath( + pxr::SdfPath("/capsule/capsule_link/capsule_visual/collision")); + EXPECT_TRUE(capsuleLinkCollision); + + sdf::usd::ParseUSDLinks( + capsuleLinkCollision, "/capsule/capsule_link", + linkCapsuleSDF, usdData, scale); + + checkLink(linkCapsuleSDF.value(), "capsule_link", 1, + ignition::math::Vector3d(0.074154, 0.074154, 0.018769), + 0u, sdf::GeometryType::EMPTY); + + const auto ellipsoidLink = stage->GetPrimAtPath( + pxr::SdfPath("/ellipsoid/ellipsoid_link")); + ASSERT_TRUE(ellipsoidLink); + + std::optional linkEllipsoidSDF; + sdf::usd::ParseUSDLinks( + ellipsoidLink, "/ellipsoid/ellipsoid_link", + linkEllipsoidSDF, usdData, scale); + + const auto ellipsoidLinkCollision = stage->GetPrimAtPath( + pxr::SdfPath("/ellipsoid/ellipsoid_link/ellipsoid_visual/collision")); + EXPECT_TRUE(ellipsoidLinkCollision); + + sdf::usd::ParseUSDLinks( + ellipsoidLinkCollision, "/ellipsoid/ellipsoid_link", + linkEllipsoidSDF, usdData, scale); + + checkLink(linkEllipsoidSDF.value(), "ellipsoid_link", 1, + ignition::math::Vector3d(0.068, 0.058, 0.026), + 0u, sdf::GeometryType::EMPTY); +} diff --git a/usd/src/usd_parser/USDMaterial.cc b/usd/src/usd_parser/USDMaterial.cc new file mode 100644 index 000000000..a4b9b232c --- /dev/null +++ b/usd/src/usd_parser/USDMaterial.cc @@ -0,0 +1,378 @@ +/* + * Copyright (C) 2022 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 +#include "USDMaterial.hh" + +#include +#include +#include +#include +#include + +#include "sdf/Pbr.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + ///////////////////////////////////////////////// + /// \brief Copy a file from one destination to another + /// \param[in] _ori The original file to copy + /// \param[inout] _dest The destination for the copy of _ori. If _dest + /// represents a file that already exists, a unique numeric suffix in the + /// form of _ will be appended to the end of the file name. + /// \return A list of UsdErrors. An empty list means no errors occurred when + /// copying _ori to _dest + UsdErrors copyFile(const std::string &_ori, std::string &_dest) + { + UsdErrors errors; + if (ignition::common::exists(_ori)) + { + // If the file exists then we append a number suffix to the destination + // file + // For example: + // /bar/foo.extension + // /bar/foo_X.extension + if (ignition::common::exists(_dest)) + { + const std::string parentPath = ignition::common::parentPath(_dest); + std::string::size_type fileExtensionIndex = _dest.rfind("."); + if (fileExtensionIndex == std::string::npos) + { + errors.emplace_back( + Error(ErrorCode::FILE_READ, "Unable to find the extension of the " + "file [" + _dest + "] which should be copied")); + return errors; + } + + const std::string fileExtension = _dest.substr(fileExtensionIndex); + std::string fileNameWithoutExtension = + ignition::common::basename(_dest); + size_t pos = fileNameWithoutExtension.find(fileExtension); + if (pos != std::string::npos) + { + // If found then erase it from string + fileNameWithoutExtension.erase(pos, fileExtension.length()); + } + int index = 0; + while (ignition::common::exists(_dest)) + { + _dest = ignition::common::joinPaths( + parentPath, + fileNameWithoutExtension + "_" + std::to_string(index) + + fileExtension); + ++index; + } + } + + std::string baseName = ignition::common::basename(_dest); + std::string pathDest = ignition::common::replaceAll(_dest, baseName, ""); + ignition::common::createDirectories(pathDest); + if (!ignition::common::copyFile(_ori, _dest)) + { + errors.emplace_back( + Error(ErrorCode::FILE_READ, "Unable to copy the file [" + _ori + + "] to [" + _dest + "]")); + } + } + else + { + errors.emplace_back( + Error(ErrorCode::FILE_READ, "File [" + _ori + "] does not exist")); + } + return errors; + } + + /// \brief Helper method for getting an asset path + /// \param[in] _tokenName Name of the asset + /// \param[in] _shader Shader that holds the desired asset + /// \return The pxr::SdfAssetPath object that contains the asset identified by + /// _tokenName in _shader + pxr::SdfAssetPath assetPath(const pxr::TfToken &_tokenName, + const pxr::UsdShadeShader &_shader) + { + pxr::SdfAssetPath assetPath; + pxr::UsdShadeInput shadeInput = _shader.GetInput(_tokenName); + shadeInput.Get(&assetPath); + return assetPath; + } + + ///////////////////////////////////////////////// + UsdErrors ParseMaterial(const pxr::UsdPrim &_prim, sdf::Material &_material) + { + UsdErrors errors; + // if the prim is a Geom then get the color values + if (_prim.IsA()) + { + auto variantGeom = pxr::UsdGeomGprim(_prim); + + pxr::VtArray color(0, 0, 0); + variantGeom.GetDisplayColorAttr().Get(&color); + + pxr::VtFloatArray displayOpacity; + const std::string displayOpacityToken = "primvars:displayOpacity"; + auto opacityAttr = _prim.GetAttribute(pxr::TfToken(displayOpacityToken)); + if (!opacityAttr) + { + errors.push_back(UsdError(UsdErrorCode::PRIM_MISSING_ATTRIBUTE, + "Prim at path [" + _prim.GetPath().GetString() + + "] does not have an attribute with a pxr::TfToken of [" + + displayOpacityToken + "]")); + return errors; + } + opacityAttr.Get(&displayOpacity); + + double alpha = 1.0; + if (displayOpacity.size() > 0) + { + alpha = 1 - displayOpacity[0]; + } + // Refer to this comment in github to understand the ambient and diffuse + // https://github.com/osrf/sdformat/pull/526#discussion_r623937715 + _material.SetAmbient( + ignition::math::Color( + ignition::math::clamp(color[0][2] / 0.4, 0.0, 1.0), + ignition::math::clamp(color[0][1] / 0.4, 0.0, 1.0), + ignition::math::clamp(color[0][0] / 0.4, 0.0, 1.0), + alpha)); + _material.SetDiffuse( + ignition::math::Color( + ignition::math::clamp(color[0][2] / 0.8, 0.0, 1.0), + ignition::math::clamp(color[0][1] / 0.8, 0.0, 1.0), + ignition::math::clamp(color[0][0] / 0.8, 0.0, 1.0), + alpha)); + } + // if the prim is a shade Material then get the texture values + else if (_prim.IsA()) + { + auto variantMaterial = pxr::UsdShadeMaterial(_prim); + for (const auto &child : _prim.GetChildren()) + { + if (child.IsA()) + { + auto variantShader = pxr::UsdShadeShader(child); + + bool enableEmission = false; + bool isPBR = false; + sdf::PbrWorkflow pbrWorkflow; + ignition::math::Color emissiveColorCommon; + + for (const auto &input : variantShader.GetInputs()) + { + if (input.GetBaseName() == "diffuse_texture") + { + pxr::SdfAssetPath materialPath = + assetPath(pxr::TfToken("diffuse_texture"), variantShader); + std::string fullAlbedoName = + ignition::common::findFile(ignition::common::basename( + materialPath.GetAssetPath()), false); + std::string dest = materialPath.GetAssetPath(); + UsdErrors errorCopy = copyFile( + fullAlbedoName, dest); + if (!errorCopy.empty()) + { + errors.insert(errors.end(), errorCopy.begin(), errorCopy.end()); + return errors; + } + pbrWorkflow.SetAlbedoMap(dest); + + // We need to set diffuse and specular to (1, 1, 1) otherwise + // the texture is completely black + _material.SetDiffuse(ignition::math::Color(1, 1, 1)); + _material.SetSpecular(ignition::math::Color(1, 1, 1)); + + isPBR = true; + } + else if (input.GetBaseName() == "normalmap_texture") + { + pxr::SdfAssetPath materialPath = + assetPath(pxr::TfToken("normalmap_texture"), variantShader); + std::string fullNormalName = + ignition::common::findFile(ignition::common::basename( + materialPath.GetAssetPath()), false); + std::string dest = materialPath.GetAssetPath(); + auto errorCopy = copyFile(fullNormalName, dest); + if (!errorCopy.empty()) + { + errors.insert(errors.end(), errorCopy.begin(), errorCopy.end()); + return errors; + } + pbrWorkflow.SetNormalMap(dest); + isPBR = true; + } + else if (input.GetBaseName() == "reflectionroughness_texture") + { + pxr::SdfAssetPath materialPath = assetPath( + pxr::TfToken("reflectionroughness_texture"), variantShader); + std::string fullRoughnessName = + ignition::common::findFile(ignition::common::basename( + materialPath.GetAssetPath()), false); + std::string dest = materialPath.GetAssetPath(); + auto errorCopy = copyFile(fullRoughnessName, dest); + if (!errorCopy.empty()) + { + errors.insert(errors.end(), errorCopy.begin(), errorCopy.end()); + return errors; + } + pbrWorkflow.SetRoughnessMap(dest); + isPBR = true; + } + else if (input.GetBaseName() == "metallic_texture") + { + pxr::SdfAssetPath materialPath = assetPath( + pxr::TfToken("metallic_texture"), variantShader); + std::string fullMetalnessName = + ignition::common::findFile(ignition::common::basename( + materialPath.GetAssetPath()), false); + std::string dest = materialPath.GetAssetPath(); + auto errorCopy = copyFile(fullMetalnessName, dest); + if (!errorCopy.empty()) + { + errors.insert(errors.end(), errorCopy.begin(), errorCopy.end()); + return errors; + } + pbrWorkflow.SetMetalnessMap(dest); + + isPBR = true; + } + else if (input.GetBaseName() == "diffuse_color_constant") + { + pxr::GfVec3f diffuseColor(0, 0, 0); + auto sourceInfoV = input.GetConnectedSources(); + if (sourceInfoV.size() > 0) + { + pxr::UsdShadeInput connectedInput = + sourceInfoV[0].source.GetInput(sourceInfoV[0].sourceName); + + const pxr::SdfPath& thisAttrPath = + connectedInput.GetAttr().GetPath(); + auto connectedPrim = + _prim.GetStage()->GetPrimAtPath(thisAttrPath.GetPrimPath()); + if (connectedPrim) + { + connectedPrim.GetAttribute( + pxr::TfToken("inputs:diffuse_color_constant")). + Get(&diffuseColor); + } + } + else + { + pxr::UsdShadeInput diffuseShaderInput = variantShader.GetInput( + pxr::TfToken("diffuse_color_constant")); + diffuseShaderInput.Get(&diffuseColor); + } + _material.SetDiffuse( + ignition::math::Color( + diffuseColor[0], + diffuseColor[1], + diffuseColor[2])); + } + else if (input.GetBaseName() == "metallic_constant") + { + pxr::UsdShadeInput metallicConstantShaderInput = + variantShader.GetInput(pxr::TfToken("metallic_constant")); + float metallicConstant; + metallicConstantShaderInput.Get(&metallicConstant); + pbrWorkflow.SetMetalness(metallicConstant); + isPBR = true; + } + else if (input.GetBaseName() == "reflection_roughness_constant") + { + auto sourceInfoV = input.GetConnectedSources(); + if (sourceInfoV.size() > 0) + { + pxr::UsdShadeInput connectedInput = + sourceInfoV[0].source.GetInput(sourceInfoV[0].sourceName); + + const pxr::SdfPath& thisAttrPath = + connectedInput.GetAttr().GetPath(); + auto connectedPrim = + _prim.GetStage()->GetPrimAtPath(thisAttrPath.GetPrimPath()); + if(connectedPrim) + { + float reflectionRoughnessConstant; + connectedPrim.GetAttribute( + pxr::TfToken("inputs:reflection_roughness_constant")). + Get(&reflectionRoughnessConstant); + pbrWorkflow.SetRoughness(reflectionRoughnessConstant); + isPBR = true; + } + } + else + { + pxr::UsdShadeInput reflectionRoughnessConstantShaderInput = + variantShader.GetInput( + pxr::TfToken("reflection_roughness_constant")); + float reflectionRoughnessConstant; + reflectionRoughnessConstantShaderInput. + Get(&reflectionRoughnessConstant); + pbrWorkflow.SetRoughness(reflectionRoughnessConstant); + isPBR = true; + } + } + else if (input.GetBaseName() == "enable_emission") + { + pxr::UsdShadeInput enableEmissiveShaderInput = + variantShader.GetInput(pxr::TfToken("enable_emission")); + enableEmissiveShaderInput.Get(&enableEmission); + } + else if (input.GetBaseName() == "emissive_color") + { + pxr::GfVec3f emissiveColor(0, 0, 0); + pxr::UsdShadeInput emissiveColorShaderInput = + variantShader.GetInput(pxr::TfToken("emissive_color")); + if (emissiveColorShaderInput.Get(&emissiveColor)) + { + emissiveColorCommon = ignition::math::Color( + emissiveColor[0], + emissiveColor[1], + emissiveColor[2]); + } + } + } + + if (enableEmission) + { + _material.SetEmissive(emissiveColorCommon); + } + + if (isPBR) + { + sdf::Pbr pbr; + pbrWorkflow.SetType(sdf::PbrWorkflowType::METAL); + pbr.SetWorkflow(sdf::PbrWorkflowType::METAL, pbrWorkflow); + _material.SetPbrMaterial(pbr); + } + } + } + } + else + { + errors.push_back(UsdError(UsdErrorCode::PRIM_INCORRECT_SCHEMA_TYPE, + "Prim at path [" + _prim.GetPath().GetString() + + "is not a pxr::UsdGeomGprim or a pxr::UsdShadeMaterial.")); + } + + return errors; + } +} +} +} diff --git a/usd/src/usd_parser/USDMaterial.hh b/usd/src/usd_parser/USDMaterial.hh new file mode 100644 index 000000000..164f1b9e2 --- /dev/null +++ b/usd/src/usd_parser/USDMaterial.hh @@ -0,0 +1,54 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#ifndef USD_PARSER_USDMATERIAL_HH_ +#define USD_PARSER_USDMATERIAL_HH_ + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Material.hh" +#include "sdf/config.hh" +#include "sdf/usd/UsdError.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// brief Parse the material in a USD prim to a sdf::Material + /// If the prim is a pxr::UsdGeomGprim, get the color values. Otherwise, + /// if the prim is a pxr::UsdShadeMaterial, get the texture values + /// \param[in] _prim USD prim where the material is extracted + /// \param[out] _material The sdf::Material representation of _prim's + /// material + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error. + UsdErrors ParseMaterial(const pxr::UsdPrim &_prim, + sdf::Material &_material); +} +} +} +#endif diff --git a/usd/src/usd_parser/USDPhysics.cc b/usd/src/usd_parser/USDPhysics.cc new file mode 100644 index 000000000..a45e1d188 --- /dev/null +++ b/usd/src/usd_parser/USDPhysics.cc @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2022 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 "USDPhysics.hh" + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/World.hh" + +namespace sdf +{ +inline namespace SDF_VERSION_NAMESPACE { +namespace usd +{ + void ParseUSDPhysicsScene( + const pxr::UsdPhysicsScene &_scene, + sdf::World &_world, + double _metersPerUnit) + { + ignition::math::Vector3d worldGravity{0, 0, -1}; + float magnitude {9.8f}; + const auto gravityAttr = _scene.GetGravityDirectionAttr(); + if (gravityAttr) + { + pxr::GfVec3f gravity; + gravityAttr.Get(&gravity); + if (!ignition::math::equal(0.0f, gravity[0]) && + !ignition::math::equal(0.0f, gravity[1]) && + !ignition::math::equal(0.0f, gravity[2])) + { + worldGravity[0] = gravity[0]; + worldGravity[1] = gravity[1]; + worldGravity[2] = gravity[2]; + } + } + + const auto magnitudeAttr = _scene.GetGravityMagnitudeAttr(); + if (magnitudeAttr) + { + magnitudeAttr.Get(&magnitude); + if (!std::isnan(magnitude) && !std::isinf(magnitude)) + { + magnitude = magnitude * _metersPerUnit; + } + else + { + // Use the default value for the gravity if it's not defined. + magnitude = 9.8f; + } + } + _world.SetGravity(worldGravity * magnitude); + } +} +} +} diff --git a/usd/src/usd_parser/USDPhysics.hh b/usd/src/usd_parser/USDPhysics.hh new file mode 100644 index 000000000..1b594ae3b --- /dev/null +++ b/usd/src/usd_parser/USDPhysics.hh @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#ifndef USD_PARSER_USDPHYSICS_HH_ +#define USD_PARSER_USDPHYSICS_HH_ + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/config.hh" +#include "sdf/usd/Export.hh" + +#include "sdf/World.hh" +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse the physics attributes of a USD file + /// \param[in] _scene USD physics scene to extract attributes from + /// \param[out] _world World interface where the data is placed + /// \param[in] _metersPerUnit meters per unit in the USD + void ParseUSDPhysicsScene( + const pxr::UsdPhysicsScene &_scene, + sdf::World &_world, + double _metersPerUnit); + } + } +} + +#endif diff --git a/usd/src/usd_parser/USDPhysics_TEST.cc b/usd/src/usd_parser/USDPhysics_TEST.cc new file mode 100644 index 000000000..d7e159265 --- /dev/null +++ b/usd/src/usd_parser/USDPhysics_TEST.cc @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2022 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 + +#include +#include + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "test_config.h" +#include "test_utils.hh" + +#include "sdf/World.hh" +#include "USDPhysics.hh" + +///////////////////////////////////////////////// +TEST(USDPhysicsTest, AvailablePhysics) +{ + const std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); + const auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + const auto physicsScene = + pxr::UsdPhysicsScene(stage->GetPrimAtPath(pxr::SdfPath("/physics"))); + EXPECT_TRUE(physicsScene); + + sdf::World world; + + const double metersPerUnit = 1.0; + + sdf::usd::ParseUSDPhysicsScene( + physicsScene, world, metersPerUnit); + EXPECT_EQ(ignition::math::Vector3d(0, 0, -9.8), world.Gravity()); +} + +///////////////////////////////////////////////// +TEST(USDPhysicsTest, UnavailablePhysics) +{ + const std::string filename = sdf::testing::TestFile("usd", "upAxisY.usda"); + const auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + const auto physicsScene = + pxr::UsdPhysicsScene(stage->GetPrimAtPath(pxr::SdfPath("/physics"))); + EXPECT_FALSE(physicsScene); + + sdf::World world; + + const double metersPerUnit = 1.0; + + sdf::usd::ParseUSDPhysicsScene( + physicsScene, world, metersPerUnit); + EXPECT_EQ(ignition::math::Vector3d(0, 0, -9.8), world.Gravity()); +} diff --git a/usd/src/usd_parser/USDSensors.cc b/usd/src/usd_parser/USDSensors.cc new file mode 100644 index 000000000..c6d646e05 --- /dev/null +++ b/usd/src/usd_parser/USDSensors.cc @@ -0,0 +1,132 @@ +/* + * Copyright (C) 2022 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 "USDSensors.hh" + +#include + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include "pxr/usd/usdGeom/camera.h" +#include "pxr/usd/usdGeom/gprim.h" +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/usd/usd_parser/USDTransforms.hh" + +#include "sdf/Camera.hh" +#include "sdf/Lidar.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + sdf::Sensor ParseSensors( + const pxr::UsdPrim &_prim, + const USDData &_usdData) + { + sdf::Sensor sensor; + + const std::string primType = + _prim.GetPrimTypeInfo().GetTypeName().GetText(); + + ignition::math::Pose3d pose; + ignition::math::Vector3d scale(1, 1, 1); + GetTransform( + _prim, + _usdData, + pose, + scale, + std::string(_prim.GetParent().GetPath().GetText())); + + if (_prim.IsA()) + { + sensor.SetType(sdf::SensorType::CAMERA); + + sdf::Camera camera; + + auto variantCamera = pxr::UsdGeomCamera(_prim); + float focalLength; + pxr::GfVec2f clippingRange; + variantCamera.GetFocalLengthAttr().Get(&focalLength); + variantCamera.GetClippingRangeAttr().Get(&clippingRange); + + sensor.SetName(_prim.GetPath().GetName()); + camera.SetName(_prim.GetPath().GetName()); + camera.SetHorizontalFov(20.955); + camera.SetLensFocalLength(focalLength); + // Camera is Y up axis, rotate the camera to match with Ignition + ignition::math::Pose3d poseCamera(0, 0, 0, IGN_PI_2, 0, -IGN_PI_2); + sensor.SetRawPose(pose * -poseCamera); + camera.SetNearClip(clippingRange[0]); + camera.SetFarClip(clippingRange[1]); + camera.SetImageWidth(640); + camera.SetImageHeight(480); + camera.SetPixelFormat(sdf::PixelFormatType::RGB_INT8); + sensor.SetCameraSensor(camera); + } + else if (primType == "Lidar") + { + sensor.SetType(sdf::SensorType::GPU_LIDAR); + + sdf::Lidar lidar; + sensor.SetName(_prim.GetPath().GetName()); + sensor.SetRawPose(pose); + + float hFOV; + float hResolution; + float vFOV; + float vResolution; + _prim.GetAttribute(pxr::TfToken("horizontalFov")).Get(&hFOV); + _prim.GetAttribute( + pxr::TfToken("horizontalResolution")).Get(&hResolution); + _prim.GetAttribute(pxr::TfToken("verticalFov")).Get(&vFOV); + _prim.GetAttribute(pxr::TfToken("verticalResolution")).Get(&vResolution); + hResolution = IGN_DTOR(hResolution); + vResolution = IGN_DTOR(vResolution); + hFOV = IGN_DTOR(hFOV); + vFOV = IGN_DTOR(vFOV); + + lidar.SetHorizontalScanMinAngle(ignition::math::Angle(-hFOV / 2)); + lidar.SetHorizontalScanMaxAngle(ignition::math::Angle(hFOV / 2)); + lidar.SetHorizontalScanResolution(1); + lidar.SetHorizontalScanSamples(hFOV / hResolution); + + lidar.SetVerticalScanMinAngle(ignition::math::Angle(-vFOV / 2)); + lidar.SetVerticalScanMaxAngle(ignition::math::Angle(vFOV / 2)); + lidar.SetVerticalScanResolution(1); + lidar.SetVerticalScanSamples(vFOV / vResolution); + + float minRange; + float maxRange; + _prim.GetAttribute(pxr::TfToken("minRange")).Get(&minRange); + _prim.GetAttribute(pxr::TfToken("maxRange")).Get(&maxRange); + + lidar.SetRangeMin(minRange); + lidar.SetRangeMax(maxRange); + lidar.SetRangeResolution(0.1); + + sensor.SetLidarSensor(lidar); + sensor.SetUpdateRate(20.0); + } + return sensor; + } +} +} +} diff --git a/usd/src/usd_parser/USDSensors.hh b/usd/src/usd_parser/USDSensors.hh new file mode 100644 index 000000000..87dac653a --- /dev/null +++ b/usd/src/usd_parser/USDSensors.hh @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#ifndef USD_PARSER_SENSORS_HH +#define USD_PARSER_SENSORS_HH + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Sensor.hh" + +#include "sdf/usd/usd_parser/USDData.hh" + +#include "sdf/config.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse a USD sensor to its SDF representation. + /// Currently, camera and lidar sensors are the only types supported. + /// \param[in] _prim The USD sensor prim + /// \param[in] _usdData Object that holds data about the USD stage + /// \return The sdf::Sensor representation of the USD sensor (_prim) + sdf::Sensor ParseSensors( + const pxr::UsdPrim &_prim, + const USDData &_usdData); + } + } +} + +#endif diff --git a/usd/src/usd_parser/USDSensors_TEST.cc b/usd/src/usd_parser/USDSensors_TEST.cc new file mode 100644 index 000000000..d36109f23 --- /dev/null +++ b/usd/src/usd_parser/USDSensors_TEST.cc @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2022 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 + +#include + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include + +#include "test_config.h" +#include "test_utils.hh" + +#include "USDSensors.hh" + +#include "sdf/Sensor.hh" +#include "sdf/Camera.hh" +#include "sdf/Lidar.hh" +#include "sdf/usd/usd_parser/USDData.hh" + +///////////////////////////////////////////////// +TEST(USDJointTest, JointTest) +{ + const std::string filename = + sdf::testing::TestFile("usd", "sensors.usda"); + const auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + sdf::usd::USDData usdData(filename); + usdData.Init(); + + const auto cameraPrim = stage->GetPrimAtPath(pxr::SdfPath( + "/camera")); + ASSERT_TRUE(cameraPrim); + + sdf::Sensor sensorCamera = sdf::usd::ParseSensors( + cameraPrim, usdData); + + EXPECT_EQ(sdf::SensorType::CAMERA, sensorCamera.Type()); + EXPECT_EQ(ignition::math::Pose3d(-1.83617, 0, 0.773532, 0, 0.267035, -0), + sensorCamera.RawPose()); + auto cameraSDF = sensorCamera.CameraSensor(); + ASSERT_TRUE(cameraSDF); + EXPECT_EQ(640u, cameraSDF->ImageWidth()); + EXPECT_EQ(480u, cameraSDF->ImageHeight()); + EXPECT_EQ(sdf::PixelFormatType::RGB_INT8, cameraSDF->PixelFormat()); + EXPECT_DOUBLE_EQ(1000000, cameraSDF->FarClip()); + EXPECT_DOUBLE_EQ(1, cameraSDF->NearClip()); + EXPECT_DOUBLE_EQ(20.955, cameraSDF->HorizontalFov().Radian()); + EXPECT_DOUBLE_EQ(24.0, cameraSDF->LensFocalLength()); + + const auto lidarPrim = stage->GetPrimAtPath(pxr::SdfPath( + "/lidar")); + ASSERT_TRUE(lidarPrim); + + sdf::Sensor sensorLidar = sdf::usd::ParseSensors( + lidarPrim, usdData); + + EXPECT_EQ(sdf::SensorType::GPU_LIDAR, sensorLidar.Type()); + auto lidarSDF = sensorLidar.LidarSensor(); + ASSERT_TRUE(lidarSDF); + EXPECT_DOUBLE_EQ(-180.00000500895632, + lidarSDF->HorizontalScanMinAngle().Degree()); + EXPECT_DOUBLE_EQ(180.00000500895632, + lidarSDF->HorizontalScanMaxAngle().Degree()); + EXPECT_DOUBLE_EQ(1, lidarSDF->HorizontalScanResolution()); + EXPECT_DOUBLE_EQ(900, lidarSDF->HorizontalScanSamples()); + + EXPECT_DOUBLE_EQ(-15.000000417413029, + lidarSDF->VerticalScanMinAngle().Degree()); + EXPECT_DOUBLE_EQ(15.000000417413029, + lidarSDF->VerticalScanMaxAngle().Degree()); + EXPECT_DOUBLE_EQ(1, lidarSDF->VerticalScanResolution()); + EXPECT_DOUBLE_EQ(7, lidarSDF->VerticalScanSamples()); + + EXPECT_DOUBLE_EQ(0.40000000596046448, lidarSDF->RangeMin()); + EXPECT_DOUBLE_EQ(100, lidarSDF->RangeMax()); + EXPECT_DOUBLE_EQ(0.1, lidarSDF->RangeResolution()); + EXPECT_DOUBLE_EQ(20, sensorLidar.UpdateRate()); +} diff --git a/usd/src/usd_parser/USDStage.cc b/usd/src/usd_parser/USDStage.cc new file mode 100644 index 000000000..3524e6f7d --- /dev/null +++ b/usd/src/usd_parser/USDStage.cc @@ -0,0 +1,139 @@ +/* + * Copyright (C) 2022 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 +#include +#include + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/usd/usd_parser/USDStage.hh" + +namespace sdf { +// Inline bracke to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ + /// \brief USDStage private data. + class USDStage::Implementation + { + public: + /// \brief Up Axis this must be "Z" or "Y". + std::string upAxis = "Z"; + + /// \brief Meter per unit + double metersPerUnit = 1.0; + + /// \brief All USD paths available in the file + std::set paths; + + std::string refFileName; + }; + + ///////////////////////////////////////////////// + USDStage::USDStage(const std::string &_refFileName) + : dataPtr(ignition::utils::MakeImpl()) + { + this->dataPtr->refFileName = _refFileName; + } + + ///////////////////////////////////////////////// + UsdErrors USDStage::Init() + { + UsdErrors errors; + + // Open the stage + auto referencee = pxr::UsdStage::Open(this->dataPtr->refFileName); + if (!referencee) + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::INVALID_USD_FILE, + "Failed to load usd file")); + return errors; + } + + // Get meters per unit + this->dataPtr->metersPerUnit = + pxr::UsdGeomGetStageMetersPerUnit(referencee); + + // is it up axis define, if so, read the value, if the value is + // not 'Y' or 'Z' throw an exception + if (referencee->HasAuthoredMetadata(pxr::UsdGeomTokens->upAxis)) + { + pxr::TfToken axis; + referencee->GetMetadata(pxr::UsdGeomTokens->upAxis, &axis); + this->dataPtr->upAxis = axis.GetText(); + + if (this->dataPtr->upAxis != "Y" && this->dataPtr->upAxis != "Z") + { + errors.emplace_back( + UsdError( + sdf::usd::UsdErrorCode::INVALID_UP_AXIS, + "Up axis should be 'Y' or 'Z'")); + return errors; + } + } + + // Keep all the USD paths + auto range = pxr::UsdPrimRange::Stage(referencee); + for (auto const &_prim : range) + { + if (_prim.IsA()) + { + continue; + } + if (_prim.IsA()) + { + continue; + } + + this->dataPtr->paths.insert(_prim.GetPath().GetName()); + } + return errors; + } + + ///////////////////////////////////////////////// + const std::string &USDStage::UpAxis() const + { + return this->dataPtr->upAxis; + } + + ///////////////////////////////////////////////// + double USDStage::MetersPerUnit() const + { + return this->dataPtr->metersPerUnit; + } + + ///////////////////////////////////////////////// + const std::set &USDStage::USDPaths() const + { + return this->dataPtr->paths; + } +} // usd +} // sdf version namespace +} // sdf diff --git a/usd/src/usd_parser/USDStage_TEST.cc b/usd/src/usd_parser/USDStage_TEST.cc new file mode 100644 index 000000000..4265f5839 --- /dev/null +++ b/usd/src/usd_parser/USDStage_TEST.cc @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2022 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 + +#include + +#include + +#include +#include + +#include "test_config.h" +#include "test_utils.hh" + +///////////////////////////////////////////////// +TEST(USDStage, Constructor) +{ + // Check up Axis equal to Z and metersPerUnit + { + std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); + sdf::usd::USDStage stage(filename); + sdf::usd::UsdErrors errors = stage.Init(); + EXPECT_EQ(0u, errors.size()); + + EXPECT_EQ("Z", stage.UpAxis()); + EXPECT_DOUBLE_EQ(0.01, stage.MetersPerUnit()); + EXPECT_EQ(25u, stage.USDPaths().size()); + } + + // Check up Axis equal to Y and metersPerUnit + { + std::string filename = sdf::testing::TestFile("usd", "upAxisY.usda"); + sdf::usd::USDStage stage(filename); + sdf::usd::UsdErrors errors = stage.Init(); + EXPECT_EQ(0u, errors.size()); + + EXPECT_EQ("Y", stage.UpAxis()); + EXPECT_DOUBLE_EQ(1.0, stage.MetersPerUnit()); + EXPECT_EQ(9u, stage.USDPaths().size()); + } + + // Wrong upaxis + { + sdf::usd::USDStage stage( + sdf::testing::TestFile("usd", "/upAxis_wrong.usda")); + sdf::usd::UsdErrors errors = stage.Init(); + EXPECT_EQ(1u, errors.size()); + } + + // Invalid file + { + sdf::usd::USDStage stage(sdf::testing::TestFile("usd", "invalid_name")); + sdf::usd::UsdErrors errors = stage.Init(); + EXPECT_EQ(1u, errors.size()); + } +} diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc new file mode 100644 index 000000000..7b30b0259 --- /dev/null +++ b/usd/src/usd_parser/USDTransforms.cc @@ -0,0 +1,342 @@ +/* + * Copyright (C) 2022 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 "sdf/usd/usd_parser/USDTransforms.hh" + +#include +#include + +#include +#include + +#include "sdf/usd/usd_parser/USDData.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ + +const char kXFormOpTranslate[] = {"xformOp:translate"}; +const char kXFormOpOrient[] = {"xformOp:orient"}; +const char kXFormOpTransform[] = {"xformOp:transform"}; +const char kXFormOpScale[] = {"xformOp:scale"}; +const char kXFormOpRotateXYZ[] = {"xformOp:rotateXYZ"}; +const char kXFormOpRotateZYX[] = {"xformOp:rotateZYX"}; + +const char kGfVec3fString[] = {"GfVec3f"}; +const char kGfVec3dString[] = {"GfVec3d"}; +const char kGfQuatfString[] = {"GfQuatf"}; +const char kGfQuatdString[] = {"GfQuatd"}; + +/// \brief Private altimeter data. +class UDSTransforms::Implementation +{ + /// \brief Scale of the schema + public: ignition::math::Vector3d scale{1, 1, 1}; + + /// \brief Rotation of the schema + public: std::optional q = std::nullopt; + + /// \brief Translation of the schema + public: ignition::math::Vector3d translate{0, 0, 0}; +}; + +///////////////////////////////////////////////// +UDSTransforms::UDSTransforms() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +////////////////////////////////////////////////// +const ignition::math::Vector3d UDSTransforms::Translation() const +{ + return this->dataPtr->translate; +} + +////////////////////////////////////////////////// +const ignition::math::Vector3d UDSTransforms::Scale() const +{ + return this->dataPtr->scale; +} + +////////////////////////////////////////////////// +const std::optional UDSTransforms::Rotation() const +{ + return this->dataPtr->q; +} + +////////////////////////////////////////////////// +void UDSTransforms::SetTranslation( + const ignition::math::Vector3d &_translate) +{ + this->dataPtr->translate = _translate; +} + +////////////////////////////////////////////////// +void UDSTransforms::SetScale( + const ignition::math::Vector3d &_scale) +{ + this->dataPtr->scale = _scale; +} + +////////////////////////////////////////////////// +void UDSTransforms::SetRotation( + const ignition::math::Quaterniond &_q) +{ + this->dataPtr->q = _q; +} + +////////////////////////////////////////////////// +/// \brief This function will parse all the parent transforms of a prim. +/// \param[in] _prim Initial prim to read the transform +/// \param[in] _usdData USDData structure to get info about the prim, for +/// example: metersperunit +/// \param[out] _tfs A vector with all the transforms +/// \param[out] _scale The scale of the prims +/// \param[in] _schemaToStop Name of the prim where the loop will stop +/// reading transforms +void GetAllTransforms( + const pxr::UsdPrim &_prim, + const USDData &_usdData, + std::vector &_tfs, + ignition::math::Vector3d &_scale, + const std::string &_schemaToStop) +{ + pxr::UsdPrim parent = _prim; + double metersPerUnit = 1.0; + std::string upAxis = "Y"; + + // this assumes that there can only be one stage + const auto stageData = _usdData.FindStage(parent.GetPath().GetName()); + if (stageData.second) { + metersPerUnit = stageData.second->MetersPerUnit(); + upAxis = stageData.second->UpAxis(); + } + + while (parent) + { + if (pxr::TfStringify(parent.GetPath()) == _schemaToStop) + { + return; + } + + UDSTransforms t = ParseUSDTransform(parent); + + ignition::math::Pose3d pose; + _scale *= t.Scale(); + + pose.Pos() = t.Translation() * metersPerUnit; + // scaling is lost when we convert to pose, so we pre-scale the + // translation to make them match the scaled values. + if (!_tfs.empty()) { + auto& child = _tfs.back(); + child.Pos().Set( + child.Pos().X() * t.Scale()[0], + child.Pos().Y() * t.Scale()[1], + child.Pos().Z() * t.Scale()[2]); + } + + if (t.Rotation()) + { + pose.Rot() = t.Rotation().value(); + } + _tfs.push_back(pose); + parent = parent.GetParent(); + } + + if (upAxis == "Y") + { + // Add additional rotation to match with Z up Axis. + // TODO(anyone) handle upAxis == "X". This is a case that is rarely + // used by other renderers + ignition::math::Pose3d poseUpAxis = ignition::math::Pose3d( + ignition::math::Vector3d(0, 0, 0), + ignition::math::Quaterniond(IGN_PI_2, 0, 0)); + _tfs.push_back(poseUpAxis); + } +} + +////////////////////////////////////////////////// +void GetTransform( + const pxr::UsdPrim &_prim, + const USDData &_usdData, + ignition::math::Pose3d &_pose, + ignition::math::Vector3d &_scale, + const std::string &_schemaToStop) +{ + std::vector tfs; + GetAllTransforms(_prim, _usdData, tfs, _scale, _schemaToStop); + for (auto & rt : tfs) + { + _pose = rt * _pose; + } +} + +////////////////////////////////////////////////// +UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) +{ + auto variantGeom = pxr::UsdGeomGprim(_prim); + auto transforms = variantGeom.GetXformOpOrderAttr(); + + pxr::GfVec3f scale(1, 1, 1); + pxr::GfVec3f translate(0, 0, 0); + pxr::GfQuatf rotationQuad(1, 0, 0, 0); + + UDSTransforms t; + + pxr::VtTokenArray xformOpOrder; + transforms.Get(&xformOpOrder); + for (const auto &op : xformOpOrder) + { + if (op == kXFormOpScale) + { + auto attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpScale)); + if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString) + { + attribute.Get(&scale); + } + else if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3dString) + { + pxr::GfVec3d scaleTmp(1, 1, 1); + attribute.Get(&scaleTmp); + scale[0] = static_cast(scaleTmp[0]); + scale[1] = static_cast(scaleTmp[1]); + scale[2] = static_cast(scaleTmp[2]); + } + t.SetScale(ignition::math::Vector3d(scale[0], scale[1], scale[2])); + } + else if (op == kXFormOpRotateZYX || op == kXFormOpRotateXYZ) + { + pxr::GfVec3f rotationEuler(0, 0, 0); + pxr::UsdAttribute attribute; + if (op == kXFormOpRotateZYX) + { + attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpRotateZYX)); + } + else + { + attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpRotateXYZ)); + } + if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString) + { + attribute.Get(&rotationEuler); + } + else if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3dString) + { + pxr::GfVec3d rotationEulerTmp(0, 0, 0); + attribute.Get(&rotationEulerTmp); + rotationEuler[0] = static_cast(rotationEulerTmp[0]); + rotationEuler[1] = static_cast(rotationEulerTmp[1]); + rotationEuler[2] = static_cast(rotationEulerTmp[2]); + } + ignition::math::Quaterniond qX, qY, qZ; + ignition::math::Angle angleX(IGN_DTOR(rotationEuler[0])); + ignition::math::Angle angleY(IGN_DTOR(rotationEuler[1])); + ignition::math::Angle angleZ(IGN_DTOR(rotationEuler[2])); + qX = ignition::math::Quaterniond(angleX.Normalized().Radian(), 0, 0); + qY = ignition::math::Quaterniond(0, angleY.Normalized().Radian(), 0); + qZ = ignition::math::Quaterniond(0, 0, angleZ.Normalized().Radian()); + + // TODO(ahcorde) This part should be reviewed, revisit how rotateXYZ + // and rotateZYX are handle. + // Related issue https://github.com/ignitionrobotics/sdformat/issues/926 + // if (op == kXFormOpRotateZYX) + // { + // std::swap(angleX, angleZ); + // } + t.SetRotation((qX * qY) * qZ); + } + else if (op == kXFormOpTranslate) + { + auto attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpTranslate)); + if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString) + { + attribute.Get(&translate); + } + else if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3dString) + { + pxr::GfVec3d translateTmp(0, 0, 0); + attribute.Get(&translateTmp); + translate[0] = static_cast(translateTmp[0]); + translate[1] = static_cast(translateTmp[1]); + translate[2] = static_cast(translateTmp[2]); + } + t.SetTranslation(ignition::math::Vector3d( + translate[0], + translate[1], + translate[2])); + } + else if (op == kXFormOpOrient) + { + auto attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpOrient)); + if (attribute.GetTypeName().GetCPPTypeName() == kGfQuatfString) + { + attribute.Get(&rotationQuad); + } + else if (attribute.GetTypeName().GetCPPTypeName() == kGfQuatdString) + { + pxr::GfQuatd rotationQuadTmp; + attribute.Get(&rotationQuadTmp); + rotationQuad.SetImaginary( + rotationQuadTmp.GetImaginary()[0], + rotationQuadTmp.GetImaginary()[1], + rotationQuadTmp.GetImaginary()[2]); + rotationQuad.SetReal(rotationQuadTmp.GetReal()); + } + ignition::math::Quaterniond q( + rotationQuad.GetReal(), + rotationQuad.GetImaginary()[0], + rotationQuad.GetImaginary()[1], + rotationQuad.GetImaginary()[2]); + t.SetRotation(q); + } + + if (op == kXFormOpTransform) + { + // TODO(koonpeng) Shear is lost (does sdformat support it?). + pxr::GfMatrix4d transform; + _prim.GetAttribute(pxr::TfToken(kXFormOpTransform)).Get(&transform); + const auto rot = transform.RemoveScaleShear(); + const auto scaleShear = transform * rot.GetInverse(); + + t.SetScale(ignition::math::Vector3d( + scaleShear[0][0], + scaleShear[1][1], + scaleShear[2][2])); + + const auto rotQuat = rot.ExtractRotationQuat(); + t.SetTranslation(ignition::math::Vector3d( + transform[3][0], + transform[3][1], + transform[3][2])); + ignition::math::Quaterniond q( + rotQuat.GetReal(), + rotQuat.GetImaginary()[0], + rotQuat.GetImaginary()[1], + rotQuat.GetImaginary()[2] + ); + t.SetRotation(q); + } + } + return t; +} +} +} +} diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc new file mode 100644 index 000000000..fcfc0929c --- /dev/null +++ b/usd/src/usd_parser/USDTransforms_TEST.cc @@ -0,0 +1,221 @@ +/* + * Copyright (C) 2022 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 +#include +#include + +#include + +#include + +#include "test_config.h" +#include "test_utils.hh" + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/usd/usd_parser/USDTransforms.hh" + +void checkTransforms( + const std::string &_primName, + pxr::UsdStageRefPtr &_stage, + const ignition::math::Vector3d &_translation, + const std::optional &_rotation, + const ignition::math::Vector3d &_scale) +{ + pxr::UsdPrim prim = _stage->GetPrimAtPath(pxr::SdfPath(_primName)); + ASSERT_TRUE(prim); + + sdf::usd::UDSTransforms usdTransforms = + sdf::usd::ParseUSDTransform(prim); + + EXPECT_EQ(_translation, usdTransforms.Translation()); + EXPECT_EQ(_scale, usdTransforms.Scale()); + if (_rotation) + { + ASSERT_TRUE(usdTransforms.Rotation()); + EXPECT_TRUE(ignition::math::equal( + _rotation.value().Roll(), + usdTransforms.Rotation().value().Roll(), 0.1)); + EXPECT_TRUE(ignition::math::equal( + _rotation.value().Pitch(), + usdTransforms.Rotation().value().Pitch(), 0.1)); + EXPECT_TRUE(ignition::math::equal( + _rotation.value().Yaw(), + usdTransforms.Rotation().value().Yaw(), 0.1)); + } +} + +///////////////////////////////////////////////// +TEST(USDTransformsTest, ParseUSDTransform) +{ + std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); + auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + checkTransforms( + "/ground_plane", + stage, + ignition::math::Vector3d(0, 0, -0.125), + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/ground_plane/link/visual/geometry", + stage, + ignition::math::Vector3d(0, 0, 0), + std::nullopt, + ignition::math::Vector3d(100, 100, 0.25) + ); + + checkTransforms( + "/cylinder", + stage, + ignition::math::Vector3d(0, -1.5, 0.5), + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/sphere", + stage, + ignition::math::Vector3d(0, 1.5, 0.5), + ignition::math::Quaterniond( + IGN_DTOR(-62), IGN_DTOR(-47.5), IGN_DTOR(-53.41)), + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/capsule", + stage, + ignition::math::Vector3d(0, -3.0, 0.5), + ignition::math::Quaterniond( + IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2)), + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/capsule/capsule_link/capsule_visual", + stage, + ignition::math::Vector3d(0, 0, 0), + ignition::math::Quaterniond(0, 0, M_PI_2), + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/ellipsoid", + stage, + ignition::math::Vector3d(0, 3.0, 0.5), + ignition::math::Quaterniond( + IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2)), + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry", + stage, + ignition::math::Vector3d(0, 0, 0), + std::nullopt, + ignition::math::Vector3d(0.4, 0.6, 1) + ); + + checkTransforms( + "/sun", + stage, + ignition::math::Vector3d(0, 0, 10), + ignition::math::Quaterniond(0, IGN_DTOR(-35), 0), + ignition::math::Vector3d(1, 1, 1) + ); +} + +///////////////////////////////////////////////// +TEST(USDTransformsTest, GetAllTransform) +{ + { + std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); + auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + sdf::usd::USDData usdData(filename); + usdData.Init(); + + pxr::UsdPrim prim = stage->GetPrimAtPath( + pxr::SdfPath( + "/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry")); + ASSERT_TRUE(prim); + + ignition::math::Pose3d pose; + ignition::math::Vector3d scale{1, 1, 1}; + + sdf::usd::GetTransform(prim, usdData, pose, scale, "/shapes"); + + EXPECT_EQ(ignition::math::Vector3d(0.4, 0.6, 1), scale); + EXPECT_EQ( + ignition::math::Pose3d( + ignition::math::Vector3d(0, 0.03, 0.005), + ignition::math::Quaterniond( + IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2))), + pose); + } + + { + std::string filename = + sdf::testing::TestFile("usd", "nested_transforms.usda"); + auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + sdf::usd::USDData usdData(filename); + usdData.Init(); + + std::function verifyNestedTf = + [&](const std::string &_path, + const ignition::math::Vector3d &_posePrim, + const ignition::math::Quaterniond &_qPrim) + { + pxr::UsdPrim prim = stage->GetPrimAtPath(pxr::SdfPath(_path)); + ASSERT_TRUE(prim); + + ignition::math::Pose3d pose; + ignition::math::Vector3d scale{1, 1, 1}; + + sdf::usd::GetTransform(prim, usdData, pose, scale, "/transforms"); + + EXPECT_EQ(ignition::math::Vector3d(1, 1, 1), scale); + EXPECT_EQ(ignition::math::Pose3d(_posePrim, _qPrim), pose); + }; + + verifyNestedTf( + "/transforms/nested_transforms_XYZ/child_transform", + ignition::math::Vector3d(0.01, 0.01, 0), + ignition::math::Quaterniond(0, 0, IGN_DTOR(90))); + verifyNestedTf( + "/transforms/nested_transforms_ZYX/child_transform", + ignition::math::Vector3d(0.02, 0.0, 0), + ignition::math::Quaterniond(IGN_DTOR(90), 0, 0)); + } +} diff --git a/usd/src/usd_parser/USDWorld.cc b/usd/src/usd_parser/USDWorld.cc new file mode 100644 index 000000000..af2e7341f --- /dev/null +++ b/usd/src/usd_parser/USDWorld.cc @@ -0,0 +1,349 @@ +/* + * Copyright (C) 2022 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 "USDWorld.hh" + +#include +#include +#include +#include +#include +#include + +#include + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/usd/usd_parser/USDData.hh" +#include "sdf/usd/usd_parser/USDStage.hh" +#include "sdf/usd/usd_parser/USDTransforms.hh" + +#include "USDJoints.hh" +#include "USDLights.hh" +#include "USDPhysics.hh" +#include "USDSensors.hh" +#include "USDLinks.hh" + +#include "sdf/Collision.hh" +#include "sdf/Light.hh" +#include "sdf/Link.hh" +#include "sdf/Model.hh" +#include "sdf/Plugin.hh" +#include "sdf/Visual.hh" +#include "sdf/World.hh" + +namespace sdf +{ +inline namespace SDF_VERSION_NAMESPACE { +namespace usd +{ + UsdErrors parseUSDWorld(const std::string &_inputFileName, + sdf::World &_world) + { + UsdErrors errors; + USDData usdData(_inputFileName); + errors = usdData.Init(); + if (!errors.empty()) + return errors; + errors = usdData.ParseMaterials(); + if (!errors.empty()) + return errors; + + auto reference = pxr::UsdStage::Open(_inputFileName); + if (!reference) + { + errors.emplace_back(UsdErrorCode::INVALID_USD_FILE, + "Unable to open [" + _inputFileName + "]"); + return errors; + } + std::string worldName = reference->GetDefaultPrim().GetName().GetText(); + if (worldName.empty()) + { + _world.SetName("world_name"); + } + else + { + _world.SetName(worldName + "_world"); + } + + std::string linkName; + std::string currentModelName; + + // USD link may have scale, store this value to apply this to the sdf visual + // the key is the name of the link and the value is the scale value + std::map linkScaleMap; + + auto range = pxr::UsdPrimRange::Stage(reference); + for (const auto &prim : range) + { + // Skip materials, the data is already available in the USDData class + if (prim.IsA() || prim.IsA()) + { + continue; + } + + std::string primName = prim.GetName(); + std::string primPath = pxr::TfStringify(prim.GetPath()); + std::string primType = prim.GetPrimTypeInfo().GetTypeName().GetText(); + + std::vector primPathTokens = + ignition::common::split(primPath, "/"); + + // This assumption on the scene graph wouldn't hold if the usd does + // not come from Isaac Sim + if (primPathTokens.size() == 1 && !prim.IsA() + && !prim.IsA() + && !prim.IsA() + && !prim.IsA() + && !prim.IsA()) + { + sdf::Model model = sdf::Model(); + model.SetName(primPathTokens[0]); + currentModelName = primPathTokens[0]; + + ignition::math::Pose3d pose; + ignition::math::Vector3d scale{1, 1, 1}; + + GetTransform( + prim, + usdData, + pose, + scale, + model.Name()); + + model.SetRawPose(pose); + model.SetStatic(!prim.HasAPI()); + + _world.AddModel(model); + } + + // In general USD models used in Issac Sim define the model path + // under a root path for example: + // -> /robot_name/robot_name_link0 + // But sometimes for enviroments it uses just a simple path: + // -> /ground_plane + // -> /wall_0 + // the shortName variable defines if this is the first case when it's + // False or when it's true then it's the second case. + // This conversion might only work with Issac Sim USDs + // TODO(adlarkin) find a better way to get root model prims/parent prims + // of lights attached to the stage: see + // https://github.com/ignitionrobotics/sdformat/issues/927 + if (primPathTokens.size() >= 2) + { + bool shortName = false; + if (primPathTokens.size() == 2) + { + if (prim.IsA() || (primType == "Plane")) + { + if (primName != "imu") + { + linkName = "/" + primPathTokens[0]; + shortName = true; + } + } + } + if (!shortName) + { + linkName = "/" + primPathTokens[0] + "/" + primPathTokens[1]; + } + } + + if (prim.IsA() || + prim.IsA()) + { + auto light = ParseUSDLights(prim, usdData, linkName); + light->SetName(primName); + if (light) + { + _world.AddLight(light.value()); + // TODO(ahcorde) Include lights which are inside links + } + continue; + } + // TODO(anyone) support converting other USD light types + + sdf::Model *modelPtr = nullptr; + if (!currentModelName.empty()) + { + modelPtr = _world.ModelByName(currentModelName); + if (!modelPtr) + { + errors.push_back(UsdError(UsdErrorCode::USD_TO_SDF_PARSING_ERROR, + "Unable to find a sdf::Model named [" + currentModelName + + "] in world named [" + _world.Name() + + "], but a sdf::Model with this name should exist.")); + return errors; + } + } + + if (prim.IsA()) + { + if (!modelPtr) + { + errors.push_back(UsdError(UsdErrorCode::USD_TO_SDF_PARSING_ERROR, + "Unable to parse joint corresponding to USD prim [" + + std::string(prim.GetName()) + + "] because the corresponding sdf::Model object wasn't found.")); + return errors; + } + sdf::Joint joint; + auto errorsJoint = ParseJoints(prim, usdData, joint); + if (!errorsJoint.empty()) + { + errors.push_back(UsdError(UsdErrorCode::USD_TO_SDF_PARSING_ERROR, + "Unable to find parse UsdPhysicsJoint [" + + std::string(prim.GetName()) + "]")); + return errors; + } + modelPtr->AddJoint(joint); + continue; + } + + if (prim.IsA()) + { + std::pair> data = + usdData.FindStage(primName); + if (!data.second) + { + errors.push_back(UsdError(UsdErrorCode::INVALID_PRIM_PATH, + "Unable to retrieve the pxr::UsdPhysicsScene named [" + + primName + "]")); + return errors; + } + + ParseUSDPhysicsScene(pxr::UsdPhysicsScene(prim), _world, + data.second->MetersPerUnit()); + continue; + } + + if (prim.IsA() || primType == "Lidar") + { + if (!linkName.empty()) + { + auto sensor = ParseSensors(prim, usdData); + auto link = modelPtr->LinkByName(linkName); + if (link != nullptr) + { + link->AddSensor(sensor); + } + } + continue; + } + + if (!prim.IsA() && !(primType == "Plane")) + { + continue; + } + + if (!modelPtr) + { + errors.push_back(UsdError(UsdErrorCode::USD_TO_SDF_PARSING_ERROR, + "Unable to parse link named [" + linkName + + "] because the corresponding sdf::Model object wasn't found.")); + return errors; + } + + std::optional optionalLink; + if (auto linkInserted = modelPtr->LinkByName(linkName)) + { + optionalLink = *linkInserted; + auto scale = linkScaleMap.find(linkName); + if (scale == linkScaleMap.end()) + { + scale = linkScaleMap.insert( + {linkName, ignition::math::Vector3d(1, 1, 1)}).first; + } + sdf::usd::ParseUSDLinks( + prim, linkName, optionalLink, usdData, scale->second); + } + else + { + ignition::math::Vector3d scale{1, 1, 1}; + + sdf::usd::ParseUSDLinks(prim, linkName, optionalLink, usdData, scale); + linkScaleMap[linkName] = scale; + + if (optionalLink && !optionalLink->Name().empty() && + !modelPtr->LinkByName(optionalLink->Name())) + { + modelPtr->AddLink(optionalLink.value()); + } + } + } + + for (unsigned int i = 0; i < _world.LightCount(); ++i) + { + auto light = _world.LightByIndex(i); + light->SetName(ignition::common::basename(light->Name())); + } + + for (unsigned int i = 0; i < _world.ModelCount(); ++i) + { + const auto m = _world.ModelByIndex(i); + + // We might include some empty models + // for example: some models create a world path to set up physics + // but there is no model data inside + // TODO(ahcorde) Add a RemoveModelByName() method in sdf::World + if (m->LinkCount() == 0) + { + sdf::Link emptyLink; + emptyLink.SetName("emptyLink"); + m->AddLink(emptyLink); + } + } + + // Add some plugins to run the Ignition Gazebo simulation + sdf::Plugin physicsPlugin; + physicsPlugin.SetName("ignition::gazebo::systems::Physics"); + physicsPlugin.SetFilename("ignition-gazebo-physics-system"); + _world.AddPlugin(physicsPlugin); + + sdf::Plugin sensorsPlugin; + sensorsPlugin.SetName("ignition::gazebo::systems::Sensors"); + sensorsPlugin.SetFilename("ignition-gazebo-sensors-system"); + _world.AddPlugin(sensorsPlugin); + + sdf::Plugin userCommandsPlugin; + userCommandsPlugin.SetName("ignition::gazebo::systems::UserCommands"); + userCommandsPlugin.SetFilename("ignition-gazebo-user-commands-system"); + _world.AddPlugin(userCommandsPlugin); + + sdf::Plugin sceneBroadcasterPlugin; + sceneBroadcasterPlugin.SetName( + "ignition::gazebo::systems::SceneBroadcaster"); + sceneBroadcasterPlugin.SetFilename( + "ignition-gazebo-scene-broadcaster-system"); + _world.AddPlugin(sceneBroadcasterPlugin); + + return errors; + } +} +} +} diff --git a/usd/src/usd_parser/USDWorld.hh b/usd/src/usd_parser/USDWorld.hh new file mode 100644 index 000000000..e51dea0e5 --- /dev/null +++ b/usd/src/usd_parser/USDWorld.hh @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#ifndef USD_PARSER_USDWORLD_HH_ +#define USD_PARSER_USDWORLD_HH_ + +#include + +#include "sdf/sdf_config.h" +#include "sdf/usd/UsdError.hh" + +#include "sdf/World.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse the world information of a USD file + /// \param[in] _inputFileNameUsd Path where the USD is located + /// \param[out] _world World interface where all USD world data is placed + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when parsing the world information of _inputFileNameUsd + UsdErrors parseUSDWorld( + const std::string &_inputFileNameUsd, + sdf::World &_world); + } + } +} +#endif diff --git a/usd/src/usd_parser/polygon_helper.cc b/usd/src/usd_parser/polygon_helper.cc new file mode 100644 index 000000000..943a6395e --- /dev/null +++ b/usd/src/usd_parser/polygon_helper.cc @@ -0,0 +1,69 @@ +/* + * Copyright (C) 2022 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 "polygon_helper.hh" + +#include +#include + +namespace sdf +{ +// Inline bracke to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ + UsdErrors PolygonToTriangles( + const pxr::VtIntArray &_faceVertexIndices, + const pxr::VtIntArray &_faceVertexCounts, + std::vector &_triangles) + { + UsdErrors errors; + // TODO(koon peng) Use more robust algorithms. + // For reference, blender supports "ear-clipping", and "Beauty". + // https://blender.stackexchange.com/questions/215553/what-algorithm-is-used-for-beauty-triangulation + // ref: https://en.wikipedia.org/wiki/Polygon_triangulation + size_t count = 0; + for (const auto &vCount : _faceVertexCounts) + { + count += vCount - 2; + } + _triangles.reserve(count * 3); + + size_t cur = 0; + for (const auto &vCount : _faceVertexCounts) + { + for (size_t i = cur + 2; i < cur + vCount; i++) + { + _triangles.emplace_back(_faceVertexIndices[cur]); + _triangles.emplace_back(_faceVertexIndices[i - 1]); + _triangles.emplace_back(_faceVertexIndices[i]); + } + cur += vCount; + } + + if (_triangles.size() != count * 3) + { + errors.push_back(UsdError(UsdErrorCode::USD_TO_SDF_POLYGON_PARSING_ERROR, + "Unable to parse the polygon mesh")); + } + + return errors; + } +} +} +} diff --git a/usd/src/usd_parser/polygon_helper.hh b/usd/src/usd_parser/polygon_helper.hh new file mode 100644 index 000000000..e325a13ac --- /dev/null +++ b/usd/src/usd_parser/polygon_helper.hh @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#ifndef USD_PARSER_POLYGON_HELPER_HH +#define USD_PARSER_POLYGON_HELPER_HH + +#include + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/config.hh" +#include "sdf/usd/Export.hh" +#include "sdf/usd/UsdError.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Converts vertex indices of a polygon mesh to vertex indices + /// of a triangle mesh. + /// \details This uses the fan-triangulating algorithm, so it only works if + /// all polygons in the mesh are convex. + /// \param[in] _faceVertexIndices A flat list of vertex indices of a polygon + /// mesh + /// \param[in] _faceVertexCounts A list containing the number of vertices for + /// each + /// face of the mesh. + /// \param[out] _triangles A flat list of vertex indices, with each face + /// converted to one or more triangles. + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when parsing the polygons. + UsdErrors PolygonToTriangles( + const pxr::VtIntArray &_faceVertexIndices, + const pxr::VtIntArray &_faceVertexCounts, + std::vector &_triangles); +} +} +} + +#endif diff --git a/usd/src/usd_parser/usd2sdf_TEST.cc b/usd/src/usd_parser/usd2sdf_TEST.cc new file mode 100644 index 000000000..47c0d4bb0 --- /dev/null +++ b/usd/src/usd_parser/usd2sdf_TEST.cc @@ -0,0 +1,159 @@ +/* + * Copyright (C) 2022 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 +#include + +#include + +#include +#include +#include + +#include + +#include "sdf/Model.hh" +#include "sdf/Root.hh" +#include "sdf/World.hh" +#include "test_config.h" +#include "test_utils.hh" + +#ifdef _WIN32 + #define popen _popen + #define pclose _pclose +#endif + +static std::string usd2sdfCommand() +{ + return ignition::common::joinPaths(std::string(PROJECT_BINARY_DIR), "bin", + "usd2sdf"); +} + +///////////////////////////////////////////////// +std::string custom_exec_str(std::string _cmd) +{ + _cmd += " 2>&1"; + FILE *pipe = popen(_cmd.c_str(), "r"); + + if (!pipe) + return "ERROR"; + + char buffer[128]; + std::string result = ""; + + while (!feof(pipe)) + { + if (fgets(buffer, 128, pipe) != NULL) + result += buffer; + } + + pclose(pipe); + return result; +} + +///////////////////////////////////////////////// +TEST(version_cmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + std::string output = + custom_exec_str(usd2sdfCommand() + " --version"); + EXPECT_EQ(output, std::string(SDF_VERSION_FULL) + "\n"); +} + +///////////////////////////////////////////////// +TEST(check_cmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + const auto tmp = ignition::common::createTempDirectory("usd", + ignition::common::tempDirectoryPath()); + + auto systemPaths = ignition::common::systemPaths(); + systemPaths->AddFilePaths(ignition::common::joinPaths( + sdf::testing::TestFile("usd"), "materials", "textures")); + // Check a good SDF file + { + const std::string path = sdf::testing::TestFile("usd", "upAxisZ.usda"); + const auto outputSdfFilePath = + ignition::common::joinPaths(tmp, "upAxisZ.sdf"); + EXPECT_FALSE(ignition::common::isFile(outputSdfFilePath)); + const std::string output = + custom_exec_str(usd2sdfCommand() + " " + path + " " + outputSdfFilePath); + + // make sure that a sdf file was generated + ASSERT_TRUE(ignition::common::isFile(outputSdfFilePath)) << output; + + // check the contents of the generated SDF file + sdf::Root root; + const auto errors = root.Load(outputSdfFilePath); + EXPECT_TRUE(errors.empty()); + + // check the value of the gravity element + ASSERT_EQ(1u, root.WorldCount()); + const auto world = root.WorldByIndex(0u); + ASSERT_NE(nullptr, world); + EXPECT_DOUBLE_EQ(0.0, world->Gravity()[0]); + EXPECT_DOUBLE_EQ(0.0, world->Gravity()[1]); + EXPECT_DOUBLE_EQ(-0.098, world->Gravity()[2]); + + auto plugins = world->Plugins(); + EXPECT_EQ(4u, plugins.size()); + EXPECT_EQ("ignition::gazebo::systems::Physics", plugins[0].Name()); + EXPECT_EQ("ignition-gazebo-physics-system", plugins[0].Filename()); + + EXPECT_EQ("ignition::gazebo::systems::Sensors", plugins[1].Name()); + EXPECT_EQ("ignition-gazebo-sensors-system", plugins[1].Filename()); + + EXPECT_EQ("ignition::gazebo::systems::UserCommands", plugins[2].Name()); + EXPECT_EQ("ignition-gazebo-user-commands-system", plugins[2].Filename()); + + EXPECT_EQ("ignition::gazebo::systems::SceneBroadcaster", plugins[3].Name()); + EXPECT_EQ( + "ignition-gazebo-scene-broadcaster-system", plugins[3].Filename()); + + // make sure all models in the USD file were correctly parsed to SDF + std::set savedModelNames; + for (unsigned int i = 0; i < world->ModelCount(); ++i) + savedModelNames.insert(world->ModelByIndex(i)->Name()); + EXPECT_EQ(6u, savedModelNames.size()); + + EXPECT_NE(savedModelNames.end(), savedModelNames.find("ground_plane")); + EXPECT_NE(savedModelNames.end(), savedModelNames.find("box")); + EXPECT_NE(savedModelNames.end(), savedModelNames.find("cylinder")); + EXPECT_NE(savedModelNames.end(), savedModelNames.find("sphere")); + EXPECT_NE(savedModelNames.end(), savedModelNames.find("capsule")); + EXPECT_NE(savedModelNames.end(), savedModelNames.find("ellipsoid")); + + // check for static/non-static models + ASSERT_NE(nullptr, world->ModelByName("ground_plane")); + EXPECT_TRUE(world->ModelByName("ground_plane")->Static()); + ASSERT_NE(nullptr, world->ModelByName("box")); + EXPECT_FALSE(world->ModelByName("box")->Static()); + + // check that models have the right links + std::function checkLink = + [&world](const std::string &_modelName, const std::string &_linkName) + { + const auto modelPtr = world->ModelByName(_modelName); + ASSERT_NE(nullptr, modelPtr); + EXPECT_EQ(1u, modelPtr->LinkCount()); + EXPECT_NE(nullptr, modelPtr->LinkByName(_linkName)); + }; + checkLink("ground_plane", "link"); + checkLink("box", "box_link"); + checkLink("cylinder", "cylinder_link"); + checkLink("sphere", "sphere_link"); + checkLink("capsule", "capsule_link"); + checkLink("ellipsoid", "ellipsoid_link"); + } +}