diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 8a0a9947a..aa170f7fe 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -4,7 +4,7 @@ on: pull_request: push: branches: - - 'sdf[0-9]?' + - 'sdf[1-9]?[0-9]' - 'main' jobs: diff --git a/CMakeLists.txt b/CMakeLists.txt index 2eec2d684..f1eed1eaa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,6 +49,13 @@ if (BUILD_SDF) # Find tinyxml2. gz_find_package(TINYXML2 REQUIRED) + ################################################# + # Find DL if doing relocatable installation + if (GZ_ENABLE_RELOCATABLE_INSTALL) + gz_find_package(DL REQUIRED) + endif() + + ################################################ # Find urdfdom parser. Logic: # diff --git a/Changelog.md b/Changelog.md index 44167531a..211119ff2 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,9 +1,147 @@ ## libsdformat 15.X -### libsdformat 15.X.X (202X-XX-XX) +### libsdformat 15.0.0 (2024-09-25) + +1. **Baseline:** this includes all changes from 14.5.0 and earlier. + +1. Use colcon for Windows building compilation + * [Pull request #1481](https://github.com/gazebosim/sdformat/pull/1481) + +1. Spec 1.12: link_state, joint_state changes + * [Pull request #1461](https://github.com/gazebosim/sdformat/pull/1461) + +1. Fix symbol checking test when compiled with debug symbols + * [Pull request #1474](https://github.com/gazebosim/sdformat/pull/1474) + +1. README: update badges for sdf15 + * [Pull request #1472](https://github.com/gazebosim/sdformat/pull/1472) + +1. Ionic Changelog + * [Pull request #1471](https://github.com/gazebosim/sdformat/pull/1471) + +1. Spec 1.12: add `_state` suffix to //state subelements + * [Pull request #1455](https://github.com/gazebosim/sdformat/pull/1455) + +1. Add optional binary relocatability + * [Pull request #1414](https://github.com/gazebosim/sdformat/pull/1414) + * [Pull request #1468](https://github.com/gazebosim/sdformat/pull/1468) + * [Pull request #1469](https://github.com/gazebosim/sdformat/pull/1469) + +1. FrameSemantics: fix NullVertex warnings + * [Pull request #1460](https://github.com/gazebosim/sdformat/pull/1460) + * [Pull request #1459](https://github.com/gazebosim/sdformat/pull/1459) + * [Pull request #1458](https://github.com/gazebosim/sdformat/pull/1458) + +1. Remove deprecated APIs + * [Pull request #1456](https://github.com/gazebosim/sdformat/pull/1456) + +1. Spec 1.12: add `//sensor/frame_id` + * [Pull request #1454](https://github.com/gazebosim/sdformat/pull/1454) + +1. Disable latex and class hierarchy generation + * [Pull request #1447](https://github.com/gazebosim/sdformat/pull/1447) + +1. Print auto inertial values with `gz sdf --print --expand-auto-inertials` + * [Pull request #1422](https://github.com/gazebosim/sdformat/pull/1422) + +1. Add cone shape to SDFormat spec + * [Pull request #1418](https://github.com/gazebosim/sdformat/pull/1418) + * [Pull request #1434](https://github.com/gazebosim/sdformat/pull/1434) + +1. Enable 24.04 CI, remove distutils dependency + * [Pull request #1408](https://github.com/gazebosim/sdformat/pull/1408) + +1. Change behavior of `Param::Get` + * [Pull request #1397](https://github.com/gazebosim/sdformat/pull/1397) + +1. Parse kinematic property in Link, expand spec documentation of property + * [Pull request #1390](https://github.com/gazebosim/sdformat/pull/1390) + * [Pull request #1399](https://github.com/gazebosim/sdformat/pull/1399) + * [Pull request #1462](https://github.com/gazebosim/sdformat/pull/1462) + +1. Spec 1.11+: add `//mesh/@optimization`, `//mesh/convex_decomposition` + * [Pull request #1382](https://github.com/gazebosim/sdformat/pull/1382) + * [Pull request #1386](https://github.com/gazebosim/sdformat/pull/1386) + * [Pull request #1403](https://github.com/gazebosim/sdformat/pull/1403) + +1. Copy 1.11 spec to 1.12 for libsdformat15 + * [Pull request #1375](https://github.com/gazebosim/sdformat/pull/1375) + +1. Fix find Python3 logic and macOS workflow + * [Pull request #1367](https://github.com/gazebosim/sdformat/pull/1367) + +1. Remove `HIDE_SYMBOLS_BY_DEFAULT`: replace by a default configuration in gz-cmake. + * [Pull request #1355](https://github.com/gazebosim/sdformat/pull/1355) + +1. Dependency version bumps in ionic: use gz-cmake4, gz-utils3, gz-math8 + * [Pull request #1340](https://github.com/gazebosim/sdformat/pull/1340) + +1. Bump major version to 15 + * [Pull request #1338](https://github.com/gazebosim/sdformat/pull/1338) ## libsdformat 14.X +### libsdformat 14.5.0 (2024-08-05) + +1. Adding Errors structure to XmlUtils + * [Pull request #1296](https://github.com/gazebosim/sdformat/pull/1296) + +1. Disable latex and class hierarchy generation + * [Pull request #1447](https://github.com/gazebosim/sdformat/pull/1447) + +1. Added SetHeightMap and Heighmap to Geometry Python binding + * [Pull request #1440](https://github.com/gazebosim/sdformat/pull/1440) + +1. workflows/ci.yml fix push branch regex + * [Pull request #1445](https://github.com/gazebosim/sdformat/pull/1445) + +1. SDF.cc update calls to use sdf::Errors output + * [Pull request #1295](https://github.com/gazebosim/sdformat/pull/1295) + +1. Added World::ActorByName + * [Pull request #1436](https://github.com/gazebosim/sdformat/pull/1436) + +### libsdformat 14.4.0 (2024-06-20) + +1. Add Cone as a primitive parametric shape. + * [Pull request #1415](https://github.com/gazebosim/sdformat/pull/1415) + * Thanks to Benjamin Perseghetti + +1. Add custom attribute to custom element in test + * [Pull request #1406](https://github.com/gazebosim/sdformat/pull/1406) + +### libsdformat 14.3.0 (2024-06-14) + +1. Backport voxel_resolution sdf element + * [Pull request #1429](https://github.com/gazebosim/sdformat/pull/1429) + +1. Added Automatic Moment of Inertia Calculations for Basic Shapes Python wrappers + * [Pull request #1424](https://github.com/gazebosim/sdformat/pull/1424) + +1. Add support for no gravity link + * [Pull request #1410](https://github.com/gazebosim/sdformat/pull/1410) + * [Pull request #1419](https://github.com/gazebosim/sdformat/pull/1419) + +1. Update default camera instrinsics skew to 0, which matches spec + * [Pull request #1423](https://github.com/gazebosim/sdformat/pull/1423) + * [Pull request #1425](https://github.com/gazebosim/sdformat/pull/1425) + +1. Allow empty strings in plugin and custom attributes + * [Pull request #1407](https://github.com/gazebosim/sdformat/pull/1407) + +1. (Backport) Enable 24.04 CI, remove distutils dependency + * [Pull request #1413](https://github.com/gazebosim/sdformat/pull/1413) + +1. Fix macOS workflow and backport windows fix + * [Pull request #1409](https://github.com/gazebosim/sdformat/pull/1409) + +1. Fix warning with pybind11 2.12 + * [Pull request #1389](https://github.com/gazebosim/sdformat/pull/1389) + +1. Add bullet and torsional friction DOM + * [Pull request #1351](https://github.com/gazebosim/sdformat/pull/1351) + * [Pull request #1427](https://github.com/gazebosim/sdformat/pull/1427) + ### libsdformat 14.2.0 (2024-04-23) 1. Fix trivial warning on 24.04 for JointAxis_TEST.cc @@ -132,6 +270,50 @@ ## libsdformat 13.X +### libsdformat 13.8.0 (2024-06-25) + +1. Added `World::ActorByName` + * [Pull request #1436](https://github.com/gazebosim/sdformat/pull/1436) + +1. Backport #1367 to Garden: Fix find Python3 logic. + * [Pull request #1370](https://github.com/gazebosim/sdformat/pull/1370) + +### libsdformat 13.7.0 (2024-06-13) + +1. Add support for no gravity link + * [Pull request #1410](https://github.com/gazebosim/sdformat/pull/1410) + * [Pull request #1419](https://github.com/gazebosim/sdformat/pull/1419) + +1. Fix macOS workflow and backport windows fix + * [Pull request #1409](https://github.com/gazebosim/sdformat/pull/1409) + +1. Fix warning with pybind11 2.12 + * [Pull request #1389](https://github.com/gazebosim/sdformat/pull/1389) + +1. Add bullet and torsional friction DOM + * [Pull request #1351](https://github.com/gazebosim/sdformat/pull/1351) + * [Pull request #1427](https://github.com/gazebosim/sdformat/pull/1427) + +1. Resolve URIs relative to file path + * [Pull request #1373](https://github.com/gazebosim/sdformat/pull/1373) + +1. Bazel updates for Garden build + * [Pull request #1239](https://github.com/gazebosim/sdformat/pull/1239) + +1. Fix static builds and optimize test compilation + * [Pull request #1343](https://github.com/gazebosim/sdformat/pull/1343) + * [Pull request #1347](https://github.com/gazebosim/sdformat/pull/1347) + +1. Install ruby commands on Windows + * [Pull request #1339](https://github.com/gazebosim/sdformat/pull/1339) + * [Pull request #1341](https://github.com/gazebosim/sdformat/pull/1341) + +1. Update github action workflows + * [Pull request #1345](https://github.com/gazebosim/sdformat/pull/1345) + +1. URDF->SDF handle links with no inertia or small mass + * [Pull request #1238](https://github.com/gazebosim/sdformat/pull/1238) + ### libsdformat 13.6.0 (2023-08-30) 1. Use relative path in an urdf include to avoid confusion between internal and system headers @@ -594,6 +776,21 @@ ## libsdformat 12.X +### libsdformat 12.8.0 (2024-06-06) + +1. Add support for no gravity link + * [Pull request #1410](https://github.com/gazebosim/sdformat/pull/1410) + +1. Add bullet and torsional friction DOM + * [Pull request #1351](https://github.com/gazebosim/sdformat/pull/1351) + +1. Fix static builds and optimize test compilation + * [Pull request #1343](https://github.com/gazebosim/sdformat/pull/1343) + * [Pull request #1347](https://github.com/gazebosim/sdformat/pull/1347) + +1. Update github action workflows + * [Pull request #1345](https://github.com/gazebosim/sdformat/pull/1345) + ### libsdformat 12.7.2 (2023-09-01) 1. Fixed 1.9/light.sdf diff --git a/Migration.md b/Migration.md index 410c4931b..5d036b71b 100644 --- a/Migration.md +++ b/Migration.md @@ -30,6 +30,35 @@ but with improved human-readability.. matches `"0"`, `"1"`, `"true"`, or `"false"` and returns `false` otherwise. +### Deprecations + +- **sdf/Camera.hh**: + + The `//sensor/camera/optical_frame_id` SDF element and corresponding functions + in the Camera DOM class are deprecated. Please specify camera frame using + the `//sensor/frame_id` SDF element instead. + + ***Deprecation:*** std::string OpticalFrameId() const + + ***Replacement:*** std::string Sensor::FrameId() const + + ***Deprecation:*** void SetOpticalFrameId(const std::string &) + + ***Replacement:*** void Sensor::SetFrameId(const std::string &) + +### Removals + +- **sdf/Joint.hh**: + + `const std::string &ChildLinkName() const` (use `ChildName()` instead) + + `const std::string &ParentLinkName() const` (use `ParentName()` instead) + + `void SetChildLinkName(const std::string &)` (use `SetChildName()` instead) + + `void SetParentLinkName(const std::string &)` (use `SetParentName()` instead) + +- **sdf/SDFImpl.hh**: + + `void Root(const ElementPtr)` (use `SetRoot(const ElementPtr)` instead) + +- **sdf/Types.hh**: + + `const std::string &kSdfScopeDelimiter` (use `kScopeDelimiter` instead) + + `const std::string &SdfScopeDelimiter()` (use `kScopeDelimiter` instead) + +- **sdf/parser.hh**: + + `bool checkJointParentChildLinkNames(const sdf::Root *)` (use `checkJointParentChildNames(const sdf::Root *)` instead) + ## libsdformat 13.x to 14.x ### Additions @@ -40,6 +69,9 @@ but with improved human-readability.. ### Modifications +1. The default camera lens intrinsics skew value in the Camera DOM class changed + from `1` to `0` to match the SDF specification. + 1. World class only renames frames with name collisions if original file version is 1.6 or earlier. Name collisions in newer files will cause `DUPLICATE_NAME` errors, which now matches the behavior of the Model class. @@ -596,6 +628,64 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. ## SDFormat specification 1.11 to 1.12 +### Additions + +1. **joint_state.sdf**: + + `//joint_state/axis_state/position` + + `//joint_state/axis_state/velocity` + + `//joint_state/axis_state/acceleration` + + `//joint_state/axis_state/effort` + + `//joint_state/axis2_state/position` + + `//joint_state/axis2_state/velocity` + + `//joint_state/axis2_state/acceleration` + + `//joint_state/axis2_state/effort` + +1. **link_state.sdf**: + + `//link_state/linear_velocity` + + `//link_state/angular_velocity` + + `//link_state/linear_acceleration` + + `//link_state/angular_acceleration` + + `//link_state/force` + + `//link_state/torque` + +1. **model.sdf**: + + `//model/model_state` + + `//model/include/model_state` + +1. **world.sdf**: + + `//world/include/model_state` + +### Modifications + +1. **state.sdf**, **model_state.sdf**, **joint_state.sdf**, **link_state.sdf**, + **light_state.sdf**: A `_state` suffix has been added to state element names + to match the `.sdf` file names and for consistency. + + `//state/light` renamed to `//state/light_state` + + `//state/model` renamed to `//state/model_state` + + `//state/model/joint` renamed to `//state/model_state/joint_state` + + `//state/model/light` renamed to `//state/model_state/light_state` + + `//state/model/link` renamed to `//state/model_state/link_state` + + `//state/model/model` renamed to `//state/model_state/model_state` + + `//state/model/link/collision` renamed to `//state/model_state/link_state/collision_state` + +1. **state.sdf**: `//state/joint_state` has been added to represent the state of a + `//world/joint` and `//state/insertions/joint` can represent inserted + `//world/joint` elements. + +### Deprecations + +1. **joint_state.sdf**: + + `//joint_state/angle` is deprecated in favor of `//joint_state/axis_state/position` + and `//joint_state/axis2_state/position`. + +1. **link_state.sdf**: + + `//link_state/velocity` is deprecated in favor of `//link_state/angular_velocity` + and `//link_state/linear_velocity`. + + `//link_state/acceleration` is deprecated in favor of `//link_state/angular_acceleration` + and `//link_state/linear_acceleration`. + + `//link_state/wrench` is deprecated in favor of `//link_state/torque` + and `//link_state/force`. + ## SDFormat specification 1.10 to 1.11 ### Additions diff --git a/README.md b/README.md index 373f69fee..0b6dfc324 100644 --- a/README.md +++ b/README.md @@ -10,10 +10,10 @@ Note: The branch name in the codecov URL & library version should be updated whe --> Build | Status -- | -- -Test coverage | [![codecov](https://codecov.io/gh/gazebosim/sdformat/tree/main/graph/badge.svg)](https://codecov.io/gh/gazebosim/sdformat/tree/main) -Ubuntu Jammy | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-main-jammy-amd64)](https://build.osrfoundation.org/job/sdformat-ci-main-jammy-amd64) -Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-main-homebrew-amd64)](https://build.osrfoundation.org/job/sdformat-ci-main-homebrew-amd64) -Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-main-win)](https://build.osrfoundation.org/job/sdformat-main-win) +Test coverage | [![codecov](https://codecov.io/gh/gazebosim/sdformat/tree/sdf15/graph/badge.svg)](https://codecov.io/gh/gazebosim/sdformat/tree/sdf15) +Ubuntu Jammy | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-sdf15-noble-amd64)](https://build.osrfoundation.org/job/sdformat-ci-sdf15-noble-amd64) +Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-sdf15-homebrew-amd64)](https://build.osrfoundation.org/job/sdformat-ci-sdf15-homebrew-amd64) +Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-sdf15-win)](https://build.osrfoundation.org/job/sdformat-sdf15-win) SDFormat is an XML file format that describes environments, objects, and robots @@ -66,7 +66,7 @@ sudo apt-get update sudo apt install libsdformat<#>-dev libsdformat<#> ``` -Be sure to replace `<#>` with a number value, such as 2 or 3, depending on +Be sure to replace `<#>` with a number value, such as 14 or 15, depending on which version you need, or leave it empty for version 1. ### macOS @@ -82,7 +82,7 @@ Install sdformat: brew install sdformat<#> ``` -Be sure to replace `<#>` with a number value, such as 1 or 2, depending on +Be sure to replace `<#>` with a number value, such as 14 or 15, depending on which version you need. ### Windows @@ -126,7 +126,7 @@ Clone the repository ```sh git clone https://github.com/gazebosim/sdformat -b sdf<#> ``` -Be sure to replace `<#>` with a number value, such as 1 or 2, depending on +Be sure to replace `<#>` with a number value, such as 14 or 15, depending on which version you need. ### Build from Source @@ -168,7 +168,7 @@ Clone the repository ```sh git clone https://github.com/gazebosim/sdformat -b sdf<#> ``` -Be sure to replace `<#>` with a number value, such as 1 or 2, depending on +Be sure to replace `<#>` with a number value, such as 14 or 15, depending on which version you need. Install dependencies @@ -213,34 +213,37 @@ conda activate gz-ws Install prerequisites: ``` -conda install tinyxml2 urdfdom --channel conda-forge +conda install cmake git vcstool colcon-common-extensions ^ +tinyxml2 urdfdom pybind11 -channel conda-forge ``` -Install Gazebo dependencies: +### Getting the sources and building from Source -You can view lists of dependencies: -``` -conda search libsdformat --channel conda-forge --info -``` +Be sure to replace `<#>` with a number value, such as 14 or 15, depending on +which version you need. + +1. Getting the sources -Install dependencies, replacing `<#>` with the desired versions: ``` -conda install libgz-cmake<#> libgz-math<#> libgz-tools<#> libgz-utils<#> --channel conda-forge +mkdir ws\src +cd ws +vcs import src --input https://raw.githubusercontent.com/gazebo-tooling/gazebodistro/master/sdformat<#>.yaml ``` -### Build from Source +2. Build from source -This assumes you have created and activated a Conda environment while installing the Prerequisites. +Note: the Gazebo library dependencies are going to be compiled from source +with sdformat although it should be possible to install them from +conda-forge on stable Gazebo releases using the standard conda install +command. -1. Configure and build - ``` - mkdir build - cd build - cmake .. -DBUILD_TESTING=OFF # Optionally, -DCMAKE_INSTALL_PREFIX=path\to\install - cmake --build . --config Release - ``` +Build the gazebo libraries needed as dependencies (skip testing to speed up the compilation) +using the [colcon](https://colcon.readthedocs.io/en/released/) tool: +``` + colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install --packages-skip sdformat<#> +``` -2. Install - ``` - cmake --install . --config Release - ``` +Build sdformat with its test suite: +``` + colcon build --cmake-args -DBUILD_TESTING=ON --merge-install --packages-select sdformat<#> +``` diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 66d4caa6a..46a775b84 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -13,9 +13,6 @@ if (DOXYGEN_FOUND) ${CMAKE_BINARY_DIR}/doxygen/html COMMAND cp ${CMAKE_SOURCE_DIR}/doc/search.js ${CMAKE_BINARY_DIR}/doxygen/html/search - COMMAND make -C ${CMAKE_BINARY_DIR}/doxygen/latex - COMMAND mv ${CMAKE_BINARY_DIR}/doxygen/latex/refman.pdf - ${CMAKE_BINARY_DIR}/doxygen/latex/sdf-${PROJECT_VERSION_FULL}.pdf COMMENT "Generating API documentation with Doxygen" VERBATIM) endif() diff --git a/doc/sdf.in b/doc/sdf.in index 9865bc35b..30298aef3 100644 --- a/doc/sdf.in +++ b/doc/sdf.in @@ -1234,7 +1234,7 @@ SERVER_BASED_SEARCH = NO # If the GENERATE_LATEX tag is set to YES (the default) Doxygen will # generate Latex output. -GENERATE_LATEX = YES +GENERATE_LATEX = NO # The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. # If a relative path is entered the value of OUTPUT_DIRECTORY will be @@ -1595,7 +1595,7 @@ PERL_PATH = /usr/bin/perl # this option also works with HAVE_DOT disabled, but it is recommended to # install and use dot, since it yields more powerful graphs. -CLASS_DIAGRAMS = YES +CLASS_DIAGRAMS = NO # You can define message sequence charts within doxygen comments using the \msc # command. Doxygen will then run the mscgen tool (see @@ -1617,7 +1617,7 @@ HIDE_UNDOC_RELATIONS = YES # toolkit from AT&T and Lucent Bell Labs. The other options in this section # have no effect if this option is set to NO (the default) -HAVE_DOT = YES +HAVE_DOT = NO # The DOT_NUM_THREADS specifies the number of dot invocations doxygen is # allowed to run in parallel. When set to 0 (the default) doxygen will diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index 5c46827c8..bb564c057 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -363,12 +363,12 @@ namespace sdf /// The Camera sensor assumes that the color and depth images are captured /// at the same frame_id. /// \return The name of the frame this camera uses in its camera_info topic. - public: const std::string OpticalFrameId() const; + public: const std::string GZ_DEPRECATED(15) OpticalFrameId() const; /// \brief Set the name of the coordinate frame relative to which this /// object's camera_info is expressed. /// \param[in] _frame The frame this camera uses in its camera_info topic. - public: void SetOpticalFrameId(const std::string &_frame); + public: void GZ_DEPRECATED(15) SetOpticalFrameId(const std::string &_frame); /// \brief Get the lens type. This is the type of the lens mapping. /// Supported values are gnomonical, stereographic, equidistant, diff --git a/include/sdf/Error.hh b/include/sdf/Error.hh index 0aace5ef7..9664cc850 100644 --- a/include/sdf/Error.hh +++ b/include/sdf/Error.hh @@ -183,6 +183,9 @@ namespace sdf /// \brief The joint axis mimic does not refer to a valid joint in the /// current scope. JOINT_AXIS_MIMIC_INVALID, + + /// \brief Error at the XML level. + XML_ERROR, }; class SDFORMAT_VISIBLE Error diff --git a/include/sdf/InstallationDirectories.hh b/include/sdf/InstallationDirectories.hh new file mode 100644 index 000000000..be3b0c678 --- /dev/null +++ b/include/sdf/InstallationDirectories.hh @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2024 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_INSTALLATION_DIRECTORIES_HH_ +#define SDF_INSTALLATION_DIRECTORIES_HH_ + +#include + +#include +#include + +namespace sdf +{ + inline namespace SDF_VERSION_NAMESPACE { + + /// \brief getInstallPrefix return the install prefix of the library + /// i.e. CMAKE_INSTALL_PREFIX unless the library has been moved + SDFORMAT_VISIBLE std::string getInstallPrefix(); + + /// \brief getSharePath return the share directory used by sdformat + SDFORMAT_VISIBLE std::string getSharePath(); + + } +} + +#endif diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index 26c4b226f..596e0a398 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -129,26 +129,6 @@ namespace sdf /// \param[in] _name Name of the child frame. public: void SetChildName(const std::string &_name); - /// \brief Get the name of this joint's parent link. - /// \return The name of the parent link. - /// \deprecated Use ParentName. - public: GZ_DEPRECATED(13) const std::string &ParentLinkName() const; - - /// \brief Set the name of the parent link. - /// \param[in] _name Name of the parent link. - /// \deprecated Use SetParentName. - public: GZ_DEPRECATED(13) void SetParentLinkName(const std::string &_name); - - /// \brief Get the name of this joint's child link. - /// \return The name of the child link. - /// \deprecated Use ChildName. - public: GZ_DEPRECATED(13) const std::string &ChildLinkName() const; - - /// \brief Set the name of the child link. - /// \param[in] _name Name of the child link. - /// \deprecated Use SetChildName. - public: GZ_DEPRECATED(13) void SetChildLinkName(const std::string &_name); - /// \brief Resolve the name of the child link from the /// FrameAttachedToGraph. /// \param[out] _body Name of child link of this joint. diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 3f433598d..dfd02f69c 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -408,12 +408,22 @@ namespace sdf /// \sa bool Model::EnableWind public: bool EnableWind() const; + /// \brief Check if this link should be subject to gravity. + /// If true, this link should be affected by gravity. + /// \return true if the model should be subject to gravity, false otherwise. + public: bool EnableGravity() const; + /// \brief Set whether this link should be subject to wind. /// \param[in] _enableWind True or false depending on whether the link /// should be subject to wind. /// \sa Model::SetEnableWind(bool) public: void SetEnableWind(bool _enableWind); + /// \brief Set whether this link should be subject to gravity. + /// \param[in] _enableGravity True or false depending on whether the link + /// should be subject to gravity. + public: void SetEnableGravity(bool _enableGravity); + /// \brief Check if this link is kinematic only /// \return true if the link is kinematic only, false otherwise. public: bool Kinematic() const; diff --git a/include/sdf/ParticleEmitter.hh b/include/sdf/ParticleEmitter.hh index c2e09dec5..0326f34c2 100644 --- a/include/sdf/ParticleEmitter.hh +++ b/include/sdf/ParticleEmitter.hh @@ -51,10 +51,7 @@ namespace sdf CYLINDER = 2, /// \brief An ellipsoid emitter. - ELLIPSOID = 3, - - /// \brief An cone emitter. - CONE = 4, + ELLIPSOID = 3 }; /// \brief A description of a particle emitter, which can be attached diff --git a/include/sdf/SDFImpl.hh b/include/sdf/SDFImpl.hh index 20e1ec19f..fb7e96ea0 100644 --- a/include/sdf/SDFImpl.hh +++ b/include/sdf/SDFImpl.hh @@ -80,6 +80,34 @@ namespace sdf bool _searchLocalPath = true, bool _useCallback = false); + /// \brief Find the absolute path of a file. + /// + /// The search order in the function is as follows: + /// 1. Using the global URI path map, search in paths associated with the URI + /// scheme of the input. + /// 2. Seach in the path defined by the macro `SDF_SHARE_PATH`. + /// 3. Search in the the libsdformat install path. The path is formed by + /// has the pattern `SDF_SHARE_PATH/sdformat//` + /// 4. Directly check if the input path exists in the filesystem. + /// 5. Seach in the path defined by the environment variable `SDF_PATH`. + /// 6. If enabled via _searchLocalPath, prepend the input with the current + /// working directory and check if the result path exists. + /// 7. If enabled via _useCallback and the global callback function is set, + /// invoke the function and return its result. + /// + /// \param[out] _errors Vector of errors. + /// \param[in] _filename Name of the file to find. + /// \param[in] _searchLocalPath True to search for the file in the current + /// working directory. + /// \param[in] _useCallback True to find a file based on a registered + /// callback if the file is not found via the normal mechanism. + /// \return File's full path. + SDFORMAT_VISIBLE + std::string findFile(sdf::Errors &_errors, + const std::string &_filename, + bool _searchLocalPath = true, + bool _useCallback = false); + /// \brief Find the absolute path of a file. /// /// This overload uses the URI path map and and the callback function @@ -99,6 +127,26 @@ namespace sdf bool _useCallback, const ParserConfig &_config); + /// \brief Find the absolute path of a file. + /// + /// This overload uses the URI path map and and the callback function + /// configured in the input ParserConfig object instead of their global + /// counterparts. + /// + /// \param[out] _errors Vector of errors. + /// \param[in] _filename Name of the file to find. + /// \param[in] _searchLocalPath True to search for the file in the current + /// working directory. + /// \param[in] _useCallback True to find a file based on a registered + /// callback if the file is not found via the normal mechanism. + /// \param[in] _config Custom parser configuration. + /// \return File's full path. + SDFORMAT_VISIBLE + std::string findFile(sdf::Errors &_errors, + const std::string &_filename, + bool _searchLocalPath, + bool _useCallback, + const ParserConfig &_config); /// \brief Associate paths to a URI. /// Example paramters: "model://", "/usr/share/models:~/.gazebo/models" @@ -121,22 +169,45 @@ namespace sdf /// \brief Destructor public: ~SDF(); public: void PrintDescription(); + public: void PrintDescription(sdf::Errors &_errors); public: void PrintDoc(); public: void Write(const std::string &_filename); + public: void Write(sdf::Errors &_errors, const std::string &_filename); /// \brief Output SDF's values to stdout. /// \param[in] _config Configuration for printing the values. public: void PrintValues(const PrintConfig &_config = PrintConfig()); + /// \brief Output SDF's values to stdout. + /// \param[out] _errors Vector of errrors. + /// \param[in] _config Configuration for printing the values. + public: void PrintValues(sdf::Errors &_errors, + const PrintConfig &_config = PrintConfig()); + /// \brief Convert the SDF values to a string representation. /// \param[in] _config Configuration for printing the values. /// \return The string representation. public: std::string ToString( const PrintConfig &_config = PrintConfig()) const; + /// \brief Convert the SDF values to a string representation. + /// \param[out] _errors Vector of errors. + /// \param[in] _config Configuration for printing the values. + /// \return The string representation. + public: std::string ToString( + sdf::Errors &_errors, + const PrintConfig &_config = PrintConfig()) const; + /// \brief Set SDF values from a string + /// \param[in] sdfData String with the values to load. public: void SetFromString(const std::string &_sdfData); + /// \brief Set SDF values from a string + /// \param[out] _errors Vector of errors. + /// \param[in] sdfData String with the values to load. + public: void SetFromString(sdf::Errors &_Errors, + const std::string &_sdfData); + /// \brief Clear the data in this object. public: void Clear(); @@ -148,11 +219,6 @@ namespace sdf /// \param[in] _root Root element public: void SetRoot(const ElementPtr _root); - /// \brief Set the root pointer - /// \param[in] _root Root element - /// \deprecated Use SetRoot - public: GZ_DEPRECATED(13) void Root(const ElementPtr _root); - /// \brief Get the path to the SDF document on disk. /// \return The full path to the SDF document. public: std::string FilePath() const; @@ -182,6 +248,13 @@ namespace sdf /// \return a wrapped clone of the SDF element public: static ElementPtr WrapInRoot(const ElementPtr &_sdf); + /// \brief wraps the SDF element into a root element with the version info. + /// \param[out] _errors Vector of errors. + /// \param[in] _sdf the sdf element. Will be cloned by this function. + /// \return a wrapped clone of the SDF element + public: static ElementPtr WrapInRoot(sdf::Errors &_errors, + const ElementPtr &_sdf); + /// \brief Get a string representation of an SDF specification file. /// This function uses a built-in version of a .sdf file located in /// the sdf directory. The parser.cc code uses this function, which avoids @@ -198,6 +271,22 @@ namespace sdf public: static const std::string &EmbeddedSpec( const std::string &_filename, const bool _quiet); + /// \brief Get a string representation of an SDF specification file. + /// This function uses a built-in version of a .sdf file located in + /// the sdf directory. The parser.cc code uses this function, which avoids + /// touching the filesystem. + /// + /// Most people should not use this function. + /// + /// \param[out] _errors Vector of errors. + /// \param[in] _filename Base name of the SDF specification file to + /// load. For example "root.sdf" or "world.sdf". + /// \return A string that contains the contents of the specified + /// _filename. An empty string is returned if the _filename could not be + /// found. + public: static const std::string &EmbeddedSpec( + sdf::Errors &_errors, const std::string &_filename); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 7b62572ec..49bc336f9 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -160,6 +160,14 @@ namespace sdf /// \param[in] _name Name of the sensor. public: void SetName(const std::string &_name); + /// \brief Get the frame ID of the sensor. + /// \return Sensor frame ID. + public: std::string FrameId() const; + + /// \brief Set the frame ID of the sensor. + /// \param[in] _frameId Frame ID to set. + public: void SetFrameId(const std::string &_frameId); + /// \brief Get the topic on which sensor data should be published. /// \return Topic for this sensor's data. public: std::string Topic() const; diff --git a/include/sdf/Surface.hh b/include/sdf/Surface.hh index 02fd98be5..caec7d0cd 100644 --- a/include/sdf/Surface.hh +++ b/include/sdf/Surface.hh @@ -17,6 +17,7 @@ #ifndef SDF_SURFACE_HH_ #define SDF_SURFACE_HH_ +#include #include #include "sdf/Element.hh" #include "sdf/Types.hh" @@ -122,6 +123,132 @@ namespace sdf GZ_UTILS_IMPL_PTR(dataPtr) }; + /// \brief BulletFriction information for a friction. + class SDFORMAT_VISIBLE BulletFriction + { + /// \brief Default constructor + public: BulletFriction(); + + /// \brief Load BulletFriction 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 friction coefficient in first friction pyramid direction. + /// \returns Friction coefficient + public: double Friction() const; + + /// \brief Set friction coefficient in first friction pyramid direction. + /// \param[in] _fricton Friction coefficient + public: void SetFriction(double _friction); + + /// \brief Get the friction coefficient in second friction pyramid + /// direction. + /// \return Second friction coefficient + public: double Friction2() const; + + /// \brief Set friction coefficient in second friction pyramid direction. + /// \param[in] _fricton Friction coefficient + public: void SetFriction2(double _friction); + + /// \brief Get the first friction pyramid direction in collision-fixed + /// reference + /// \return First friction pyramid direction. + public: const gz::math::Vector3d &Fdir1() const; + + /// \brief Set the first friction pyramid direction in collision-fixed + /// reference + /// \param[in] _fdir First friction pyramid direction. + public: void SetFdir1(const gz::math::Vector3d &_fdir); + + /// \brief Get the rolling friction coefficient + /// \return Rolling friction coefficient + public: double RollingFriction() const; + + /// \brief Set the rolling friction coefficient + /// \param[in] _slip1 Rolling friction coefficient + public: void SetRollingFriction(double _friction); + + /// \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. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + + /// \brief Torsional friction + class SDFORMAT_VISIBLE Torsional + { + /// \brief Default constructor + public: Torsional(); + + /// \brief Load torsional 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 torsional friction coefficient. + /// \return Torsional friction coefficient + public: double Coefficient() const; + + /// \brief Set the torsional friction coefficient. + /// \param[in] _fricton Torsional friction coefficient + public: void SetCoefficient(double _coefficient); + + /// \brief Get whether the patch radius is used to calculate torsional + /// friction. + /// \return True if patch radius is used. + public: bool UsePatchRadius() const; + + /// \brief Set whether to use patch radius for torsional friction + /// calculation. + /// \param[in] _usePatchRadius True to use patch radius. + /// False to use surface radius. + public: void SetUsePatchRadius(bool _usePatchRadius); + + /// \brief Get the radius of contact patch surface. + /// \return Patch radius + public: double PatchRadius() const; + + /// \brief Set the radius of contact patch surface. + /// \param[in] _radius Patch radius + public: void SetPatchRadius(double _radius); + + /// \brief Get the surface radius on the contact point + /// \return Surface radius + public: double SurfaceRadius() const; + + /// \brief Set the surface radius on the contact point. + /// \param[in] _radius Surface radius + public: void SetSurfaceRadius(double _radius); + + /// \brief Get the ODE force dependent slip for torsional friction + /// \return Force dependent slip for torsional friction. + public: double ODESlip() const; + + /// \brief Set the ODE force dependent slip for torsional friction + /// \param[in] _slip Force dependent slip for torsional friction. + public: void SetODESlip(double _slip); + + /// \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. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + /// \brief Friction information for a surface. class SDFORMAT_VISIBLE Friction { @@ -145,6 +272,24 @@ namespace sdf /// \param[in] _ode The ODE object. public: void SetODE(const sdf::ODE &_ode); + /// \brief Get the associated BulletFriction object + /// \return Pointer to the associated BulletFriction object, + /// nullptr if the Surface doesn't contain a BulletFriction element. + public: const sdf::BulletFriction *BulletFriction() const; + + /// \brief Set the associated BulletFriction object. + /// \param[in] _bullet The BulletFriction object. + public: void SetBulletFriction(const sdf::BulletFriction &_bullet); + + /// \brief Get the torsional friction + /// \return Pointer to the torsional friction + /// nullptr if the Surface doesn't contain a torsional friction element. + public: const sdf::Torsional *Torsional() const; + + /// \brief Set the torsional friction + /// \param[in] _torsional The torsional friction. + public: void SetTorsional(const sdf::Torsional &_torsional); + /// \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 diff --git a/include/sdf/Types.hh b/include/sdf/Types.hh index f2f19248c..26e25cdac 100644 --- a/include/sdf/Types.hh +++ b/include/sdf/Types.hh @@ -36,22 +36,8 @@ namespace sdf inline namespace SDF_VERSION_NAMESPACE { // - namespace internal - { - /// \brief Initializes the scope delimiter as a function-local static - /// variable so it can be used to initialize kSdfScopeDelimiter. - /// \note This should not be used directly in user code. It will likely be - /// removed in libsdformat 15 with kSdfScopeDelimiter. - SDFORMAT_VISIBLE const std::string &SdfScopeDelimiter(); - } // namespace internal - constexpr std::string_view kScopeDelimiter{"::"}; - // Deprecated because it violates the Google Style Guide as it is not - // trivially destructible. Please use sdf::kScopeDelimiter instead. - GZ_DEPRECATED(14) - inline const std::string &kSdfScopeDelimiter = internal::SdfScopeDelimiter(); - /// \brief The source path replacement if it was parsed from a string, /// instead of a file. constexpr char kSdfStringSource[] = ""; @@ -158,12 +144,6 @@ namespace sdf public: int32_t nsec; }; - /// \brief A class for inertial information about a link. - class SDFORMAT_VISIBLE GZ_DEPRECATED(13) Inertia - { - public: double mass; - }; - /// \brief Transforms a string to its lowercase equivalent /// \param[in] _in String to convert to lowercase /// \return Lowercase equilvalent of _in. diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 7e449abab..757f4f45d 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -278,6 +278,20 @@ namespace sdf /// \return True if there exists an actor with the given name. public: bool ActorNameExists(const std::string &_name) const; + /// \brief Get an actor based on a name. + /// \param[in] _name Name of the actor. + /// \return Pointer to the actor. Nullptr if an actor with the given name + /// does not exist. + /// \sa bool ActorNameExists(const std::string &_name) const + public: const Actor *ActorByName(const std::string &_name) const; + + /// \brief Get a mutable actor based on a name. + /// \param[in] _name Name of the actor. + /// \return Pointer to the actor. Nullptr if an actor with the given name + /// does not exist. + /// \sa bool ActorNameExists(const std::string &_name) const + public: Actor *ActorByName(const std::string &_name); + /// \brief Get the number of explicit frames that are immediate (not nested) /// children of this World object. /// \remark FrameByName() can find explicit frames that are not immediate diff --git a/include/sdf/config.hh.in b/include/sdf/config.hh.in index 6a32cb8bc..456eb12cc 100644 --- a/include/sdf/config.hh.in +++ b/include/sdf/config.hh.in @@ -48,11 +48,11 @@ #cmakedefine SDFORMAT_DISABLE_CONSOLE_LOGFILE 1 #ifndef SDF_SHARE_PATH -#define SDF_SHARE_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/" +#define SDF_SHARE_PATH _Pragma ("GCC warning \"'SDF_SHARE_PATH' macro is deprecated, use sdf::getSharePath() function instead. \"") "${CMAKE_INSTALL_FULL_DATAROOTDIR}/" #endif #ifndef SDF_VERSION_PATH -#define SDF_VERSION_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/${PROJECT_VERSION}" +#define SDF_VERSION_PATH _Pragma ("GCC warning \"'SDF_VERSION_PATH' macro is deprecated and should not be used. \"") "${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/${PROJECT_VERSION}" #endif #endif // #ifndef SDF_CONFIG_HH_ diff --git a/include/sdf/parser.hh b/include/sdf/parser.hh index a7dee740a..a96b484e3 100644 --- a/include/sdf/parser.hh +++ b/include/sdf/parser.hh @@ -503,17 +503,6 @@ namespace sdf SDFORMAT_VISIBLE bool checkJointParentChildNames(const sdf::Root *_root); - /// \brief Check that all joints in contained models specify parent - /// and child link names that match the names of sibling links. - /// This checks recursively and should check the files exhaustively - /// rather than terminating early when the first error is found. - /// \param[in] _root SDF Root object to check recursively. - /// \return True if all models have joints with valid parent and child - /// link names. - /// \deprecated Use checkJointParentChildNames. - SDFORMAT_VISIBLE GZ_DEPRECATED(13) - bool checkJointParentChildLinkNames(const sdf::Root *_root); - /// \brief Check that all joints in contained models specify parent /// and child names that match the names of sibling links, joints, /// models, or frames. diff --git a/python/src/sdf/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc index 6103541d1..80892100e 100644 --- a/python/src/sdf/_gz_sdformat_pybind11.cc +++ b/python/src/sdf/_gz_sdformat_pybind11.cc @@ -84,6 +84,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { sdf::python::defineAltimeter(m); sdf::python::defineAtmosphere(m); sdf::python::defineBox(m); + sdf::python::defineBulletFriction(m); sdf::python::defineCamera(m); sdf::python::defineCapsule(m); sdf::python::defineCollision(m); @@ -136,6 +137,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { sdf::python::defineSky(m); sdf::python::defineSphere(m); sdf::python::defineSurface(m); + sdf::python::defineTorsional(m); sdf::python::defineVisual(m); sdf::python::defineWorld(m); diff --git a/python/src/sdf/pyBox.cc b/python/src/sdf/pyBox.cc index 31d9d8f08..453438119 100644 --- a/python/src/sdf/pyBox.cc +++ b/python/src/sdf/pyBox.cc @@ -16,6 +16,7 @@ #include "pyBox.hh" #include +#include #include "sdf/Box.hh" @@ -42,6 +43,8 @@ void defineBox(pybind11::object module) pybind11::overload_cast<>(&sdf::Box::Shape, pybind11::const_), pybind11::return_value_policy::reference, "Get a mutable Gazebo Math representation of this Box.") + .def("calculate_inertial", &sdf::Box::CalculateInertial, + "Calculate and return the Inertial values for the Box.") .def("__copy__", [](const sdf::Box &self) { return sdf::Box(self); }) diff --git a/python/src/sdf/pyCamera.cc b/python/src/sdf/pyCamera.cc index e91683d5e..11c78f97b 100644 --- a/python/src/sdf/pyCamera.cc +++ b/python/src/sdf/pyCamera.cc @@ -32,6 +32,9 @@ namespace python ///////////////////////////////////////////////// void defineCamera(pybind11::object module) { + // \todo(iche033) OpticalFrameId and SetOpticalFrameId are deprecated + // Remove sdformat16 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION pybind11::class_ cameraModule(module, "Camera"); cameraModule .def(pybind11::init<>()) @@ -296,6 +299,8 @@ void defineCamera(pybind11::object module) .value("BAYER_BGGR8", sdf::PixelFormatType::BAYER_BGGR8) .value("BAYER_GBRG8", sdf::PixelFormatType::BAYER_GBRG8) .value("BAYER_GRBG8", sdf::PixelFormatType::BAYER_GRBG8); + + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } } // namespace python } // namespace SDF_VERSION_NAMESPACE diff --git a/python/src/sdf/pyCapsule.cc b/python/src/sdf/pyCapsule.cc index ac25cd917..50ca4337d 100644 --- a/python/src/sdf/pyCapsule.cc +++ b/python/src/sdf/pyCapsule.cc @@ -16,6 +16,7 @@ #include "pyCapsule.hh" #include +#include #include "sdf/Capsule.hh" @@ -41,6 +42,8 @@ void defineCapsule(pybind11::object module) "Get the capsule's length in meters.") .def("set_length", &sdf::Capsule::SetLength, "Set the capsule's length in meters.") + .def("calculate_inertial", &sdf::Capsule::CalculateInertial, + "Calculate and return the Inertial values for the Capsule.") .def( "shape", pybind11::overload_cast<>(&sdf::Capsule::Shape, pybind11::const_), diff --git a/python/src/sdf/pyCollision.cc b/python/src/sdf/pyCollision.cc index c7305063e..d01f6fba3 100644 --- a/python/src/sdf/pyCollision.cc +++ b/python/src/sdf/pyCollision.cc @@ -17,6 +17,7 @@ #include "pyCollision.hh" #include +#include #include "sdf/Collision.hh" #include "sdf/Geometry.hh" @@ -37,6 +38,12 @@ void defineCollision(pybind11::object module) geometryModule .def(pybind11::init<>()) .def(pybind11::init()) + .def("set_density", &sdf::Collision::SetDensity, "Set the density of the collision.") + .def("density", &sdf::Collision::Density, "Get the density of the collision.") + .def("calculate_inertial", + pybind11::overload_cast( + &sdf::Collision::CalculateInertial), + "Calculate and return the MassMatrix for the collision.") .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.") diff --git a/python/src/sdf/pyCylinder.cc b/python/src/sdf/pyCylinder.cc index 83af7dda4..d8515ddc6 100644 --- a/python/src/sdf/pyCylinder.cc +++ b/python/src/sdf/pyCylinder.cc @@ -16,6 +16,7 @@ #include "pyCylinder.hh" #include +#include #include "sdf/Cylinder.hh" @@ -33,6 +34,8 @@ void defineCylinder(pybind11::object module) pybind11::class_(module, "Cylinder") .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Cylinder::CalculateInertial, + "Calculate and return the Inertial values for the Cylinder.") .def("radius", &sdf::Cylinder::Radius, "Get the cylinder's radius in meters.") .def("set_radius", &sdf::Cylinder::SetRadius, diff --git a/python/src/sdf/pyEllipsoid.cc b/python/src/sdf/pyEllipsoid.cc index 89fdb1454..47ea60c96 100644 --- a/python/src/sdf/pyEllipsoid.cc +++ b/python/src/sdf/pyEllipsoid.cc @@ -16,6 +16,7 @@ #include "pyEllipsoid.hh" #include +#include #include "sdf/Ellipsoid.hh" @@ -33,6 +34,8 @@ void defineEllipsoid(pybind11::object module) pybind11::class_(module, "Ellipsoid") .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Ellipsoid::CalculateInertial, + "Calculate and return the Inertial values for the Ellipsoid.") .def("radii", &sdf::Ellipsoid::Radii, "Get the ellipsoid's radii in meters.") .def("set_radii", &sdf::Ellipsoid::SetRadii, diff --git a/python/src/sdf/pyGeometry.cc b/python/src/sdf/pyGeometry.cc index be54d3ac0..8d7873bc3 100644 --- a/python/src/sdf/pyGeometry.cc +++ b/python/src/sdf/pyGeometry.cc @@ -17,6 +17,7 @@ #include "pyGeometry.hh" #include +#include #include "sdf/ParserConfig.hh" @@ -26,6 +27,7 @@ #include "sdf/Cylinder.hh" #include "sdf/Ellipsoid.hh" #include "sdf/Geometry.hh" +#include "sdf/Heightmap.hh" #include "sdf/Mesh.hh" #include "sdf/Plane.hh" #include "sdf/Sphere.hh" @@ -46,6 +48,8 @@ void defineGeometry(pybind11::object module) geometryModule .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Geometry::CalculateInertial, + "Calculate and return the Mass Matrix values for the Geometry") .def("type", &sdf::Geometry::Type, "Get the type of geometry.") .def("set_type", &sdf::Geometry::SetType, @@ -98,6 +102,11 @@ void defineGeometry(pybind11::object module) "geometry is not a mesh.") .def("set_mesh_shape", &sdf::Geometry::SetMeshShape, "Set the mesh shape.") + .def("set_heightmap_shape", &sdf::Geometry::SetHeightmapShape, + "Set the heightmap shape.") + .def("heightmap_shape", &sdf::Geometry::HeightmapShape, + pybind11::return_value_policy::reference, + "Get the heightmap geometry.") .def("__copy__", [](const sdf::Geometry &self) { return sdf::Geometry(self); }) diff --git a/python/src/sdf/pyLink.cc b/python/src/sdf/pyLink.cc index 8fd3b6a33..0431cb437 100644 --- a/python/src/sdf/pyLink.cc +++ b/python/src/sdf/pyLink.cc @@ -41,6 +41,16 @@ void defineLink(pybind11::object module) pybind11::class_(module, "Link") .def(pybind11::init<>()) .def(pybind11::init()) + .def("resolve_auto_inertials", &sdf::Link::ResolveAutoInertials, + "Calculate & set inertial values for the link") + .def("auto_inertia", &sdf::Link::AutoInertia, + "Check if the automatic calculation for the link inertial is enabled or not.") + .def("set_auto_inertia", &sdf::Link::SetAutoInertia, + "Enable automatic inertial calculations by setting autoInertia to true") + .def("auto_inertia_saved", &sdf::Link::AutoInertiaSaved, + "Check if the inertial values for this link were saved.") + .def("set_auto_inertia_saved", &sdf::Link::SetAutoInertiaSaved, + "Set the autoInertiaSaved() values") .def("name", &sdf::Link::Name, "Get the name of the link.") .def("set_name", &sdf::Link::SetName, @@ -168,6 +178,13 @@ void defineLink(pybind11::object module) .def("set_kinematic", &sdf::Link::SetKinematic, "Set whether this link is kinematic only.") + .def("enable_gravity", + &sdf::Link::EnableGravity, + "Check if this link should be subject to gravity. " + "If true, this link should be affected by gravity.") + .def("set_enable_gravity", + &sdf::Link::SetEnableGravity, + "Set whether this link should be subject to gravity.") .def("add_collision", &sdf::Link::AddCollision, "Add a collision to the link.") diff --git a/python/src/sdf/pyModel.cc b/python/src/sdf/pyModel.cc index fce94d0b9..1127945de 100644 --- a/python/src/sdf/pyModel.cc +++ b/python/src/sdf/pyModel.cc @@ -46,6 +46,8 @@ void defineModel(pybind11::object module) }, "Check that the FrameAttachedToGraph and PoseRelativeToGraph " "are valid.") + .def("resolve_auto_inertials", &sdf::Model::ResolveAutoInertials, + "Calculate and set the inertials for all the links belonging to the model object") .def("name", &sdf::Model::Name, "Get the name of model.") .def("set_name", &sdf::Model::SetName, diff --git a/python/src/sdf/pyParserConfig.cc b/python/src/sdf/pyParserConfig.cc index 73d8b1990..2e58422fe 100644 --- a/python/src/sdf/pyParserConfig.cc +++ b/python/src/sdf/pyParserConfig.cc @@ -46,6 +46,12 @@ void defineParserConfig(pybind11::object module) .def("find_file_callback", &sdf::ParserConfig::FindFileCallback, "Get the find file callback function") + .def("calculate_inertial_configuration", + &sdf::ParserConfig::CalculateInertialConfiguration, + "Get the current configuration for the CalculateInertial() function") + .def("set_calculate_inertial_configuration", + &sdf::ParserConfig::SetCalculateInertialConfiguration, + "Set the configuration for the CalculateInertial() function") .def("set_find_callback", &sdf::ParserConfig::SetFindCallback, "Set the callback to use when libsdformat can't find a file.") @@ -96,6 +102,10 @@ void defineParserConfig(pybind11::object module) .value("WARN", sdf::EnforcementPolicy::WARN) .value("LOG", sdf::EnforcementPolicy::LOG); + pybind11::enum_( + module, "ConfigureResolveAutoInertials") + .value("SKIP_CALCULATION_IN_LOAD", sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD) + .value("SAVE_CALCULATION", sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION); } } // namespace python } // namespace SDF_VERSION_NAMESPACE diff --git a/python/src/sdf/pyParticleEmitter.cc b/python/src/sdf/pyParticleEmitter.cc index 42d70cd43..0e9873c57 100644 --- a/python/src/sdf/pyParticleEmitter.cc +++ b/python/src/sdf/pyParticleEmitter.cc @@ -154,7 +154,6 @@ void defineParticleEmitter(pybind11::object module) pybind11::enum_(particleEmitterModule, "ParticleEmitterType") .value("POINT", sdf::ParticleEmitterType::POINT) .value("BOX", sdf::ParticleEmitterType::BOX) - .value("CONE", sdf::ParticleEmitterType::CONE) .value("CYLINDER", sdf::ParticleEmitterType::CYLINDER) .value("ELLIPSOID", sdf::ParticleEmitterType::ELLIPSOID); } diff --git a/python/src/sdf/pyRoot.cc b/python/src/sdf/pyRoot.cc index 42c21907a..2da83830a 100644 --- a/python/src/sdf/pyRoot.cc +++ b/python/src/sdf/pyRoot.cc @@ -38,6 +38,8 @@ void defineRoot(pybind11::object module) { pybind11::class_(module, "Root") .def(pybind11::init<>()) + .def("resolve_auto_inertials", &sdf::Root::ResolveAutoInertials, + "Calculate and set the inertial properties") .def("load", [](Root &self, const std::string &_filename) { @@ -46,7 +48,7 @@ void defineRoot(pybind11::object module) "Parse the given SDF file, and generate objects based on types " "specified in the SDF file.") .def("load", - [](Root &self, const std::string &_filename, + [](Root &self, const std::string &_filename, const ParserConfig &_config) { ThrowIfErrors(self.Load(_filename, _config)); diff --git a/python/src/sdf/pySensor.cc b/python/src/sdf/pySensor.cc index 063830def..db788aa5c 100644 --- a/python/src/sdf/pySensor.cc +++ b/python/src/sdf/pySensor.cc @@ -51,6 +51,10 @@ void defineSensor(pybind11::object module) "Get the name of the sensor.") .def("set_name", &sdf::Sensor::SetName, "Set the name of the sensor.") + .def("frame_id", &sdf::Sensor::FrameId, + "Get the frame id of the sensor.") + .def("set_frame_id", &sdf::Sensor::SetFrameId, + "Set the frame id of the sensor.") .def("topic", &sdf::Sensor::Topic, "Get the topic on which sensor data should be published.") .def("set_topic", &sdf::Sensor::SetTopic, diff --git a/python/src/sdf/pySphere.cc b/python/src/sdf/pySphere.cc index 06e991df7..ee6197b38 100644 --- a/python/src/sdf/pySphere.cc +++ b/python/src/sdf/pySphere.cc @@ -16,6 +16,7 @@ #include "pySphere.hh" #include +#include #include "sdf/Sphere.hh" @@ -33,6 +34,8 @@ void defineSphere(pybind11::object module) pybind11::class_(module, "Sphere") .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Sphere::CalculateInertial, + "Calculate and return the Inertial values for the Sphere.") .def("radius", &sdf::Sphere::Radius, "Get the sphere's radius in meters.") .def("set_radius", &sdf::Sphere::SetRadius, diff --git a/python/src/sdf/pySurface.cc b/python/src/sdf/pySurface.cc index dc7be622b..ed8545e95 100644 --- a/python/src/sdf/pySurface.cc +++ b/python/src/sdf/pySurface.cc @@ -55,6 +55,16 @@ void defineFriction(pybind11::object module) "Get the ODE object.") .def("set_ode", &sdf::Friction::SetODE, "Set the ODE object.") + .def("bullet_friction", &sdf::Friction::BulletFriction, + pybind11::return_value_policy::reference_internal, + "Get the bullet friction object.") + .def("set_bullet_friction", &sdf::Friction::SetBulletFriction, + "Set the bullet friction object.") + .def("torsional", &sdf::Friction::Torsional, + pybind11::return_value_policy::reference_internal, + "Get the torsional friction object.") + .def("set_torsional", &sdf::Friction::SetTorsional, + "Set the torsional friction object.") .def("__copy__", [](const sdf::Friction &self) { return sdf::Friction(self); }) @@ -118,6 +128,68 @@ void defineSurface(pybind11::object module) return sdf::Surface(self); }, "memo"_a); } +///////////////////////////////////////////////// +void defineBulletFriction(pybind11::object module) +{ + pybind11::class_(module, "BulletFriction") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("friction", &sdf::BulletFriction::Friction, + "Get the friction parameter.") + .def("set_friction", &sdf::BulletFriction::SetFriction, + "Set the friction parameter.") + .def("friction2", &sdf::BulletFriction::Friction2, + "Get the friction parameter.") + .def("set_friction2", &sdf::BulletFriction::SetFriction2, + "Set the friction2 parameter.") + .def("fdir1", &sdf::BulletFriction::Fdir1, + "Get the fdir1 parameter.") + .def("set_fdir1", &sdf::BulletFriction::SetFdir1, + "Set the fdir1 parameter.") + .def("rolling_friction", &sdf::BulletFriction::RollingFriction, + "Get the rolling fricion parameter.") + .def("set_rolling_friction", &sdf::BulletFriction::SetRollingFriction, + "Set the rolling friction parameter.") + .def("__copy__", [](const sdf::BulletFriction &self) { + return sdf::BulletFriction(self); + }) + .def("__deepcopy__", [](const sdf::BulletFriction &self, pybind11::dict) { + return sdf::BulletFriction(self); + }, "memo"_a); +} +///////////////////////////////////////////////// +void defineTorsional(pybind11::object module) +{ + pybind11::class_(module, "Torsional") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("coefficient", &sdf::Torsional::Coefficient, + "Get the coefficient parameter.") + .def("set_coefficient", &sdf::Torsional::SetCoefficient, + "Set the coefficient parameter.") + .def("use_patch_radius", &sdf::Torsional::UsePatchRadius, + "Get whether to use patch radius for torsional friction calculation.") + .def("set_use_patch_radius", &sdf::Torsional::SetUsePatchRadius, + "Set whether to use patch radius for torsional friction calculation.") + .def("patch_radius", &sdf::Torsional::PatchRadius, + "Get the radius of contact patch surface.") + .def("set_patch_radius", &sdf::Torsional::SetPatchRadius, + "Set the radius of contact patch surface.") + .def("surface_radius", &sdf::Torsional::SurfaceRadius, + "Get the surface radius on the contact point.") + .def("set_surface_radius", &sdf::Torsional::SetSurfaceRadius, + "Set the surface radius on the contact point.") + .def("ode_slip", &sdf::Torsional::ODESlip, + "Get the ODE force dependent slip for torsional friction.") + .def("set_ode_slip", &sdf::Torsional::SetODESlip, + "Set the ODE force dependent slip for torsional friction.") + .def("__copy__", [](const sdf::Torsional &self) { + return sdf::Torsional(self); + }) + .def("__deepcopy__", [](const sdf::Torsional &self, pybind11::dict) { + return sdf::Torsional(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 index 75e29f691..b45b547a2 100644 --- a/python/src/sdf/pySurface.hh +++ b/python/src/sdf/pySurface.hh @@ -52,6 +52,18 @@ void defineODE(pybind11::object module); * \param[in] module a pybind11 module to add the definition to */ void defineSurface(pybind11::object module); + +/// Define a pybind11 wrapper for an sdf::BulletFriction +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineBulletFriction(pybind11::object module); + +/// Define a pybind11 wrapper for an sdf::Torsional +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineTorsional(pybind11::object module); } // namespace python } // namespace SDF_VERSION_NAMESPACE } // namespace sdf diff --git a/python/src/sdf/pyWorld.cc b/python/src/sdf/pyWorld.cc index 41b930fc4..5eb400630 100644 --- a/python/src/sdf/pyWorld.cc +++ b/python/src/sdf/pyWorld.cc @@ -48,6 +48,8 @@ void defineWorld(pybind11::object module) geometryModule .def(pybind11::init<>()) .def(pybind11::init()) + .def("resolve_auto_inertials", &sdf::World::ResolveAutoInertials, + "Calculate and set the inertials for all the models in the world object") .def("validate_graphs", &sdf::World::ValidateGraphs, "Check that the FrameAttachedToGraph and PoseRelativeToGraph " "are valid.") @@ -98,13 +100,13 @@ void defineWorld(pybind11::object module) pybind11::overload_cast( &sdf::World::ModelByIndex), pybind11::return_value_policy::reference_internal, - "Get a mutable immediate (not nested) child joint model on an index.") + "Get a mutable immediate (not nested) model based on an index.") .def("model_by_name", pybind11::overload_cast( &sdf::World::ModelByName), pybind11::return_value_policy::reference_internal, - "Get a mutable immediate (not nested) mutable child model based on an " - "index.") + "Get a mutable immediate (not nested) mutable child model based on a " + "name.") .def("model_name_exists", &sdf::World::ModelNameExists, "Get whether a model name exists.") .def("name_exists_in_frame_attached_to_graph", @@ -137,6 +139,16 @@ void defineWorld(pybind11::object module) "Remove all joints.") .def("actor_count", &sdf::World::ActorCount, "Get the number of actors.") + .def("actor_by_index", + pybind11::overload_cast( + &sdf::World::ActorByIndex), + pybind11::return_value_policy::reference_internal, + "Get a mutable actor based on an index.") + .def("actor_by_name", + pybind11::overload_cast( + &sdf::World::ActorByName), + pybind11::return_value_policy::reference_internal, + "Get a mutable actor based on a name.") .def("frame_count", &sdf::World::FrameCount, "Get the number of explicit frames that are immediate (not nested) " "children of this World object.") diff --git a/python/test/pyBox_TEST.py b/python/test/pyBox_TEST.py index d98440de5..84ce40bee 100644 --- a/python/test/pyBox_TEST.py +++ b/python/test/pyBox_TEST.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz_test_deps.math import Vector3d +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Box import unittest @@ -33,7 +33,7 @@ def test_assignment(self): box = Box() box.set_size(size) - box2 = box; + box2 = box self.assertEqual(size, box2.size()) self.assertEqual(1 * 2 * 3, box2.shape().volume()) @@ -55,5 +55,39 @@ def test_shape(self): box.shape().set_size(Vector3d(1, 2, 3)) self.assertEqual(Vector3d(1, 2, 3), box.size()) + def test_calculate_inertial(self): + box = Box() + density = 2710 + + box.set_size(Vector3d(-1, 1, 0)) + invalidBoxInertial = box.calculate_inertial(density) + self.assertEqual(None, invalidBoxInertial) + + l = 2.0 + w = 2.0 + h = 2.0 + box.set_size(Vector3d(l, w, h)) + + expectedMass = box.shape().volume() * density + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + boxInertial = box.calculate_inertial(density) + self.assertEqual(box.shape().material().density(), density) + self.assertNotEqual(None, boxInertial) + self.assertEqual(expectedInertial, boxInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + boxInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + boxInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), boxInertial.pose()) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCamera_TEST.py b/python/test/pyCamera_TEST.py index 6daca4cbe..c2a33312e 100644 --- a/python/test/pyCamera_TEST.py +++ b/python/test/pyCamera_TEST.py @@ -191,7 +191,7 @@ def test_default_construction(self): cam.set_lens_intrinsics_cy(123) self.assertAlmostEqual(123, cam.lens_intrinsics_cy()) - self.assertAlmostEqual(1.0, cam.lens_intrinsics_skew()) + self.assertAlmostEqual(0.0, cam.lens_intrinsics_skew()) cam.set_lens_intrinsics_skew(2.3) self.assertAlmostEqual(2.3, cam.lens_intrinsics_skew()) diff --git a/python/test/pyCapsule_TEST.py b/python/test/pyCapsule_TEST.py index 52472ed91..976e53c7b 100644 --- a/python/test/pyCapsule_TEST.py +++ b/python/test/pyCapsule_TEST.py @@ -16,6 +16,7 @@ import math +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Capsule import unittest @@ -56,7 +57,7 @@ def test_move_construction(self): def test_copy_construction(self): - capsule = Capsule(); + capsule = Capsule() capsule.set_radius(0.2) capsule.set_length(3.0) @@ -65,29 +66,29 @@ def test_copy_construction(self): self.assertEqual(3.0, capsule2.length()) def test_copy_construction(self): - capsule = Capsule(); + capsule = Capsule() capsule.set_radius(0.2) capsule.set_length(3.0) - capsule2 = copy.deepcopy(capsule); + 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 = Capsule() capsule1.set_radius(0.2) capsule1.set_length(3.0) - capsule2 = Capsule(); + 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; + capsule1 = copy.deepcopy(capsule2) + capsule2 = tmp self.assertEqual(2, capsule1.radius()) self.assertEqual(30, capsule1.length()) @@ -97,7 +98,7 @@ def test_assignment_after_move(self): def test_shape(self): - capsule = Capsule(); + capsule = Capsule() self.assertEqual(0.5, capsule.radius()) self.assertEqual(1.0, capsule.length()) @@ -106,6 +107,43 @@ def test_shape(self): self.assertEqual(0.123, capsule.radius()) self.assertEqual(0.456, capsule.length()) + def test_calculate_inertial(self): + capsule = Capsule() + + # density of aluminium + density = 2710 + l = 2.0 + r = 0.1 + + capsule.set_length(l) + capsule.set_radius(r) + + expectedMass = capsule.shape().volume() * density + cylinderVolume = math.pi * r * r * l + sphereVolume = math.pi * 4. / 3. * r * r * r + volume = cylinderVolume + sphereVolume + cylinderMass = expectedMass * cylinderVolume / volume + sphereMass = expectedMass * sphereVolume / volume + + ixxIyy = (1 / 12.0) * cylinderMass * (3 * r * r + l * l) + sphereMass * ( + 0.4 * r * r + 0.375 * r * l + 0.25 * l * l) + izz = r*r * (0.5 * cylinderMass + 0.4 * sphereMass) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixxIyy, ixxIyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + capsuleInertial = capsule.calculate_inertial(density) + self.assertEqual(capsule.shape().material().density(), density) + self.assertNotEqual(None, capsuleInertial) + self.assertEqual(expectedInertial, capsuleInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + capsuleInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + capsuleInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), capsuleInertial.pose()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCollision_TEST.py b/python/test/pyCollision_TEST.py index b4275d038..7068c53d2 100644 --- a/python/test/pyCollision_TEST.py +++ b/python/test/pyCollision_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from gz_test_deps.math import Pose3d +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import (Box, Collision, Cone, Contact, Cylinder, Error, - Geometry, Plane, Surface, Sphere, + Geometry, ParserConfig, Plane, Root, Surface, Sphere, SDFErrorsException) import gz_test_deps.sdformat as sdf import unittest @@ -26,6 +26,7 @@ class CollisionTEST(unittest.TestCase): def test_default_construction(self): collision = Collision() self.assertTrue(not collision.name()) + self.assertEqual(collision.density(), 1000.0) collision.set_name("test_collison") self.assertEqual(collision.name(), "test_collison") @@ -40,6 +41,9 @@ def test_default_construction(self): with self.assertRaises(SDFErrorsException): semanticPose.resolve() + collision.set_density(1240.0) + self.assertAlmostEqual(collision.density(), 1240.0) + 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()) @@ -131,5 +135,137 @@ def test_set_surface(self): self.assertEqual(collision.surface().contact().collide_bitmask(), 0x2) + def test_incorrect_box_collision_calculate_inertial(self): + collision = Collision() + self.assertAlmostEqual(1000.0, collision.density()) + + # sdf::ElementPtr sdf(new sdf::Element()) + # collision.Load(sdf) + + collisionInertial = Inertiald() + sdfParserConfig = ParserConfig() + geom = Geometry() + box = Box() + + # Invalid Inertial test + box.set_size(Vector3d(-1, 1, 0)) + geom.set_type(sdf.GeometryType.BOX) + geom.set_box_shape(box) + collision.set_geometry(geom) + + errors = [] + + collision.calculate_inertial(errors, collisionInertial, sdfParserConfig) + self.assertFalse(len(errors)) + + + def test_correct_box_collision_calculate_inertial(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(None, errors) + + model = root.model() + link = model.link_by_index(0) + collision = link.collision_by_index(0) + + inertialErr = [] + root.resolve_auto_inertials(inertialErr, sdfParserConfig) + + l = 2.0 + w = 2.0 + h = 2.0 + + expectedMass = l * w * h * collision.density() + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + self.assertEqual(len(inertialErr), 0) + self.assertAlmostEqual(1240.0, collision.density()) + self.assertAlmostEqual(expectedMass, link.inertial().mass_matrix().mass()) + self.assertEqual(expectedInertial.mass_matrix(), link.inertial().mass_matrix()) + self.assertEqual(expectedInertial.pose(), link.inertial().pose()) + + def test_calculate_inertial_pose_not_relative_to_link(self): + + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " 0 0 1 0 0 0" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 0 0 -1 0 0 0" + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + # self.assertNotEqual(None, root.Element()) + + model = root.model() + link = model.link_by_index(0) + collision = link.collision_by_index(0) + + inertialErr = [] + root.resolve_auto_inertials(inertialErr, sdfParserConfig) + + l = 2.0 + w = 2.0 + h = 2.0 + + expectedMass = l * w * h * collision.density() + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + self.assertEqual(len(inertialErr), 0) + self.assertAlmostEqual(1240.0, collision.density()) + self.assertAlmostEqual(expectedMass, link.inertial().mass_matrix().mass()) + self.assertEqual(expectedInertial.mass_matrix(), link.inertial().mass_matrix()) + self.assertEqual(expectedInertial.pose(), link.inertial().pose()) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCylinder_TEST.py b/python/test/pyCylinder_TEST.py index 99bcf7823..b5387dd0b 100644 --- a/python/test/pyCylinder_TEST.py +++ b/python/test/pyCylinder_TEST.py @@ -16,6 +16,7 @@ import math +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Cylinder import unittest @@ -64,7 +65,7 @@ def test_assignment(self): def test_copy_construction(self): - cylinder = Cylinder(); + cylinder = Cylinder() cylinder.set_radius(0.2) cylinder.set_length(3.0) @@ -81,11 +82,11 @@ def test_copy_construction(self): self.assertEqual(3.0, cylinder2.length()) def test_deepcopy(self): - cylinder = Cylinder(); + cylinder = Cylinder() cylinder.set_radius(0.2) cylinder.set_length(3.0) - cylinder2 = copy.deepcopy(cylinder); + cylinder2 = copy.deepcopy(cylinder) self.assertEqual(0.2, cylinder2.radius()) self.assertEqual(3.0, cylinder2.length()) @@ -98,7 +99,7 @@ def test_deepcopy(self): self.assertEqual(3.0, cylinder2.length()) def test_shape(self): - cylinder = Cylinder(); + cylinder = Cylinder() self.assertEqual(0.5, cylinder.radius()) self.assertEqual(1.0, cylinder.length()) @@ -107,6 +108,44 @@ def test_shape(self): self.assertEqual(0.123, cylinder.radius()) self.assertEqual(0.456, cylinder.length()) + def test_calculate_interial(self): + cylinder = Cylinder() + + # density of aluminium + density = 2170 + + # Invalid dimensions leading to None return in + # CalculateInertial() + cylinder.set_length(-1) + cylinder.set_radius(0) + invalidCylinderInertial = cylinder.calculate_inertial(density) + self.assertEqual(None, invalidCylinderInertial) + + l = 2.0 + r = 0.1 + + cylinder.set_length(l) + cylinder.set_radius(r) + + expectedMass = cylinder.shape().volume() * density + ixxIyy = (1 / 12.0) * expectedMass * (3 * r * r + l * l) + izz = 0.5 * expectedMass * r * r + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixxIyy, ixxIyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + cylinderInertial = cylinder.calculate_inertial(density) + self.assertEqual(cylinder.shape().mat().density(), density) + self.assertNotEqual(None, cylinderInertial) + self.assertEqual(expectedInertial, cylinderInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + cylinderInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + cylinderInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), cylinderInertial.pose()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyEllipsoid_TEST.py b/python/test/pyEllipsoid_TEST.py index a102a42ad..c12690ffe 100644 --- a/python/test/pyEllipsoid_TEST.py +++ b/python/test/pyEllipsoid_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz_test_deps.math import Vector3d +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d import math from gz_test_deps.sdformat import Ellipsoid import unittest @@ -51,7 +51,7 @@ def test_deepcopy(self): def test_deepcopy_after_assignment(self): - ellipsoid1 = Ellipsoid(); + ellipsoid1 = Ellipsoid() expectedradii1 = Vector3d(1.0, 2.0, 3.0) ellipsoid1.set_radii(expectedradii1) @@ -62,8 +62,8 @@ def test_deepcopy_after_assignment(self): # This is similar to what std::swap does except it uses std::move for each # assignment tmp = copy.deepcopy(ellipsoid1) - ellipsoid1 = ellipsoid2; - ellipsoid2 = tmp; + ellipsoid1 = ellipsoid2 + ellipsoid2 = tmp self.assertEqual(expectedradii1, ellipsoid2.shape().radii()) self.assertEqual(expectedradii2, ellipsoid1.shape().radii()) @@ -77,6 +77,44 @@ def test_shape(self): ellipsoid.shape().set_radii(expectedradii) self.assertEqual(expectedradii, ellipsoid.radii()) + def test_calculate_inertial(self): + ellipsoid = Ellipsoid() + + # density of aluminium + density = 2170 + + # Invalid dimensions leading to std::nullopt return in + # CalculateInertial() + ellipsoid.set_radii(Vector3d(-1, 2, 0)) + invalidEllipsoidInertial = ellipsoid.calculate_inertial(density) + self.assertEqual(None, invalidEllipsoidInertial) + + a = 1.0 + b = 10.0 + c = 100.0 + + ellipsoid.set_radii(Vector3d(a, b, c)) + + expectedMass = ellipsoid.shape().volume() * density + ixx = (expectedMass / 5.0) * (b * b + c * c) + iyy = (expectedMass / 5.0) * (a * a + c * c) + izz = (expectedMass / 5.0) * (a * a + b * b) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + ellipsoidInertial = ellipsoid.calculate_inertial(density) + self.assertEqual(ellipsoid.shape().material().density(), density) + self.assertNotEqual(None, ellipsoidInertial) + self.assertEqual(expectedInertial, ellipsoidInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + ellipsoidInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + ellipsoidInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), ellipsoidInertial.pose()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py index 6eea3f5f4..c124f2f71 100644 --- a/python/test/pyGeometry_TEST.py +++ b/python/test/pyGeometry_TEST.py @@ -13,10 +13,11 @@ # limitations under the License. import copy -from gz_test_deps.sdformat import (Geometry, Box, Capsule, Cone, Cylinder, Ellipsoid, - Mesh, Plane, Sphere) -from gz_test_deps.math import Vector3d, Vector2d +from gz_test_deps.sdformat import (Element, Error, Geometry, Box, Capsule, Cone, Cylinder, Ellipsoid, + Heightmap, Mesh, ParserConfig, Plane, Sphere) +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d, Vector2d import gz_test_deps.sdformat as sdf +import math import unittest @@ -124,6 +125,7 @@ def test_capsule(self): self.assertEqual(0.123, geom.capsule_shape().radius()) self.assertEqual(4.56, geom.capsule_shape().length()) + def test_cone(self): geom = Geometry() geom.set_type(sdf.GeometryType.CONE) @@ -138,6 +140,7 @@ def test_cone(self): self.assertEqual(0.123, geom.cone_shape().radius()) self.assertEqual(4.56, geom.cone_shape().length()) + def test_cylinder(self): geom = Geometry() geom.set_type(sdf.GeometryType.CYLINDER) @@ -185,6 +188,23 @@ def test_mesh(self): self.assertEqual("orange", geom.mesh_shape().submesh()) self.assertTrue(geom.mesh_shape().center_submesh()) + def test_heighmap(self): + geom = Geometry() + + geom.set_type(sdf.GeometryType.HEIGHTMAP) + heightmap = Heightmap() + geom.set_heightmap_shape(heightmap) + + self.assertEqual(sdf.GeometryType.HEIGHTMAP, geom.type()) + self.assertEqual(None, geom.box_shape()) + self.assertEqual(None, geom.capsule_shape()) + self.assertEqual(None, geom.cylinder_shape()) + self.assertEqual(None, geom.ellipsoid_shape()) + self.assertEqual(None, geom.sphere_shape()) + self.assertEqual(None, geom.plane_shape()) + self.assertEqual(None, geom.mesh_shape()) + self.assertNotEqual(None, geom.heightmap_shape()) + def test_plane(self): geom = Geometry() @@ -201,5 +221,172 @@ def test_plane(self): self.assertEqual(Vector2d(9, 8), geom.plane_shape().size()) + def test_calculate_inertial(self): + geom = Geometry() + + # Density of Aluminimum + density = 2170.0 + expectedMass = 0 + expectedMassMat = MassMatrix3d() + expectedInertial = Inertiald() + sdfParserConfig = ParserConfig() + errors = [] + + # Not supported geom type + geom.set_type(sdf.GeometryType.EMPTY) + element = Element() + notSupportedInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + self.assertEqual(notSupportedInertial, None) + + box = Box() + l = 2.0 + w = 2.0 + h = 2.0 + box.set_size(Vector3d(l, w, h)) + + expectedMass = box.shape().volume() * density + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixx, iyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.BOX) + geom.set_box_shape(box) + boxInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, boxInertial) + self.assertEqual(expectedInertial, boxInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), boxInertial.pose()) + + # Capsule + capsule = Capsule() + l = 2.0 + r = 0.1 + capsule.set_length(l) + capsule.set_radius(r) + + expectedMass = capsule.shape().volume() * density + cylinderVolume = math.pi * r * r * l + sphereVolume = math.pi * 4. / 3. * r * r * r + volume = cylinderVolume + sphereVolume + cylinderMass = expectedMass * cylinderVolume / volume + sphereMass = expectedMass * sphereVolume / volume + ixxIyy = (1 / 12.0) * cylinderMass * (3 * r *r + l * l) + sphereMass * ( + 0.4 * r * r + 0.375 * r * l + 0.25 * l * l) + izz = r * r * (0.5 * cylinderMass + 0.4 * sphereMass) + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixxIyy, ixxIyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.CAPSULE) + geom.set_capsule_shape(capsule) + capsuleInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, capsuleInertial) + self.assertEqual(expectedInertial, capsuleInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), capsuleInertial.pose()) + + # Cylinder + cylinder = Cylinder() + l = 2.0 + r = 0.1 + + cylinder.set_length(l) + cylinder.set_radius(r) + + expectedMass = cylinder.shape().volume() * density + ixxIyy = (1/12.0) * expectedMass * (3*r*r + l*l) + izz = 0.5 * expectedMass * r * r + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixxIyy, ixxIyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.CYLINDER) + geom.set_cylinder_shape(cylinder) + cylinderInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, cylinderInertial) + self.assertEqual(expectedInertial, cylinderInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), cylinderInertial.pose()) + + # Ellipsoid + ellipsoid = Ellipsoid() + + a = 1.0 + b = 10.0 + c = 100.0 + + ellipsoid.set_radii(Vector3d(a, b, c)) + + expectedMass = ellipsoid.shape().volume() * density + ixx = (expectedMass / 5.0) * (b*b + c*c) + iyy = (expectedMass / 5.0) * (a*a + c*c) + izz = (expectedMass / 5.0) * (a*a + b*b) + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixx, iyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.ELLIPSOID) + geom.set_ellipsoid_shape(ellipsoid) + ellipsoidInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, ellipsoidInertial) + self.assertEqual(expectedInertial, ellipsoidInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), ellipsoidInertial.pose()) + + # Sphere + sphere = Sphere() + r = 0.1 + + sphere.set_radius(r) + + expectedMass = sphere.shape().volume() * density + ixxIyyIzz = 0.4 * expectedMass * r * r + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments( + Vector3d(ixxIyyIzz, ixxIyyIzz, ixxIyyIzz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.SPHERE) + geom.set_sphere_shape(sphere) + sphereInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, sphereInertial) + self.assertEqual(expectedInertial, sphereInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), sphereInertial.pose()) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyLink_TEST.py b/python/test/pyLink_TEST.py index e7a7a77a2..154b085b9 100644 --- a/python/test/pyLink_TEST.py +++ b/python/test/pyLink_TEST.py @@ -14,8 +14,8 @@ import copy from gz_test_deps.math import Pose3d, Inertiald, MassMatrix3d, Vector3d -from gz_test_deps.sdformat import (Collision, Light, Link, Projector, Sensor, - Visual, SDFErrorsException) +from gz_test_deps.sdformat import (Collision, Light, Link, ParserConfig, Projector, Sensor, + Visual, Root, SDFErrorsException) import unittest import math @@ -61,6 +61,18 @@ def test_default_construction(self): link.set_enable_wind(True) self.assertTrue(link.enable_wind()) + self.assertTrue(link.enable_gravity()) + link.set_enable_gravity(False) + self.assertFalse(link.enable_gravity()) + + self.assertFalse(link.auto_inertia_saved()) + link.set_auto_inertia_saved(True) + self.assertTrue(link.auto_inertia_saved()) + + self.assertFalse(link.auto_inertia()) + link.set_auto_inertia(True) + self.assertTrue(link.auto_inertia()) + self.assertFalse(link.kinematic()) link.set_kinematic(True) self.assertTrue(link.kinematic()) @@ -317,8 +329,8 @@ def test_mutable_by_index(self): pr = link.projector_by_index(0) self.assertNotEqual(None, pr) self.assertEqual("projector1", pr.name()) - pr.set_name("projector2"); - self.assertEqual("projector2", link.projector_by_index(0).name()); + pr.set_name("projector2") + self.assertEqual("projector2", link.projector_by_index(0).name()) def test_mutable_by_name(self): link = Link() @@ -380,7 +392,7 @@ def test_mutable_by_name(self): self.assertFalse(link.sensor_name_exists("sensor1")) self.assertTrue(link.sensor_name_exists("sensor2")) - # // Modify the particle emitter + # # Modify the particle emitter # sdf::ParticleEmitter *p = link.ParticleEmitterByName("pe1") # self.assertNotEqual(None, p) # self.assertEqual("pe1", p.name()) @@ -389,12 +401,154 @@ def test_mutable_by_name(self): # self.assertTrue(link.particle_emitter_name_exists("pe2")) # Modify the projector - pr = link.projector_by_name("projector1"); - self.assertNotEqual(None, pr); - self.assertEqual("projector1", pr.name()); - pr.set_name("projector2"); - self.assertFalse(link.projector_name_exists("projector1")); - self.assertTrue(link.projector_name_exists("projector2")); + pr = link.projector_by_name("projector1") + self.assertNotEqual(None, pr) + self.assertEqual("projector1", pr.name()) + pr.set_name("projector2") + self.assertFalse(link.projector_name_exists("projector1")) + self.assertTrue(link.projector_name_exists("projector2")) + + def test_resolveauto_inertialsWithNoCollisionsInLink(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + with self.assertRaises(SDFErrorsException): + errors = root.load_sdf_string(sdf, sdfParserConfig) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + # Default Inertial values set during load + self.assertEqual(1.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Vector3d.ONE, + link.inertial().mass_matrix().diagonal_moments()) + + def test_resolveauto_inertialsWithMultipleCollisions(self): + sdf = "" + \ + "" + \ + " " + \ + " 0 0 1.0 0 0 0" + \ + " " + \ + " " + \ + " " + \ + " 0 0 -0.5 0 0 0" + \ + " 2.0" + \ + " " + \ + " " + \ + " 1 1 1" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 0 0 0.5 0 0 0" + \ + " 4" + \ + " " + \ + " " + \ + " 0.5" + \ + " 1.0" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + # Mass of cube(volume * density) + mass of cylinder(volume * density) + expectedMass = 1.0 * 2.0 + math.pi * 0.5 * 0.5 * 1 * 4.0 + + self.assertAlmostEqual(expectedMass, link.inertial().mass_matrix().mass()) + self.assertEqual(Vector3d(2.013513, 2.013513, 0.72603), + link.inertial().mass_matrix().diagonal_moments()) + + def test_inertial_values_given_with_auto_set_to_true(self): + sdf = "" + \ + "" + \ + " " + \ + " " + \ + " " + \ + " 4.0" + \ + " 1 1 1 2 2 2" + \ + " " + \ + " 1" + \ + " 1" + \ + " 1" + \ + " " + \ + " " + \ + " " + \ + " 2.0" + \ + " " + \ + " " + \ + " 1 1 1" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + self.assertNotEqual(4.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Pose3d.ZERO, link.inertial().pose()) + self.assertEqual(Vector3d(0.33333, 0.33333, 0.33333), + link.inertial().mass_matrix().diagonal_moments()) + + def test_resolveauto_inertialsCalledWithAutoFalse(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + # Default Inertial values set during load + self.assertEqual(1.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Vector3d.ONE, + link.inertial().mass_matrix().diagonal_moments()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyParserConfig_TEST.py b/python/test/pyParserConfig_TEST.py index a56bfc405..52917dd30 100644 --- a/python/test/pyParserConfig_TEST.py +++ b/python/test/pyParserConfig_TEST.py @@ -26,7 +26,7 @@ def test_construction(self): self.assertFalse(config.uri_path_map()) self.assertFalse(config.find_file_callback()) - testDir = source_file(); + testDir = source_file() config.add_uri_path("file://", testDir) self.assertTrue(config.uri_path_map()) @@ -35,7 +35,7 @@ def test_construction(self): self.assertEqual(it[0], testDir) def testFunc(argument): - return "test/dir2"; + return "test/dir2" config.set_find_callback(testFunc) self.assertTrue(config.find_file_callback()) @@ -102,7 +102,7 @@ def test_copy(self): # so we'll use the source path testDir1 = source_file() - config1 = ParserConfig(); + config1 = ParserConfig() config1.add_uri_path("file://", testDir1) it = config1.uri_path_map().get("file://") diff --git a/python/test/pyParticleEmitter_TEST.py b/python/test/pyParticleEmitter_TEST.py index dc44557bf..9995eb66d 100644 --- a/python/test/pyParticleEmitter_TEST.py +++ b/python/test/pyParticleEmitter_TEST.py @@ -34,8 +34,6 @@ def test_default_construction(self): self.assertTrue(emitter.set_type("box")) self.assertEqual("box", emitter.type_str()) self.assertEqual(ParticleEmitter.ParticleEmitterType.BOX, emitter.type()) - emitter.set_type(ParticleEmitter.ParticleEmitterType.CONE) - self.assertEqual("cone", emitter.type_str()) emitter.set_type(ParticleEmitter.ParticleEmitterType.CYLINDER) self.assertEqual("cylinder", emitter.type_str()) diff --git a/python/test/pyRoot_TEST.py b/python/test/pyRoot_TEST.py index 0df1d3023..0a41bf667 100644 --- a/python/test/pyRoot_TEST.py +++ b/python/test/pyRoot_TEST.py @@ -14,7 +14,7 @@ import copy from gz_test_deps.math import Pose3d -from gz_test_deps.sdformat import (Error, Model, Light, Root, SDF_VERSION, +from gz_test_deps.sdformat import (ConfigureResolveAutoInertials, Error, Model, ParserConfig, Light, Root, SDF_VERSION, SDFErrorsException, SDF_PROTOCOL_VERSION, World) import gz_test_deps.sdformat as sdf @@ -121,7 +121,7 @@ def test_string_light_sdf_parse(self): # ///////////////////////////////////////////////// # TEST(DOMRoot, StringActorSdfParse) # { - # std::string sdf = "" + # sdf = "" # " " # " " # " 0 0 1.0 0 0 0" @@ -324,6 +324,39 @@ def test_element_to_light(self): # ASSERT_EQ(None, root2.Actor()) self.assertEqual(0, root2.world_count()) + def test_resolve_auto_inertials_with_save_calculation_configuration(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(None, errors) + + model = root.model() + link = model.link_by_index(0) + + sdfParserConfig.set_calculate_inertial_configuration( + ConfigureResolveAutoInertials.SAVE_CALCULATION) + + inertialErr = [] + root.resolve_auto_inertials(inertialErr, sdfParserConfig) + self.assertEqual(len(inertialErr), 0) + self.assertTrue(link.auto_inertia_saved()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pySensor_TEST.py b/python/test/pySensor_TEST.py index 4a7f28415..204bd4adf 100644 --- a/python/test/pySensor_TEST.py +++ b/python/test/pySensor_TEST.py @@ -60,12 +60,14 @@ def test_default_construction(self): self.assertAlmostEqual(0.0, sensor.update_rate()) + self.assertFalse(sensor.frame_id()) self.assertFalse(sensor.topic()) self.assertFalse(sensor == sensor2) self.assertTrue(sensor != sensor2) def test_copy_construction(self): sensor = Sensor() + sensor.set_frame_id("test_frame_id") sensor.set_raw_pose(Pose3d(1, 2, 3, 0, 0, 0)) sensor.set_type(sdf.Sensortype.MAGNETOMETER) sensor.set_pose_relative_to("a_frame") @@ -79,6 +81,7 @@ def test_copy_construction(self): sensor2 = Sensor(sensor) + self.assertEqual("test_frame_id", sensor.frame_id()) self.assertEqual(sdf.Sensortype.MAGNETOMETER, sensor.type()) self.assertEqual(Pose3d(1, 2, 3, 0, 0, 0), sensor.raw_pose()) self.assertEqual("a_frame", sensor.pose_relative_to()) @@ -86,6 +89,7 @@ def test_copy_construction(self): self.assertAlmostEqual(mag.x_noise().mean(), sensor.magnetometer_sensor().x_noise().mean()) + self.assertEqual("test_frame_id", sensor2.frame_id()) self.assertEqual(sdf.Sensortype.MAGNETOMETER, sensor2.type()) self.assertEqual(Pose3d(1, 2, 3, 0, 0, 0), sensor2.raw_pose()) self.assertEqual("a_frame", sensor2.pose_relative_to()) @@ -96,6 +100,7 @@ def test_copy_construction(self): def test_deepcopy(self): sensor = Sensor() + sensor.set_frame_id("test_frame_id") sensor.set_raw_pose(Pose3d(1, 2, 3, 0, 0, 0)) sensor.set_type(sdf.Sensortype.MAGNETOMETER) sensor.set_pose_relative_to("a_frame") @@ -108,6 +113,7 @@ def test_deepcopy(self): sensor2 = copy.deepcopy(sensor) + self.assertEqual("test_frame_id", sensor.frame_id()) self.assertEqual(sdf.Sensortype.MAGNETOMETER, sensor.type()) self.assertEqual(Pose3d(1, 2, 3, 0, 0, 0), sensor.raw_pose()) self.assertEqual("a_frame", sensor.pose_relative_to()) @@ -115,6 +121,7 @@ def test_deepcopy(self): self.assertAlmostEqual(mag.x_noise().mean(), sensor.magnetometer_sensor().x_noise().mean()) + self.assertEqual("test_frame_id", sensor2.frame_id()) self.assertEqual(sdf.Sensortype.MAGNETOMETER, sensor2.type()) self.assertEqual(Pose3d(1, 2, 3, 0, 0, 0), sensor2.raw_pose()) self.assertEqual("a_frame", sensor2.pose_relative_to()) diff --git a/python/test/pySphere_TEST.py b/python/test/pySphere_TEST.py index 0636d602b..f687f6815 100644 --- a/python/test/pySphere_TEST.py +++ b/python/test/pySphere_TEST.py @@ -15,66 +15,101 @@ import copy import math from gz_test_deps.sdformat import Sphere +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d 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()); + 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); + sphere = Sphere() + sphere.set_radius(0.2) - sphere2 = sphere; - self.assertEqual(0.2, sphere2.radius()); + sphere2 = sphere + self.assertEqual(0.2, sphere2.radius()) def test_copy_construction(self): - sphere = Sphere(); - sphere.set_radius(0.2); + sphere = Sphere() + sphere.set_radius(0.2) - sphere2 = Sphere(sphere); - self.assertEqual(0.2, sphere2.radius()); + 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()); + 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); + sphere = Sphere() + sphere.set_radius(0.2) - sphere2 = copy.deepcopy(sphere); - self.assertEqual(0.2, sphere2.radius()); + sphere2 = copy.deepcopy(sphere) + self.assertEqual(0.2, sphere2.radius()) def test_deepcopy_after_assignment(self): - sphere1 = Sphere(); - sphere1.set_radius(0.1); + sphere1 = Sphere() + sphere1.set_radius(0.1) - sphere2 = Sphere(); - sphere2.set_radius(0.2); + sphere2 = Sphere() + sphere2.set_radius(0.2) # This is similar to what swap does - tmp = copy.deepcopy(sphere1); - sphere1 = sphere2; - sphere2 = tmp; + tmp = copy.deepcopy(sphere1) + sphere1 = sphere2 + sphere2 = tmp - self.assertEqual(0.2, sphere1.radius()); - self.assertEqual(0.1, sphere2.radius()); + 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 = Sphere() + self.assertEqual(1.0, sphere.radius()) - sphere.shape().set_radius(0.123); - self.assertEqual(0.123, sphere.radius()); + sphere.shape().set_radius(0.123) + self.assertEqual(0.123, sphere.radius()) + + def test_calculate_inertial(self): + sphere = Sphere() + + # density of aluminium + density = 2170 + + sphere.set_radius(-2) + invalidSphereInertial = sphere.calculate_inertial(density) + self.assertEqual(None, invalidSphereInertial) + + r = 0.1 + + sphere.set_radius(r) + + expectedMass = sphere.shape().volume() * density + ixxIyyIzz = 0.4 * expectedMass * r * r + + expectedMassMat = MassMatrix3d( + expectedMass, Vector3d(ixxIyyIzz, ixxIyyIzz, ixxIyyIzz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + sphereInertial = sphere.calculate_inertial(density) + self.assertEqual(sphere.shape().material().density(), density) + self.assertNotEqual(None, sphereInertial) + self.assertEqual(expectedInertial, sphereInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + sphereInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + sphereInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), sphereInertial.pose()) if __name__ == '__main__': diff --git a/python/test/pySurface_TEST.py b/python/test/pySurface_TEST.py index 2651b1113..ed6875c0d 100644 --- a/python/test/pySurface_TEST.py +++ b/python/test/pySurface_TEST.py @@ -14,7 +14,8 @@ import copy from gz_test_deps.math import Vector3d -from gz_test_deps.sdformat import Surface, Contact, Friction, ODE +from gz_test_deps.sdformat import Surface, Contact, Friction, ODE, \ + BulletFriction, Torsional import unittest @@ -34,8 +35,24 @@ def test_assigment_construction(self): ode.set_slip1(3) ode.set_slip2(4) ode.set_fdir1(Vector3d(1, 2, 3)) + + bullet = BulletFriction() + bullet.set_friction(0.11) + bullet.set_friction2(0.22) + bullet.set_fdir1(Vector3d(3, 2, 1)) + bullet.set_rolling_friction(0.33) + + torsional = Torsional() + torsional.set_coefficient(1.2) + torsional.set_use_patch_radius(True) + torsional.set_patch_radius(0.5) + torsional.set_surface_radius(0.15) + torsional.set_ode_slip(0.01) + friction = Friction() friction.set_ode(ode) + friction.set_bullet_friction(bullet) + friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) surface1.set_contact(contact) surface1.set_friction(friction) @@ -48,6 +65,17 @@ def test_assigment_construction(self): self.assertEqual(surface2.friction().ode().slip2(), 4) self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) surface1.set_contact(contact) @@ -60,6 +88,20 @@ def test_assigment_construction(self): ode.set_slip2(4.1) ode.set_fdir1(Vector3d(1.1, 2.1, 3.1)) friction.set_ode(ode) + + bullet.set_friction(0.33) + bullet.set_friction2(0.45) + bullet.set_fdir1(Vector3d(0, 1, 2)) + bullet.set_rolling_friction(0.03) + friction.set_bullet_friction(bullet) + + torsional.set_coefficient(2.1) + torsional.set_use_patch_radius(False) + torsional.set_patch_radius(0.1) + torsional.set_surface_radius(0.9) + torsional.set_ode_slip(0.7) + friction.set_torsional(torsional) + surface1.set_friction(friction) self.assertEqual(surface1.friction().ode().mu(), 1.1) self.assertEqual(surface1.friction().ode().mu2(), 1.2) @@ -74,6 +116,29 @@ def test_assigment_construction(self): self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1.1, 2.1, 3.1)) + self.assertEqual(surface1.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface1.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface1.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface1.friction().bullet_friction().rolling_friction(), + 0.03) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.03) + + self.assertEqual(surface1.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface1.friction().torsional().use_patch_radius()) + self.assertEqual(surface1.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface1.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface1.friction().torsional().ode_slip(), 0.7) + self.assertEqual(surface2.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.7) def test_copy_construction(self): surface1 = Surface() @@ -84,8 +149,21 @@ def test_copy_construction(self): ode.set_slip1(3) ode.set_slip2(4) ode.set_fdir1(Vector3d(1, 2, 3)) + bullet = BulletFriction() + bullet.set_friction(0.11) + bullet.set_friction2(0.22) + bullet.set_fdir1(Vector3d(3, 2, 1)) + bullet.set_rolling_friction(0.33) + torsional = Torsional() + torsional.set_coefficient(1.2) + torsional.set_use_patch_radius(True) + torsional.set_patch_radius(0.5) + torsional.set_surface_radius(0.15) + torsional.set_ode_slip(0.01) friction = Friction() friction.set_ode(ode) + friction.set_bullet_friction(bullet) + friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) surface1.set_contact(contact) surface1.set_friction(friction) @@ -98,6 +176,17 @@ def test_copy_construction(self): self.assertEqual(surface2.friction().ode().slip2(), 4) self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) surface1.set_contact(contact) @@ -110,6 +199,20 @@ def test_copy_construction(self): ode.set_slip2(4.1) ode.set_fdir1(Vector3d(1.1, 2.1, 3.1)) friction.set_ode(ode) + + bullet.set_friction(0.33) + bullet.set_friction2(0.45) + bullet.set_fdir1(Vector3d(0, 1, 2)) + bullet.set_rolling_friction(0.03) + friction.set_bullet_friction(bullet) + + torsional.set_coefficient(2.1) + torsional.set_use_patch_radius(False) + torsional.set_patch_radius(0.1) + torsional.set_surface_radius(0.9) + torsional.set_ode_slip(0.7) + friction.set_torsional(torsional) + surface1.set_friction(friction) self.assertEqual(surface1.friction().ode().mu(), 1.1) self.assertEqual(surface1.friction().ode().mu2(), 1.2) @@ -124,6 +227,29 @@ def test_copy_construction(self): self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface1.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface1.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface1.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface1.friction().bullet_friction().rolling_friction(), + 0.03) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + + self.assertEqual(surface1.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface1.friction().torsional().use_patch_radius()) + self.assertEqual(surface1.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface1.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface1.friction().torsional().ode_slip(), 0.7) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) def test_deepcopy(self): surface1 = Surface() @@ -134,8 +260,21 @@ def test_deepcopy(self): ode.set_slip1(3) ode.set_slip2(4) ode.set_fdir1(Vector3d(1, 2, 3)) + bullet = BulletFriction() + bullet.set_friction(0.11) + bullet.set_friction2(0.22) + bullet.set_fdir1(Vector3d(3, 2, 1)) + bullet.set_rolling_friction(0.33) + torsional = Torsional() + torsional.set_coefficient(1.2) + torsional.set_use_patch_radius(True) + torsional.set_patch_radius(0.5) + torsional.set_surface_radius(0.15) + torsional.set_ode_slip(0.01) friction = Friction() friction.set_ode(ode) + friction.set_bullet_friction(bullet) + friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) surface1.set_contact(contact) surface1.set_friction(friction) @@ -148,6 +287,17 @@ def test_deepcopy(self): self.assertEqual(surface2.friction().ode().slip2(), 4) self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) surface1.set_contact(contact) @@ -160,7 +310,19 @@ def test_deepcopy(self): ode.set_slip2(4.1) ode.set_fdir1(Vector3d(1.1, 2.1, 3.1)) friction.set_ode(ode) + bullet.set_friction(0.33) + bullet.set_friction2(0.45) + bullet.set_fdir1(Vector3d(0, 1, 2)) + bullet.set_rolling_friction(0.03) + friction.set_bullet_friction(bullet) + torsional.set_coefficient(2.1) + torsional.set_use_patch_radius(False) + torsional.set_patch_radius(0.1) + torsional.set_surface_radius(0.9) + torsional.set_ode_slip(0.7) + friction.set_torsional(torsional) surface1.set_friction(friction) + self.assertEqual(surface1.friction().ode().mu(), 1.1) self.assertEqual(surface1.friction().ode().mu2(), 1.2) self.assertEqual(surface1.friction().ode().slip1(), 3.1) @@ -174,6 +336,29 @@ def test_deepcopy(self): self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface1.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface1.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface1.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface1.friction().bullet_friction().rolling_friction(), + 0.03) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + + self.assertEqual(surface1.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface1.friction().torsional().use_patch_radius()) + self.assertEqual(surface1.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface1.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface1.friction().torsional().ode_slip(), 0.7) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) def test_default_contact_construction(self): contact = Contact() @@ -196,6 +381,21 @@ def test_default_ode_construction(self): self.assertEqual(ode.fdir1(), Vector3d(0, 0, 0)) + def test_default_bullet_friction_construction(self): + bullet = BulletFriction() + self.assertEqual(bullet.friction(), 1.0) + self.assertEqual(bullet.friction2(), 1.0) + self.assertEqual(bullet.fdir1(), + Vector3d(0, 0, 0)) + self.assertEqual(bullet.rolling_friction(), 1.0) + + def test_default_torsional_construction(self): + torsional = Torsional() + self.assertEqual(torsional.coefficient(), 1.0) + self.assertTrue(torsional.use_patch_radius()) + self.assertEqual(torsional.patch_radius(), 0.0) + self.assertEqual(torsional.surface_radius(), 0.0) + self.assertEqual(torsional.ode_slip(), 0.0) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py index f77fcdc8e..f8f5f9d0c 100644 --- a/python/test/pyWorld_TEST.py +++ b/python/test/pyWorld_TEST.py @@ -14,8 +14,9 @@ import copy from gz_test_deps.math import Color, Vector3d, SphericalCoordinates +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import (Atmosphere, Gui, Physics, Plugin, Error, - Frame, Joint, Light, Model, Scene, World) + Frame, Joint, Light, Model, ParserConfig, Root, Scene, World) import gz_test_deps.sdformat as sdf import unittest import math @@ -87,8 +88,8 @@ def test_default_construction(self): "PoseRelativeToGraph error: scope does not point to a valid graph.") # world doesn't have graphs, so no names should exist in graphs - self.assertFalse(world.name_exists_in_frame_attached_to_graph("")); - self.assertFalse(world.name_exists_in_frame_attached_to_graph("link")); + self.assertFalse(world.name_exists_in_frame_attached_to_graph("")) + self.assertFalse(world.name_exists_in_frame_attached_to_graph("link")) def test_copy_construction(self): @@ -443,6 +444,55 @@ def test_plugins(self): world.clear_plugins() self.assertEqual(0, len(world.plugins())) + def test_resolve_auto_inertials(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(None, errors) + + world = root.world_by_index(0) + model = world.model_by_index(0) + link = model.link_by_index(0) + + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + + l = 2.0 + w = 2.0 + h = 2.0 + + expectedMass = l * w * h * 1240.0 + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + self.assertEqual(0, len(errors)) + self.assertEqual(expectedInertial, link.inertial()) if __name__ == '__main__': unittest.main() diff --git a/sdf/1.11/particle_emitter.sdf b/sdf/1.11/particle_emitter.sdf index 1be49ccc8..d6a2fdf7e 100755 --- a/sdf/1.11/particle_emitter.sdf +++ b/sdf/1.11/particle_emitter.sdf @@ -7,7 +7,7 @@ - The type of a particle emitter. One of "box", "cylinder", "cone", "ellipsoid", or "point". + The type of a particle emitter. One of "box", "cylinder", "ellipsoid", or "point". @@ -26,8 +26,6 @@ depending on the emmiter type: - point: The area is ignored. - box: The area is interpreted as width X height X depth. - - cone: The area is interpreted as the bounding box of the - cone. The cone is oriented along the Z-axis. - cylinder: The area is interpreted as the bounding box of the cylinder. The cylinder is oriented along the Z-axis. - ellipsoid: The area is interpreted as the bounding box of an diff --git a/sdf/1.12/CMakeLists.txt b/sdf/1.12/CMakeLists.txt index 08b4bf240..82d9ebf71 100644 --- a/sdf/1.12/CMakeLists.txt +++ b/sdf/1.12/CMakeLists.txt @@ -26,6 +26,7 @@ set (sdfs imu.sdf inertial.sdf joint.sdf + joint_state.sdf lidar.sdf light.sdf light_state.sdf diff --git a/sdf/1.12/camera.sdf b/sdf/1.12/camera.sdf index 8fd218caf..cf959397b 100644 --- a/sdf/1.12/camera.sdf +++ b/sdf/1.12/camera.sdf @@ -222,7 +222,7 @@ - + An optional frame id name to be used in the camera_info message header. diff --git a/sdf/1.12/joint_state.sdf b/sdf/1.12/joint_state.sdf new file mode 100644 index 000000000..096b56965 --- /dev/null +++ b/sdf/1.12/joint_state.sdf @@ -0,0 +1,133 @@ + + + + The joint state element encapsulates variables within a joint that may + change over time, currently limited to the joint angle. + + + + Name of the joint + + + + + Index of the axis. + + + Angle of an axis + + + + + Contains the state of the first joint axis. + + + + The position of the first joint axis. + + + + If this is a rotational axis and this attribute is true, + the joint position is expressed in units of degrees [deg], + otherwise it is expressed in radians [rad]. + If this axis is translational (such as a prismatic joint), the + units will be interpreted in meters [m] regardless of the value of + this attribute. + + + + + + The velocity of the first joint axis. + + + + If this is a rotational axis and this attribute is true, + the joint velocity is expressed in units of degrees per + second [deg/s], otherwise it is expressed in radians per second + [rad/s]. + If this axis is translational (such as a prismatic joint), the + units will be interpreted in meters per second [m/s] regardless of + the value of this attribute. + + + + + + The acceleration of the first joint axis. + + + + If this is a rotational axis and this attribute is true, + the joint acceleration is expressed in units of degrees per + second per second [deg/s^2], otherwise it is expressed in radians per + second per second [rad/s^2]. + If this axis is translational (such as a prismatic joint), the + units will be interpreted in meters per second per second [m/s^2] + regardless of the value of this attribute. + + + + + + The effort applied at the first joint axis. + + + + + + Contains the state of the second joint axis. + + + + The position of the second joint axis. + + + + If this is a rotational axis and this attribute is true, + the joint position is expressed in units of degrees [deg], + otherwise it is expressed in radians [rad]. + If this axis is translational (such as a prismatic joint), the + units will be interpreted in meters [m] regardless of the value of + this attribute. + + + + + + The velocity of the second joint axis. + + + + If this is a rotational axis and this attribute is true, + the joint velocity is expressed in units of degrees per + second [deg/s], otherwise it is expressed in radians per second + [rad/s]. + If this axis is translational (such as a prismatic joint), the + units will be interpreted in meters per second [m/s] regardless of + the value of this attribute. + + + + + + The acceleration of the second joint axis. + + + + If this is a rotational axis and this attribute is true, + the joint acceleration is expressed in units of degrees per + second per second [deg/s^2], otherwise it is expressed in radians per + second per second [rad/s^2]. + If this axis is translational (such as a prismatic joint), the + units will be interpreted in meters per second per second [m/s^2] + regardless of the value of this attribute. + + + + + + The effort applied at the second joint axis. + + + diff --git a/sdf/1.12/light_state.sdf b/sdf/1.12/light_state.sdf index 673efcdb4..54bebb02f 100644 --- a/sdf/1.12/light_state.sdf +++ b/sdf/1.12/light_state.sdf @@ -1,6 +1,9 @@ - - Light state + + + The light state element encapsulates variables within a light that may + change over time, currently limited to its pose. + Name of the light diff --git a/sdf/1.12/link.sdf b/sdf/1.12/link.sdf index b518ff3c5..856629a37 100644 --- a/sdf/1.12/link.sdf +++ b/sdf/1.12/link.sdf @@ -19,7 +19,7 @@ - If true, the link is kinematic only + If true, the link is kinematic only. A kinematic link does not react to forces such as gravity, applied force / torque, or influences from other dynamic bodies. The link can be animated by changing its position or velocity. Kinematic links can also be connected by joints, and moved by joint position or velocity commands. diff --git a/sdf/1.12/link_state.sdf b/sdf/1.12/link_state.sdf index 28fff465d..d086e4856 100644 --- a/sdf/1.12/link_state.sdf +++ b/sdf/1.12/link_state.sdf @@ -1,33 +1,85 @@ - - Link state + + + The link state element encapsulates variables within a link that may + change over time, including pose, velocity, acceleration, applied wrench, + and the state of attached collisions. + Name of the link - + + Angular velocity of the link frame relative to the world frame. + + + + + If true, the angular velocity is expressed in units of degrees per + second [deg/s], otherwise it is expressed in radians per second [rad/s]. + + + + + + Linear velocity of the link frame relative to the world frame. + + + + Velocity of the link. The x, y, z components of the pose correspond to the linear velocity of the link, and the roll, pitch, yaw components correspond to the angular velocity of the link - + + + Angular acceleration of the link frame relative to the world frame. + + + + + If true, the angular acceleration is expressed in units of degrees per + second per second [deg/s^2], otherwise it is expressed in radians per + second per second [rad/s^2]. + + + + + + + Linear acceleration of the link frame relative to the world frame. + + + + Acceleration of the link. The x, y, z components of the pose correspond to the linear acceleration of the link, and the roll, pitch, yaw components correspond to the angular acceleration of the link - + + Torque acting on the link relative to the world frame. + + + + + + Force acting on the link at the link frame relative to the world frame. + + + + Force and torque applied to the link. The x, y, z components of the pose correspond to the force applied to the link, and the roll, pitch, yaw components correspond to the torque applied to the link - + Collision state diff --git a/sdf/1.12/model.sdf b/sdf/1.12/model.sdf index 57dc83a01..de8b43130 100644 --- a/sdf/1.12/model.sdf +++ b/sdf/1.12/model.sdf @@ -62,6 +62,7 @@ URI to a resource, such as a model + @@ -85,6 +86,8 @@ + + If set to true, all links in the model will be affected by the wind. Can be overriden by the link wind property. diff --git a/sdf/1.12/model_state.sdf b/sdf/1.12/model_state.sdf index 3fd296ff2..cbbbfd0de 100644 --- a/sdf/1.12/model_state.sdf +++ b/sdf/1.12/model_state.sdf @@ -1,28 +1,18 @@ - - Model state + + + The model state element encapsulates variables within a model that may + change over time, including object poses, the states of its nested models + and links and joints, and changes in model scale. + Name of the model - - Joint angle + - - Name of the joint - - - - - Index of the axis. - - - Angle of an axis - - - - + A nested model state element Name of the model. diff --git a/sdf/1.12/particle_emitter.sdf b/sdf/1.12/particle_emitter.sdf index aec7b90cc..d6a2fdf7e 100755 --- a/sdf/1.12/particle_emitter.sdf +++ b/sdf/1.12/particle_emitter.sdf @@ -7,7 +7,7 @@ - The type of a particle emitter. One of "box", "cone", "cylinder", "ellipsoid", or "point". + The type of a particle emitter. One of "box", "cylinder", "ellipsoid", or "point". @@ -26,8 +26,6 @@ depending on the emmiter type: - point: The area is ignored. - box: The area is interpreted as width X height X depth. - - cone: The area is interpreted as the bounding box of the - cone. The cone is oriented along the Z-axis. - cylinder: The area is interpreted as the bounding box of the cylinder. The cylinder is oriented along the Z-axis. - ellipsoid: The area is interpreted as the bounding box of an diff --git a/sdf/1.12/sensor.sdf b/sdf/1.12/sensor.sdf index 78954738b..4e0876fa7 100644 --- a/sdf/1.12/sensor.sdf +++ b/sdf/1.12/sensor.sdf @@ -59,6 +59,10 @@ If true, the sensor will publish performance metrics + + An optional frame id which indicates the sensor's frame of reference. + + diff --git a/sdf/1.12/state.sdf b/sdf/1.12/state.sdf index 6a13e28bf..30fbd4e42 100644 --- a/sdf/1.12/state.sdf +++ b/sdf/1.12/state.sdf @@ -1,5 +1,12 @@ + + The state element encapsulates variables within a world that may change + over time, including object poses, dynamic states such as velocity and + acceleration, a description of objects added to the world, and a list + of objects deleted from the world. + + Name of the world this state applies to @@ -22,13 +29,14 @@ - A list containing the entire description of entities inserted. + A list containing the entire description of entities inserted into the world. + - A list of names of deleted entities/ + A list of names of entities deleted from the world./ The name of a deleted entity. @@ -38,4 +46,6 @@ + + diff --git a/sdf/1.12/world.sdf b/sdf/1.12/world.sdf index 6235e193b..d39ad8819 100644 --- a/sdf/1.12/world.sdf +++ b/sdf/1.12/world.sdf @@ -40,6 +40,7 @@ Override the static value of the included entity. + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index dfd569186..3ffa05625 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -23,6 +23,24 @@ gz_create_core_library(SOURCES ${sources} CXX_STANDARD 17 LEGACY_PROJECT_PREFIX SDFormat ) +gz_add_get_install_prefix_impl(GET_INSTALL_PREFIX_FUNCTION sdf::getInstallPrefix + GET_INSTALL_PREFIX_HEADER sdf/InstallationDirectories.hh + OVERRIDE_INSTALL_PREFIX_ENV_VARIABLE SDF_INSTALL_PREFIX) + +# CMAKE_INSTALL_DATAROOTDIR may be an absolute path, let's make sure to use the +# relative version +if(IS_ABSOLUTE "${CMAKE_INSTALL_DATAROOTDIR}") + file(RELATIVE_PATH CMAKE_INSTALL_RELATIVE_DATAROOTDIR "${CMAKE_INSTALL_PREFIX}" "${CMAKE_INSTALL_DATAROOTDIR}") +else() + set(CMAKE_INSTALL_RELATIVE_DATAROOTDIR "${CMAKE_INSTALL_DATAROOTDIR}") +endif() + +set_property( + SOURCE InstallationDirectories.cc + PROPERTY COMPILE_DEFINITIONS + CMAKE_INSTALL_RELATIVE_DATAROOTDIR="${CMAKE_INSTALL_RELATIVE_DATAROOTDIR}" +) + target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PUBLIC @@ -99,6 +117,8 @@ if (BUILD_TESTING) ${PROJECT_SOURCE_DIR}/test LIB_DEPS library_for_tests + ENVIRONMENT + SDF_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} ) if (TARGET UNIT_gz_TEST) diff --git a/src/Camera.cc b/src/Camera.cc index 2f2ea7a2b..88c0e584b 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -208,7 +208,7 @@ class sdf::Camera::Implementation public: double lensProjectionTy{0.0}; /// \brief lens instrinsics s. - public: double lensIntrinsicsS{1.0}; + public: double lensIntrinsicsS{0.0}; /// \brief True if this camera has custom intrinsics values public: bool hasIntrinsics = false; @@ -752,6 +752,9 @@ void Camera::SetSaveFramesPath(const std::string &_path) ////////////////////////////////////////////////// bool Camera::operator==(const Camera &_cam) const { + + // \todo(iche033) Remove in sdformat16 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION return this->Name() == _cam.Name() && this->HorizontalFov() == _cam.HorizontalFov() && this->ImageWidth() == _cam.ImageWidth() && @@ -764,6 +767,7 @@ bool Camera::operator==(const Camera &_cam) const this->ImageNoise() == _cam.ImageNoise() && this->VisibilityMask() == _cam.VisibilityMask() && this->OpticalFrameId() == _cam.OpticalFrameId(); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } ////////////////////////////////////////////////// @@ -1332,8 +1336,11 @@ sdf::ElementPtr Camera::ToElement() const this->SegmentationType()); } + // \todo(iche033) Remove in sdformat16 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION elem->GetElement("optical_frame_id")->Set( this->OpticalFrameId()); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION return elem; } diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 68b61beac..bfcda9261 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -134,9 +134,12 @@ TEST(DOMCamera, Construction) cam.SetPoseRelativeTo("/frame"); EXPECT_EQ("/frame", cam.PoseRelativeTo()); + // \todo(iche033) Remove in sdformat16 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION EXPECT_TRUE(cam.OpticalFrameId().empty()); cam.SetOpticalFrameId("/optical_frame"); EXPECT_EQ("/optical_frame", cam.OpticalFrameId()); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION EXPECT_EQ("stereographic", cam.LensType()); cam.SetLensType("custom"); @@ -214,7 +217,7 @@ TEST(DOMCamera, Construction) cam.SetLensProjectionTy(2); EXPECT_DOUBLE_EQ(2, cam.LensProjectionTy()); - EXPECT_DOUBLE_EQ(1.0, cam.LensIntrinsicsSkew()); + EXPECT_DOUBLE_EQ(0, cam.LensIntrinsicsSkew()); cam.SetLensIntrinsicsSkew(2.3); EXPECT_DOUBLE_EQ(2.3, cam.LensIntrinsicsSkew()); @@ -273,7 +276,12 @@ TEST(DOMCamera, ToElement) cam.SetPoseRelativeTo("/frame"); cam.SetSaveFrames(true); cam.SetSaveFramesPath("/tmp"); + + // \todo(iche033) Remove in sdformat16 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION cam.SetOpticalFrameId("/optical_frame"); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + cam.SetCameraInfoTopic("/camera_info_test"); cam.SetTriggerTopic("/trigger_topic_test"); cam.SetTriggered(true); @@ -296,7 +304,12 @@ TEST(DOMCamera, ToElement) EXPECT_EQ("/frame", cam2.PoseRelativeTo()); EXPECT_TRUE(cam2.SaveFrames()); EXPECT_EQ("/tmp", cam2.SaveFramesPath()); + + // \todo(iche033) Remove in sdformat16 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION EXPECT_EQ("/optical_frame", cam2.OpticalFrameId()); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + EXPECT_EQ("/camera_info_test", cam2.CameraInfoTopic()); EXPECT_EQ("/trigger_topic_test", cam2.TriggerTopic()); EXPECT_TRUE(cam2.Triggered()); diff --git a/src/Converter.cc b/src/Converter.cc index 8947d3333..41d550df1 100644 --- a/src/Converter.cc +++ b/src/Converter.cc @@ -264,7 +264,7 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem, } else if (name == "copy") { - Move(_elem, childElem, true); + Move(_elem, childElem, true, _errors); } else if (name == "map") { @@ -272,7 +272,7 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem, } else if (name == "move") { - Move(_elem, childElem, false); + Move(_elem, childElem, false, _errors); } else if (name == "add") { @@ -923,7 +923,8 @@ void Converter::Map(tinyxml2::XMLElement *_elem, ///////////////////////////////////////////////// void Converter::Move(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_moveElem, - const bool _copy) + const bool _copy, + sdf::Errors &_errors) { SDF_ASSERT(_elem != NULL, "SDF element is NULL"); SDF_ASSERT(_moveElem != NULL, "Move element is NULL"); @@ -1024,7 +1025,8 @@ void Converter::Move(tinyxml2::XMLElement *_elem, if (toElemStr && !toAttrStr) { - tinyxml2::XMLNode *cloned = DeepClone(moveFrom->GetDocument(), moveFrom); + tinyxml2::XMLNode *cloned = DeepClone(_errors, moveFrom->GetDocument(), + moveFrom); tinyxml2::XMLElement *moveTo = static_cast(cloned); moveTo->SetValue(toName); diff --git a/src/Converter.hh b/src/Converter.hh index e992b124c..cfba5413f 100644 --- a/src/Converter.hh +++ b/src/Converter.hh @@ -108,9 +108,11 @@ namespace sdf /// \param[in] _moveElem A 'convert' element that describes the move /// operation. /// \param[in] _copy True to copy the element + /// \param[out] _errors Vector of errors. private: static void Move(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_moveElem, - const bool _copy); + const bool _copy, + sdf::Errors &_errors); /// \brief Add an element or attribute to an element. /// \param[in] _elem The element to receive the value. diff --git a/src/Converter_TEST.cc b/src/Converter_TEST.cc index 443774017..d56cba30a 100644 --- a/src/Converter_TEST.cc +++ b/src/Converter_TEST.cc @@ -3370,7 +3370,8 @@ TEST(Converter, World_17_to_18) ASSERT_TRUE(errors.empty()); // Compare converted xml with expected - std::string convertedXmlStr = ElementToString(xmlDoc.RootElement()); + std::string convertedXmlStr = ElementToString(errors, xmlDoc.RootElement()); + ASSERT_TRUE(errors.empty()); ASSERT_FALSE(convertedXmlStr.empty()); std::string expectedXmlStr = R"( @@ -3393,7 +3394,9 @@ TEST(Converter, World_17_to_18) tinyxml2::XMLDocument expectedXmlDoc; expectedXmlDoc.Parse(expectedXmlStr.c_str()); - EXPECT_EQ(ElementToString(expectedXmlDoc.RootElement()), convertedXmlStr); + EXPECT_EQ(ElementToString(errors, expectedXmlDoc.RootElement()), + convertedXmlStr); + ASSERT_TRUE(errors.empty()); // Check some basic elements tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); @@ -3509,7 +3512,8 @@ TEST(Converter, World_17_to_18) ASSERT_TRUE(errors.empty()); // Compare converted xml with expected - convertedXmlStr = ElementToString(xmlDoc.RootElement()); + convertedXmlStr = ElementToString(errors, xmlDoc.RootElement()); + ASSERT_TRUE(errors.empty()); ASSERT_FALSE(convertedXmlStr.empty()); expectedXmlStr = R"( @@ -3577,7 +3581,9 @@ TEST(Converter, World_17_to_18) expectedXmlDoc.Clear(); expectedXmlDoc.Parse(expectedXmlStr.c_str()); - EXPECT_EQ(ElementToString(expectedXmlDoc.RootElement()), convertedXmlStr); + EXPECT_EQ(ElementToString(errors, expectedXmlDoc.RootElement()), + convertedXmlStr); + ASSERT_TRUE(errors.empty()); // ------- Another flattened world in 1.7 format @@ -3616,7 +3622,8 @@ TEST(Converter, World_17_to_18) EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Compare converted xml with expected - convertedXmlStr = ElementToString(xmlDoc.RootElement()); + convertedXmlStr = ElementToString(errors, xmlDoc.RootElement()); + ASSERT_TRUE(errors.empty()); ASSERT_FALSE(convertedXmlStr.empty()); expectedXmlStr = R"( @@ -3655,7 +3662,9 @@ TEST(Converter, World_17_to_18) expectedXmlDoc.Clear(); expectedXmlDoc.Parse(expectedXmlStr.c_str()); - EXPECT_EQ(ElementToString(expectedXmlDoc.RootElement()), convertedXmlStr); + EXPECT_EQ(ElementToString(errors, expectedXmlDoc.RootElement()), + convertedXmlStr); + ASSERT_TRUE(errors.empty()); // Check some basic elements convertedElem = xmlDoc.FirstChildElement(); diff --git a/src/Element.cc b/src/Element.cc index 9ded14004..fd8950d7f 100644 --- a/src/Element.cc +++ b/src/Element.cc @@ -671,7 +671,8 @@ void ElementPrivate::PrintAttributes(sdf::Errors &_errors, // better separation of concerns if the conversion process set the // required attributes with their default values. if ((*aiter)->GetSet() || (*aiter)->GetRequired() || - _includeDefaultAttributes) + _includeDefaultAttributes || + ((*aiter)->GetKey().find(':') != std::string::npos)) { const std::string key = (*aiter)->GetKey(); const auto it = attributeExceptions.find(key); diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index e4d0aedad..40df3c4c4 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -77,6 +77,8 @@ FindSourceVertex(const ScopedGraph &_graph, using DirectedEdge = typename ScopedGraph::Edge; using Vertex = typename ScopedGraph::Vertex; using VertexId = gz::math::graph::VertexId; + using VertexType = typename ScopedGraph::VertexType; + using gz::math::graph::NullVertex; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; @@ -86,7 +88,7 @@ FindSourceVertex(const ScopedGraph &_graph, _errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "Unable to resolve pose, invalid vertex[" + std::to_string(_id) + "] " "in PoseRelativeToGraph."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } if (_id == _graph.ScopeVertexId()) @@ -106,7 +108,7 @@ FindSourceVertex(const ScopedGraph &_graph, _errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error: multiple incoming edges to " "current vertex [" + vertex.get().Name() + "]."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } auto const &edge = incidentsTo.begin()->second; vertex = _graph.Graph().VertexFromId(edge.get().Vertices().first); @@ -116,7 +118,7 @@ FindSourceVertex(const ScopedGraph &_graph, _errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, "PoseRelativeToGraph cycle detected, already visited vertex [" + vertex.get().Name() + "]."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } if (vertex.get().Id() == _graph.ScopeVertexId()) { @@ -129,7 +131,7 @@ FindSourceVertex(const ScopedGraph &_graph, if (vertex.get().Id() != _graph.ScopeVertexId()) { // Error, the root vertex is not the same as the the source - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } return PairType(vertex, edges); @@ -157,6 +159,8 @@ FindSinkVertex( using DirectedEdge = typename ScopedGraph::Edge; using Vertex = typename ScopedGraph::Vertex; using VertexId = gz::math::graph::VertexId; + using VertexType = typename ScopedGraph::VertexType; + using gz::math::graph::NullVertex; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; @@ -166,7 +170,7 @@ FindSinkVertex( _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "Invalid vertex[" + std::to_string(_id) + "] " "in FrameAttachedToGraph."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } std::set visited; @@ -180,7 +184,7 @@ FindSinkVertex( _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: multiple outgoing edges from " "current vertex [" + vertex.get().Name() + "]."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } auto const &edge = incidentsFrom.begin()->second; vertex = _graph.Graph().VertexFromId(edge.get().Vertices().second); @@ -190,7 +194,7 @@ FindSinkVertex( _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_CYCLE, "FrameAttachedToGraph cycle detected, already visited vertex [" + vertex.get().Name() + "]."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } visited.insert(vertex.get().Id()); incidentsFrom = _graph.Graph().IncidentsFrom(vertex); diff --git a/src/InstallationDirectories.cc b/src/InstallationDirectories.cc new file mode 100644 index 000000000..a260a32b4 --- /dev/null +++ b/src/InstallationDirectories.cc @@ -0,0 +1,130 @@ +/* + * Copyright (C) 2024 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 + +namespace sdf +{ +inline namespace SDF_VERSION_NAMESPACE { + +// We locally import the gz::common::joinPaths function +// See https://github.com/gazebosim/gz-physics/pull/507#discussion_r1186919267 +// for more details + +// Function imported from +// https://github.com/gazebosim/gz-common/blob/ignition-common4_4.6.2/src/FilesystemBoost.cc#L507 +#ifndef WIN32 +static const char preferred_separator = '/'; +#else // Windows +static const char preferred_separator = '\\'; +#endif +const std::string separator(const std::string &_p) +{ + return _p + preferred_separator; +} + +// Function imported from +// https://github.com/gazebosim/gz-common/blob/ignition-common4_4.6.2/src/Filesystem.cc#L227 +std::string checkWindowsPath(const std::string _path) +{ + if (_path.empty()) + return _path; + + // Check if this is a http or https, if so change backslashes generated by + // jointPaths to '/' + if ((_path.size() > 7 && 0 == _path.compare(0, 7, "http://")) || + (_path.size() > 8 && 0 == _path.compare(0, 8, "https://"))) + { + return std::regex_replace(_path, std::regex(R"(\\)"), "/"); + } + + // This is a Windows path, convert all '/' into backslashes + std::string result = std::regex_replace(_path, std::regex(R"(/)"), "\\"); + std::string drive_letters; + + // only Windows contains absolute paths starting with drive letters + if (result.length() > 3 && 0 == result.compare(1, 2, ":\\")) + { + drive_letters = result.substr(0, 3); + result = result.substr(3); + } + result = drive_letters + std::regex_replace( + result, std::regex("[<>:\"|?*]"), ""); + return result; +} + +// Function imported from +// https://github.com/gazebosim/gz-common/blob/gz-common5_5.6.0/src/Filesystem.cc#L142 +std::string joinPaths(const std::string &_path1, + const std::string &_path2) +{ + namespace fs = std::filesystem; + fs::path p1{_path1}; + fs::path p2{_path2}; + + if (p1.empty()) + { + p1 = std::string{fs::path::preferred_separator}; + } + + bool is_url = false; + + if (_path1.find("://") == std::string::npos) + p1 = p1.lexically_normal(); + else + is_url = true; + + // TODO(mjcarroll) Address the case that path2 is also a URI. + // It's likely not a valid scenario, but not currently covered by our test + // suite and doesn't return an error. + if (_path2.find("://") == std::string::npos) + p2 = p2.lexically_normal(); + else + is_url = true; + + if (p2.string()[0] == fs::path::preferred_separator) + { + p2 = fs::path{p2.string().substr(1)}; + } + + auto ret = (p1 / p2); + + if (is_url) + { + std::string path = ret.string(); + std::replace(path.begin(), path.end(), + static_cast(fs::path::preferred_separator), '/'); + return path; + } + else + { + return ret.lexically_normal().string(); + } +} + +std::string getSharePath() +{ + return sdf::joinPaths( + getInstallPrefix(), CMAKE_INSTALL_RELATIVE_DATAROOTDIR); +} + +} +} diff --git a/src/Joint.cc b/src/Joint.cc index 2b8fd8282..93cc5900c 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -306,30 +306,6 @@ void Joint::SetChildName(const std::string &_name) this->dataPtr->childName = _name; } -///////////////////////////////////////////////// -const std::string &Joint::ParentLinkName() const -{ - return this->ParentName(); -} - -///////////////////////////////////////////////// -void Joint::SetParentLinkName(const std::string &_name) -{ - this->SetParentName(_name); -} - -///////////////////////////////////////////////// -const std::string &Joint::ChildLinkName() const -{ - return this->ChildName(); -} - -///////////////////////////////////////////////// -void Joint::SetChildLinkName(const std::string &_name) -{ - this->SetChildName(_name); -} - ///////////////////////////////////////////////// const JointAxis *Joint::Axis(const unsigned int _index) const { diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc index f4c23abf9..83217b744 100644 --- a/src/Joint_TEST.cc +++ b/src/Joint_TEST.cc @@ -30,10 +30,6 @@ TEST(DOMJoint, Construction) EXPECT_TRUE(joint.ParentName().empty()); EXPECT_TRUE(joint.ChildName().empty()); - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - EXPECT_TRUE(joint.ParentLinkName().empty()); - EXPECT_TRUE(joint.ChildLinkName().empty()); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION EXPECT_EQ(gz::math::Pose3d::Zero, joint.RawPose()); EXPECT_TRUE(joint.PoseRelativeTo().empty()); @@ -65,18 +61,6 @@ TEST(DOMJoint, Construction) joint.SetName("test_joint"); EXPECT_EQ("test_joint", joint.Name()); - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - joint.SetParentLinkName("parent"); - EXPECT_EQ("parent", joint.ParentLinkName()); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - EXPECT_EQ("parent", joint.ParentName()); - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - joint.SetChildLinkName("child"); - EXPECT_EQ("child", joint.ChildLinkName()); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - EXPECT_EQ("child", joint.ChildName()); - joint.SetParentName("parent2"); EXPECT_EQ("parent2", joint.ParentName()); diff --git a/src/Link.cc b/src/Link.cc index 06de2fff8..19c3569fe 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -87,6 +87,9 @@ class sdf::Link::Implementation /// \brief True if this link should be subject to wind, false otherwise. public: bool enableWind = false; + /// \brief True if this link should be subject to gravity, false otherwise. + public: bool enableGravity = true; + /// \brief True if this link is kinematic only public: bool kinematic = false; @@ -316,6 +319,9 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) this->dataPtr->enableWind = _sdf->Get("enable_wind", this->dataPtr->enableWind).first; + this->dataPtr->enableGravity = _sdf->Get("gravity", + this->dataPtr->enableGravity).first; + this->dataPtr->kinematic = _sdf->Get("kinematic", this->dataPtr->kinematic).first; @@ -865,12 +871,24 @@ bool Link::EnableWind() const return this->dataPtr->enableWind; } +///////////////////////////////////////////////// +bool Link::EnableGravity() const +{ + return this->dataPtr->enableGravity; +} + ///////////////////////////////////////////////// void Link::SetEnableWind(const bool _enableWind) { this->dataPtr->enableWind = _enableWind; } +///////////////////////////////////////////////// +void Link::SetEnableGravity(bool _enableGravity) +{ + this->dataPtr->enableGravity = _enableGravity; +} + ///////////////////////////////////////////////// bool Link::Kinematic() const { diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index ed4853fe8..7a9207dd0 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -72,6 +72,10 @@ TEST(DOMLink, Construction) link.SetEnableWind(true); EXPECT_TRUE(link.EnableWind()); + EXPECT_TRUE(link.EnableGravity()); + link.SetEnableGravity(false); + EXPECT_FALSE(link.EnableGravity()); + EXPECT_FALSE(link.Kinematic()); link.SetKinematic(true); EXPECT_TRUE(link.Kinematic()); diff --git a/src/ParamPassing.cc b/src/ParamPassing.cc index 1b80d09cf..5bf0c97a9 100644 --- a/src/ParamPassing.cc +++ b/src/ParamPassing.cc @@ -52,7 +52,7 @@ void updateParams(const ParserConfig &_config, _errors.push_back({ErrorCode::ATTRIBUTE_MISSING, "Element identifier requires an element_id attribute, but the " "element_id is not set. Skipping element alteration:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -67,7 +67,7 @@ void updateParams(const ParserConfig &_config, _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, "Missing name after double colons in element identifier. " "Skipping element alteration:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -83,7 +83,7 @@ void updateParams(const ParserConfig &_config, { _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, "Action [" + actionStr + "] is not a valid action. Skipping " - "element alteration:\n" + ElementToString(childElemXml) + "element alteration:\n" + ElementToString(_errors, childElemXml) }); continue; } @@ -102,7 +102,7 @@ void updateParams(const ParserConfig &_config, _errors.push_back({ErrorCode::ATTRIBUTE_MISSING, "Element to be added is missing a 'name' attribute. " "Skipping element addition:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -112,7 +112,7 @@ void updateParams(const ParserConfig &_config, { _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, "The 'name' attribute can not be empty. Skipping element addition:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -129,7 +129,7 @@ void updateParams(const ParserConfig &_config, + " element_id='" + childElemXml->Attribute("element_id") + "'> because element already exists in included model. " + "Skipping element addition:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -157,7 +157,8 @@ void updateParams(const ParserConfig &_config, _errors.push_back({ErrorCode::ELEMENT_MISSING, "Could not find element <" + std::string(childElemXml->Name()) + " element_id='" + childElemXml->Attribute("element_id") + "'>. " + - "Skipping element modification:\n" + ElementToString(childElemXml) + "Skipping element modification:\n" + + ElementToString(_errors, childElemXml) }); continue; } @@ -194,7 +195,7 @@ void updateParams(const ParserConfig &_config, { _errors.push_back({ErrorCode::ELEMENT_INVALID, "Unable to convert XML to SDF. Skipping element replacement:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -355,7 +356,7 @@ ElementPtr initElementDescription(const tinyxml2::XMLElement *_xml, _errors.push_back({ErrorCode::ELEMENT_INVALID, "Element [" + std::string(_xml->Name()) + "] is not a defined " "SDF element. Skipping element alteration\n: " - + ElementToString(_xml) + + ElementToString(_errors, _xml) }); return nullptr; } @@ -384,7 +385,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Missing an action attribute. Skipping child element modification " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -397,7 +398,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "child element modification with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -411,7 +412,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Could not find element. Skipping child element removal " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); } else @@ -430,7 +431,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Could not find element. Skipping child element modification " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); } else @@ -455,7 +456,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "child element modification with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -468,7 +469,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Unable to convert XML to SDF. Skipping child element alteration " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -486,7 +487,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Could not find element. Skipping child element replacement " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -498,7 +499,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Replacement element is missing a 'name' attribute. " "Skipping element replacement <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) - + "'>:\n" + ElementToString(xmlChild) + + "'>:\n" + ElementToString(_errors, xmlChild) }); continue; } @@ -525,7 +526,7 @@ void add(const ParserConfig &_config, const std::string &_source, { _errors.push_back({ErrorCode::ELEMENT_INVALID, "Unable to convert XML to SDF. Skipping element addition:\n" - + ElementToString(_childXml) + + ElementToString(_errors, _childXml) }); } } @@ -557,7 +558,8 @@ void modifyAttributes(tinyxml2::XMLElement *_xml, { _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, "Attribute [" + attrName + "] is invalid. " - "Skipping attribute modification in:\n" + ElementToString(_xml) + "Skipping attribute modification in:\n" + + ElementToString(_errors, _xml) }); continue; } @@ -582,7 +584,7 @@ void modifyChildren(tinyxml2::XMLElement *_xml, { _errors.push_back({ErrorCode::ELEMENT_MISSING, "Could not find element [" + elemName + "]. " - "Skipping modification for:\n" + ElementToString(_xml) + "Skipping modification for:\n" + ElementToString(_errors, _xml) }); continue; } @@ -599,7 +601,7 @@ void modifyChildren(tinyxml2::XMLElement *_xml, _errors.push_back({ErrorCode::ELEMENT_INVALID, "Value [" + std::string(xmlChild->GetText()) + "] for element [" + elemName + "] is invalid. Skipping modification for:\n" - + ElementToString(_xml) + + ElementToString(_errors, _xml) }); continue; } @@ -620,9 +622,9 @@ void modifyChildren(tinyxml2::XMLElement *_xml, // sdf has child elements but no children were specified in xml std::stringstream ss; ss << "No modifications for element " - << ElementToString(xmlChild) + << ElementToString(_errors, xmlChild) << " provided, skipping modification for:\n" - << ElementToString(_xml); + << ElementToString(_errors, _xml); Error err(ErrorCode::WARNING, ss.str()); enforceConfigurablePolicyCondition( _config.WarningsPolicy(), err, _errors); @@ -650,7 +652,7 @@ void modify(tinyxml2::XMLElement *_xml, const sdf::ParserConfig &_config, _errors.push_back({ErrorCode::ELEMENT_INVALID, "Value [" + std::string(_xml->GetText()) + "] for element [" + std::string(_xml->Name()) + "] is invalid. Skipping modification for:\n" - + ElementToString(_xml) + + ElementToString(_errors, _xml) }); } } @@ -688,7 +690,7 @@ void remove(const tinyxml2::XMLElement *_xml, const sdf::ParserConfig &_config, + std::string(xmlParent->Name()) + " element_id='" + std::string(xmlParent->Attribute("element_id")) + "'> with parent <" + std::string(_xml->Name()) + ">:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } diff --git a/src/ParticleEmitter.cc b/src/ParticleEmitter.cc index 75c7ac438..b0d07a205 100644 --- a/src/ParticleEmitter.cc +++ b/src/ParticleEmitter.cc @@ -34,13 +34,12 @@ using namespace sdf; /// Particle emitter type strings. These should match the data in /// `enum class ParticleEmitterType` located in ParticleEmitter.hh, and the size /// template parameter should match the number of elements as well. -constexpr std::array kEmitterTypeStrs = +constexpr std::array kEmitterTypeStrs = { "point", "box", "cylinder", - "ellipsoid", - "cone", + "ellipsoid" }; class sdf::ParticleEmitter::Implementation diff --git a/src/ParticleEmitter_TEST.cc b/src/ParticleEmitter_TEST.cc index a8970b9be..121cf2808 100644 --- a/src/ParticleEmitter_TEST.cc +++ b/src/ParticleEmitter_TEST.cc @@ -35,8 +35,6 @@ TEST(DOMParticleEmitter, Construction) EXPECT_TRUE(emitter.SetType("box")); EXPECT_EQ("box", emitter.TypeStr()); EXPECT_EQ(sdf::ParticleEmitterType::BOX, emitter.Type()); - emitter.SetType(sdf::ParticleEmitterType::CONE); - EXPECT_EQ("cone", emitter.TypeStr()); emitter.SetType(sdf::ParticleEmitterType::CYLINDER); EXPECT_EQ("cylinder", emitter.TypeStr()); diff --git a/src/SDF.cc b/src/SDF.cc index 7a43d0f9a..d9fa57dbd 100644 --- a/src/SDF.cc +++ b/src/SDF.cc @@ -27,11 +27,14 @@ #include "sdf/parser.hh" #include "sdf/Assert.hh" #include "sdf/Console.hh" +#include "sdf/Error.hh" #include "sdf/Filesystem.hh" +#include "sdf/InstallationDirectories.hh" #include "sdf/SDFImpl.hh" #include "SDFImplPrivate.hh" #include "sdf/sdf_config.h" #include "EmbeddedSdf.hh" +#include "Utils.hh" #include @@ -45,11 +48,15 @@ std::string SDF::version = SDF_VERSION; // NOLINT(runtime/string) std::string sdfSharePath() { -#ifdef SDF_SHARE_PATH - if (std::string(SDF_SHARE_PATH) != "/") - return SDF_SHARE_PATH; -#endif - return ""; + std::string sharePath = sdf::getSharePath(); + if (sharePath != "/") + { + return sharePath; + } + else + { + return ""; + } } ///////////////////////////////////////////////// @@ -61,14 +68,40 @@ void setFindCallback(std::function _cb) ///////////////////////////////////////////////// std::string findFile( const std::string &_filename, bool _searchLocalPath, bool _useCallback) +{ + sdf::Errors errors; + std::string result = findFile(errors, _filename, _searchLocalPath, + _useCallback, ParserConfig::GlobalConfig()); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +std::string findFile( + sdf::Errors &_errors, const std::string &_filename, bool _searchLocalPath, + bool _useCallback) { return findFile( - _filename, _searchLocalPath, _useCallback, ParserConfig::GlobalConfig()); + _errors, _filename, _searchLocalPath, + _useCallback, ParserConfig::GlobalConfig()); } ///////////////////////////////////////////////// std::string findFile(const std::string &_filename, bool _searchLocalPath, bool _useCallback, const ParserConfig &_config) +{ + sdf::Errors errors; + std::string result = findFile(errors, _filename, _searchLocalPath, + _useCallback, _config); + sdf::throwOrPrintErrors(errors); + return result; +} + + +///////////////////////////////////////////////// +std::string findFile(sdf::Errors &_errors, const std::string &_filename, + bool _searchLocalPath, bool _useCallback, + const ParserConfig &_config) { // Check to see if _filename is URI. If so, resolve the URI path. for (const auto &[uriScheme, paths] : _config.URIPathMap()) @@ -162,8 +195,9 @@ std::string findFile(const std::string &_filename, bool _searchLocalPath, { if (!_config.FindFileCallback()) { - sdferr << "Tried to use callback in sdf::findFile(), but the callback " - "is empty. Did you call sdf::setFindCallback()?\n"; + _errors.push_back({sdf::ErrorCode::FILE_READ, + "Tried to use callback in sdf::findFile(), but the callback " + "is empty. Did you call sdf::setFindCallback()?"}); return std::string(); } else @@ -195,13 +229,29 @@ SDF::~SDF() ///////////////////////////////////////////////// void SDF::PrintDescription() { - this->Root()->PrintDescription(""); + sdf::Errors errors; + this->PrintDescription(errors); + sdf::throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void SDF::PrintDescription(sdf::Errors &_errors) +{ + this->Root()->PrintDescription(_errors, ""); } ///////////////////////////////////////////////// void SDF::PrintValues(const PrintConfig &_config) { - this->Root()->PrintValues("", _config); + sdf::Errors errors; + this->PrintValues(errors, _config); + sdf::throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void SDF::PrintValues(sdf::Errors &_errors, const PrintConfig &_config) +{ + this->Root()->PrintValues(_errors, "", _config); } ///////////////////////////////////////////////// @@ -319,13 +369,22 @@ void SDF::PrintDoc() ///////////////////////////////////////////////// void SDF::Write(const std::string &_filename) { - std::string string = this->Root()->ToString(""); + sdf::Errors errors; + this->Write(errors, _filename); + sdf::throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void SDF::Write(sdf::Errors &_errors, const std::string &_filename) +{ + std::string string = this->Root()->ToString(_errors, ""); std::ofstream out(_filename.c_str(), std::ios::out); if (!out) { - sdferr << "Unable to open file[" << _filename << "] for writing\n"; + _errors.push_back({sdf::ErrorCode::FILE_READ, + "Unable to open file[" + _filename + "] for writing."}); return; } out << string; @@ -334,6 +393,16 @@ void SDF::Write(const std::string &_filename) ///////////////////////////////////////////////// std::string SDF::ToString(const PrintConfig &_config) const +{ + sdf::Errors errors; + std::string result = this->ToString(errors, _config); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +std::string SDF::ToString(sdf::Errors &_errors, + const PrintConfig &_config) const { std::ostringstream stream; @@ -343,7 +412,7 @@ std::string SDF::ToString(const PrintConfig &_config) const stream << "\n"; } - stream << this->Root()->ToString("", _config); + stream << this->Root()->ToString(_errors, "", _config); if (this->Root()->GetName() != "sdf") { @@ -355,11 +424,21 @@ std::string SDF::ToString(const PrintConfig &_config) const ///////////////////////////////////////////////// void SDF::SetFromString(const std::string &_sdfData) +{ + sdf::Errors errors; + this->SetFromString(errors, _sdfData); + sdf::throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void SDF::SetFromString(sdf::Errors &_errors, + const std::string &_sdfData) { sdf::initFile("root.sdf", this->Root()); - if (!sdf::readString(_sdfData, this->Root())) + if (!sdf::readString(_sdfData, this->Root(), _errors)) { - sdferr << "Unable to parse sdf string[" << _sdfData << "]\n"; + _errors.push_back({sdf::ErrorCode::PARSING_ERROR, + "Unable to parse sdf string[" + _sdfData + "]"}); } } @@ -383,12 +462,6 @@ void SDF::SetRoot(const ElementPtr _root) this->dataPtr->root = _root; } -///////////////////////////////////////////////// -void SDF::Root(const ElementPtr _root) -{ - this->SetRoot(_root); -} - ///////////////////////////////////////////////// std::string SDF::FilePath() const { @@ -429,12 +502,21 @@ void SDF::Version(const std::string &_version) ///////////////////////////////////////////////// ElementPtr SDF::WrapInRoot(const ElementPtr &_sdf) +{ + sdf::Errors errors; + ElementPtr result = SDF::WrapInRoot(errors, _sdf); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +ElementPtr SDF::WrapInRoot(sdf::Errors &_errors, const ElementPtr &_sdf) { ElementPtr root(new Element); root->SetName("sdf"); std::stringstream v; v << Version(); - root->AddAttribute("version", "string", v.str(), true, "version"); + root->AddAttribute("version", "string", v.str(), true, _errors, "version"); root->InsertElement(_sdf->Clone()); return root; } @@ -442,6 +524,19 @@ ElementPtr SDF::WrapInRoot(const ElementPtr &_sdf) ///////////////////////////////////////////////// const std::string &SDF::EmbeddedSpec( const std::string &_filename, const bool _quiet) +{ + sdf::Errors errors; + const std::string &result = EmbeddedSpec(errors, _filename); + if (!_quiet) + { + sdf::throwOrPrintErrors(errors); + } + return result; +} + +///////////////////////////////////////////////// +const std::string &SDF::EmbeddedSpec( + sdf::Errors &_errors, const std::string &_filename) { try { @@ -450,9 +545,9 @@ const std::string &SDF::EmbeddedSpec( } catch(const std::out_of_range &) { - if (!_quiet) - sdferr << "Unable to find SDF filename[" << _filename << "] with " - << "version " << SDF::Version() << "\n"; + _errors.push_back({sdf::ErrorCode::FILE_READ, + "Unable to find SDF filename[" + _filename + "] with " + + "version " + SDF::Version()}); } // An empty SDF string is returned if a query into the embeddedSdf map fails. diff --git a/src/SDF_TEST.cc b/src/SDF_TEST.cc index 44bb9f284..6b81db9f9 100644 --- a/src/SDF_TEST.cc +++ b/src/SDF_TEST.cc @@ -290,14 +290,6 @@ TEST(SDF, SetRoot) elem->AddValue("bool", "true", false, "description"); s.SetRoot(elem); EXPECT_EQ(elem, s.Root()); - - // Test deprecated setter, remove in libsdformat14 - s.SetRoot(nullptr); - EXPECT_EQ(nullptr, s.Root()); - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - s.Root(elem); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - EXPECT_EQ(elem, s.Root()); } //////////////////////////////////////////////////// @@ -742,6 +734,46 @@ bool create_new_temp_dir(std::string &_new_temp_path) return true; } +///////////////////////////////////////////////// +TEST(SDF, ErrorOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + sdf::Errors errors; + + // Test findFile + EXPECT_EQ(sdf::findFile(errors, "adfjialkas31", false, true), ""); + EXPECT_EQ(errors.size(), 1) << errors; + EXPECT_NE(std::string::npos, + errors[0].Message().find("Tried to use callback in sdf::findFile(), " + "but the callback is empty. Did you call " + "sdf::setFindCallback()?")) + << errors[0].Message(); + errors.clear(); + + sdf::SDF sdf; + sdf.SetFromString(errors, "banana"); + EXPECT_EQ(errors.size(), 2) << errors; + EXPECT_NE(std::string::npos, + errors[0].Message().find("Error parsing XML from string")) + << errors[0].Message(); + EXPECT_NE(std::string::npos, + errors[1].Message().find("Unable to parse sdf string[banana]")) + << errors[1].Message(); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +} + ///////////////////////////////////////////////// bool g_findFileCbCalled = false; std::string findFileCb(const std::string &) diff --git a/src/Sensor.cc b/src/Sensor.cc index 78ac7087b..8f3b12055 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -82,6 +82,9 @@ class sdf::Sensor::Implementation /// \brief Name of the sensor. public: std::string name = ""; + /// \brief Sensor frame ID + public: std::string frameId{""}; + /// \brief Sensor data topic. public: std::string topic = ""; @@ -243,7 +246,8 @@ Errors Sensor::Load(ElementPtr _sdf) "The supplied sensor name [" + this->dataPtr->name + "] is reserved."}); } - + this->dataPtr->frameId = _sdf->Get("frame_id", + this->dataPtr->frameId).first; this->dataPtr->updateRate = _sdf->Get("update_rate", this->dataPtr->updateRate).first; this->dataPtr->topic = _sdf->Get("topic"); @@ -439,6 +443,18 @@ void Sensor::SetName(const std::string &_name) this->dataPtr->name = _name; } +///////////////////////////////////////////////// +std::string Sensor::FrameId() const +{ + return this->dataPtr->frameId; +} + +///////////////////////////////////////////////// +void Sensor::SetFrameId(const std::string &_frameId) +{ + this->dataPtr->frameId = _frameId; +} + ///////////////////////////////////////////////// std::string Sensor::Topic() const { @@ -750,6 +766,7 @@ sdf::ElementPtr Sensor::ToElement(sdf::Errors &_errors) const } poseElem->Set(this->RawPose()); + elem->GetElement("frame_id")->Set(this->FrameId()); elem->GetElement("topic")->Set(this->Topic()); elem->GetElement("update_rate")->Set(this->UpdateRate()); elem->GetElement("enable_metrics")->Set(this->EnableMetrics()); diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index 8596cabce..482c6f1f9 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -60,6 +60,9 @@ TEST(DOMSensor, Construction) EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } + EXPECT_TRUE(sensor.FrameId().empty()); + sensor.SetFrameId("new_frame_id"); + EXPECT_EQ("new_frame_id", sensor.FrameId()); EXPECT_DOUBLE_EQ(0.0, sensor.UpdateRate()); @@ -76,6 +79,7 @@ TEST(DOMSensor, MoveConstructor) sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetPoseRelativeTo("a_frame"); sensor.SetUpdateRate(0.123); + sensor.SetFrameId("new_frame_id"); sdf::Noise noise; noise.SetMean(0.1); @@ -88,6 +92,7 @@ TEST(DOMSensor, MoveConstructor) EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); + EXPECT_EQ("new_frame_id", sensor2.FrameId()); ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor2.MagnetometerSensor()->XNoise().Mean()); @@ -102,6 +107,7 @@ TEST(DOMSensor, CopyConstructor) sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetPoseRelativeTo("a_frame"); sensor.SetUpdateRate(0.123); + sensor.SetFrameId("new_frame_id"); sdf::Noise noise; noise.SetMean(0.1); @@ -114,6 +120,7 @@ TEST(DOMSensor, CopyConstructor) EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor.RawPose()); EXPECT_EQ("a_frame", sensor.PoseRelativeTo()); + EXPECT_EQ("new_frame_id", sensor.FrameId()); ASSERT_TRUE(nullptr != sensor.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor.MagnetometerSensor()->XNoise().Mean()); @@ -121,6 +128,7 @@ TEST(DOMSensor, CopyConstructor) EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); + EXPECT_EQ("new_frame_id", sensor2.FrameId()); ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor2.MagnetometerSensor()->XNoise().Mean()); @@ -134,6 +142,7 @@ TEST(DOMSensor, MoveAssignment) sensor.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetPoseRelativeTo("a_frame"); + sensor.SetFrameId("new_frame_id"); sdf::Noise noise; noise.SetMean(0.1); @@ -147,6 +156,7 @@ TEST(DOMSensor, MoveAssignment) EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); + EXPECT_EQ("new_frame_id", sensor2.FrameId()); ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor2.MagnetometerSensor()->XNoise().Mean()); @@ -159,6 +169,7 @@ TEST(DOMSensor, CopyAssignment) sensor.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetPoseRelativeTo("a_frame"); + sensor.SetFrameId("new_frame_id"); sdf::Noise noise; noise.SetMean(0.1); @@ -172,6 +183,7 @@ TEST(DOMSensor, CopyAssignment) EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor.RawPose()); EXPECT_EQ("a_frame", sensor.PoseRelativeTo()); + EXPECT_EQ("new_frame_id", sensor.FrameId()); ASSERT_TRUE(nullptr != sensor.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor.MagnetometerSensor()->XNoise().Mean()); @@ -179,6 +191,7 @@ TEST(DOMSensor, CopyAssignment) EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); + EXPECT_EQ("new_frame_id", sensor.FrameId()); ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor2.MagnetometerSensor()->XNoise().Mean()); @@ -191,11 +204,13 @@ TEST(DOMSensor, CopyAssignmentAfterMove) sensor1.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); sensor1.SetType(sdf::SensorType::MAGNETOMETER); sensor1.SetPoseRelativeTo("frame1"); + sensor1.SetFrameId("new_frame_id"); sdf::Sensor sensor2; sensor2.SetRawPose(gz::math::Pose3d(4, 5, 6, 0, 0, 0)); sensor2.SetType(sdf::SensorType::CAMERA); sensor2.SetPoseRelativeTo("frame2"); + sensor2.SetFrameId("new_frame_id2"); // This is similar to what std::swap does except it uses std::move for each // assignment @@ -206,10 +221,12 @@ TEST(DOMSensor, CopyAssignmentAfterMove) EXPECT_EQ(sdf::SensorType::CAMERA, sensor1.Type()); EXPECT_EQ(gz::math::Pose3d(4, 5, 6, 0, 0, 0), sensor1.RawPose()); EXPECT_EQ("frame2", sensor1.PoseRelativeTo()); + EXPECT_EQ("new_frame_id2", sensor1.FrameId()); EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("frame1", sensor2.PoseRelativeTo()); + EXPECT_EQ("new_frame_id", sensor2.FrameId()); } ///////////////////////////////////////////////// @@ -509,6 +526,7 @@ TEST(DOMSensor, ToElement) // test calling ToElement on a DOM object constructed without calling Load sdf::Sensor sensor; sensor.SetName("my_sensor"); + sensor.SetFrameId("my_sensor_frame_id"); sensor.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetPoseRelativeTo("a_frame"); @@ -534,6 +552,7 @@ TEST(DOMSensor, ToElement) sensor2.Load(sensorElem); EXPECT_EQ("my_sensor", sensor2.Name()); + EXPECT_EQ("my_sensor_frame_id", sensor2.FrameId()); EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); diff --git a/src/Surface.cc b/src/Surface.cc index 5bc84970d..0d10e4e9e 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -15,6 +15,9 @@ * */ +#include +#include + #include "sdf/Element.hh" #include "sdf/parser.hh" #include "sdf/Surface.hh" @@ -64,18 +67,74 @@ class sdf::ODE::Implementation public: double slip2 = 0.0; }; +class sdf::BulletFriction::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 friction{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 friction2{1.0}; + + /// \brief Unit vector specifying first friction pyramid direction in + /// collision-fixed reference frame. + public: gz::math::Vector3d fdir1{0, 0, 0}; + + /// \brief Rolling friction coefficient. + public: double rollingFriction{1.0}; +}; + +class sdf::Torsional::Implementation +{ + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf{nullptr}; + + /// \brief Torsional friction coefficient. Uunitless maximum ratio of + /// tangential stress to normal stress. + public: double coefficient{1.0}; + + /// \brief If this flag is true, torsional friction is calculated using the + /// "patch_radius" parameter. If this flag is set to false, + /// "surface_radius" (R) and contact depth (d) are used to compute the patch + /// radius as sqrt(R*d). + public: bool usePatchRadius{true}; + + /// \brief Radius of contact patch surface. + public: double patchRadius{0.0}; + + /// \brief Surface radius on the point of contact. + public: double surfaceRadius{0.0}; + + /// \brief Force dependent slip for torsional friction. + /// equivalent to inverse of viscous damping coefficient with units of + /// rad/s/(Nm). A slip value of 0 is infinitely viscous. + public: double odeSlip{0.0}; +}; + class sdf::Friction::Implementation { - /// \brief The object storing contact parameters + /// \brief The object storing ode parameters public: sdf::ODE ode; + /// \brief The object storing bullet friction parameters + public: std::optional bullet; + + /// \brief The object storing torsional parameters + public: std::optional torsional; + /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf{nullptr}; }; class sdf::Surface::Implementation { - /// \brief The object storing contact parameters + /// \brief The object storing friction parameters public: sdf::Friction friction; /// \brief The object storing contact parameters @@ -85,6 +144,219 @@ class sdf::Surface::Implementation public: sdf::ElementPtr sdf{nullptr}; }; +///////////////////////////////////////////////// +Torsional::Torsional() + : dataPtr(gz::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors Torsional::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 BulletFriction, 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() != "torsional") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a BulletFriction, but the provided SDF element " + "is not a ."}); + return errors; + } + + this->dataPtr->coefficient = _sdf->Get( + "coefficient", this->dataPtr->coefficient).first; + this->dataPtr->usePatchRadius = _sdf->Get( + "use_patch_radius", this->dataPtr->usePatchRadius).first; + this->dataPtr->patchRadius = _sdf->Get( + "patch_radius", this->dataPtr->patchRadius).first; + this->dataPtr->surfaceRadius = _sdf->Get( + "surface_radius", this->dataPtr->surfaceRadius).first; + + if (_sdf->HasElement("ode")) + { + this->dataPtr->odeSlip = _sdf->GetElement("ode")->Get( + "slip", this->dataPtr->odeSlip).first; + } + + return errors; +} + +///////////////////////////////////////////////// +double Torsional::Coefficient() const +{ + return this->dataPtr->coefficient; +} + +///////////////////////////////////////////////// +void Torsional::SetCoefficient(double _coefficient) +{ + this->dataPtr->coefficient = _coefficient; +} + +///////////////////////////////////////////////// +bool Torsional::UsePatchRadius() const +{ + return this->dataPtr->usePatchRadius; +} + +///////////////////////////////////////////////// +void Torsional::SetUsePatchRadius(bool _usePatchRadius) +{ + this->dataPtr->usePatchRadius = _usePatchRadius; +} + +///////////////////////////////////////////////// +double Torsional::PatchRadius() const +{ + return this->dataPtr->patchRadius; +} + +///////////////////////////////////////////////// +void Torsional::SetPatchRadius(double _radius) +{ + this->dataPtr->patchRadius = _radius; +} + +///////////////////////////////////////////////// +double Torsional::SurfaceRadius() const +{ + return this->dataPtr->surfaceRadius; +} + +///////////////////////////////////////////////// +void Torsional::SetSurfaceRadius(double _radius) +{ + this->dataPtr->surfaceRadius = _radius; +} + +///////////////////////////////////////////////// +double Torsional::ODESlip() const +{ + return this->dataPtr->odeSlip; +} + +///////////////////////////////////////////////// +void Torsional::SetODESlip(double _slip) +{ + this->dataPtr->odeSlip = _slip; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Torsional::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +BulletFriction::BulletFriction() + : dataPtr(gz::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors BulletFriction::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 BulletFriction, 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() != "bullet") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a BulletFriction, but the provided SDF element " + "is not a ."}); + return errors; + } + + this->dataPtr->friction = _sdf->Get( + "friction", this->dataPtr->friction).first; + this->dataPtr->friction2 = _sdf->Get( + "friction2", this->dataPtr->friction2).first; + this->dataPtr->fdir1 = _sdf->Get("fdir1", + this->dataPtr->fdir1).first; + this->dataPtr->rollingFriction = _sdf->Get( + "rolling_friction", this->dataPtr->rollingFriction).first; + + return errors; +} + +///////////////////////////////////////////////// +double BulletFriction::Friction() const +{ + return this->dataPtr->friction; +} + +///////////////////////////////////////////////// +void BulletFriction::SetFriction(double _friction) +{ + this->dataPtr->friction = _friction; +} + +///////////////////////////////////////////////// +double BulletFriction::Friction2() const +{ + return this->dataPtr->friction2; +} + +///////////////////////////////////////////////// +void BulletFriction::SetFriction2(double _friction2) +{ + this->dataPtr->friction2 = _friction2; +} + +///////////////////////////////////////////////// +const gz::math::Vector3d &BulletFriction::Fdir1() const +{ + return this->dataPtr->fdir1; +} + +///////////////////////////////////////////////// +void BulletFriction::SetFdir1(const gz::math::Vector3d &_fdir) +{ + this->dataPtr->fdir1 = _fdir; +} + +///////////////////////////////////////////////// +double BulletFriction::RollingFriction() const +{ + return this->dataPtr->rollingFriction; +} + +///////////////////////////////////////////////// +void BulletFriction::SetRollingFriction(double _rollingFriction) +{ + this->dataPtr->rollingFriction = _rollingFriction; +} + +///////////////////////////////////////////////// +sdf::ElementPtr BulletFriction::Element() const +{ + return this->dataPtr->sdf; +} ///////////////////////////////////////////////// ODE::ODE() @@ -235,6 +507,20 @@ Errors Friction::Load(ElementPtr _sdf) errors.insert(errors.end(), err.begin(), err.end()); } + if (_sdf->HasElement("bullet")) + { + this->dataPtr->bullet.emplace(); + Errors err = this->dataPtr->bullet->Load(_sdf->GetElement("bullet")); + errors.insert(errors.end(), err.begin(), err.end()); + } + + if (_sdf->HasElement("torsional")) + { + this->dataPtr->torsional.emplace(); + Errors err = this->dataPtr->torsional->Load(_sdf->GetElement("torsional")); + errors.insert(errors.end(), err.begin(), err.end()); + } + return errors; } @@ -256,6 +542,30 @@ const sdf::ODE *Friction::ODE() const return &this->dataPtr->ode; } +///////////////////////////////////////////////// +void Friction::SetBulletFriction(const sdf::BulletFriction &_bullet) +{ + this->dataPtr->bullet = _bullet; +} + +///////////////////////////////////////////////// +const sdf::BulletFriction *Friction::BulletFriction() const +{ + return optionalToPointer(this->dataPtr->bullet); +} + +///////////////////////////////////////////////// +void Friction::SetTorsional(const sdf::Torsional &_torsional) +{ + this->dataPtr->torsional = _torsional; +} + +///////////////////////////////////////////////// +const sdf::Torsional *Friction::Torsional() const +{ + return optionalToPointer(this->dataPtr->torsional); +} + ///////////////////////////////////////////////// Contact::Contact() : dataPtr(gz::utils::MakeImpl()) @@ -429,5 +739,34 @@ sdf::ElementPtr Surface::ToElement(sdf::Errors &_errors) const ode->GetElement("fdir1", _errors)->Set( _errors, this->dataPtr->friction.ODE()->Fdir1()); + if (this->dataPtr->friction.BulletFriction()) + { + sdf::ElementPtr bullet = frictionElem->GetElement("bullet"); + bullet->GetElement("friction")->Set( + this->dataPtr->friction.BulletFriction()->Friction()); + bullet->GetElement("friction2")->Set( + this->dataPtr->friction.BulletFriction()->Friction2()); + bullet->GetElement("fdir1")->Set( + this->dataPtr->friction.BulletFriction()->Fdir1()); + bullet->GetElement("rolling_friction")->Set( + this->dataPtr->friction.BulletFriction()->RollingFriction()); + } + + if (this->dataPtr->friction.Torsional()) + { + sdf::ElementPtr torsional = frictionElem->GetElement("torsional"); + torsional->GetElement("coefficient")->Set( + this->dataPtr->friction.Torsional()->Coefficient()); + torsional->GetElement("use_patch_radius")->Set( + this->dataPtr->friction.Torsional()->UsePatchRadius()); + torsional->GetElement("patch_radius")->Set( + this->dataPtr->friction.Torsional()->PatchRadius()); + torsional->GetElement("surface_radius")->Set( + this->dataPtr->friction.Torsional()->SurfaceRadius()); + + torsional->GetElement("ode")->GetElement("slip")->Set( + this->dataPtr->friction.Torsional()->ODESlip()); + } + return elem; } diff --git a/src/Surface_TEST.cc b/src/Surface_TEST.cc index 8498be8f1..a7b2341d6 100644 --- a/src/Surface_TEST.cc +++ b/src/Surface_TEST.cc @@ -16,6 +16,7 @@ */ #include +#include #include "sdf/Surface.hh" #include "test_utils.hh" @@ -150,6 +151,19 @@ TEST(DOMsurface, ToElement) ode.SetSlip2(4); ode.SetFdir1(gz::math::Vector3d(1, 2, 3)); friction.SetODE(ode); + sdf::BulletFriction bullet; + bullet.SetFriction(0.3); + bullet.SetFriction2(0.5); + bullet.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet.SetRollingFriction(1.3); + friction.SetBulletFriction(bullet); + sdf::Torsional torsional; + torsional.SetCoefficient(0.5); + torsional.SetUsePatchRadius(false); + torsional.SetPatchRadius(0.1); + torsional.SetSurfaceRadius(0.3); + torsional.SetODESlip(0.2); + friction.SetTorsional(torsional); contact.SetCollideBitmask(0x12); surface1.SetContact(contact); surface1.SetFriction(friction); @@ -168,6 +182,18 @@ TEST(DOMsurface, ToElement) EXPECT_EQ(surface2.Friction()->ODE()->Fdir1(), gz::math::Vector3d(1, 2, 3)); + EXPECT_DOUBLE_EQ(0.3, surface2.Friction()->BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, surface2.Friction()->BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), + surface2.Friction()->BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, + surface2.Friction()->BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, surface2.Friction()->Torsional()->Coefficient()); + EXPECT_FALSE(surface2.Friction()->Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, surface2.Friction()->Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, surface2.Friction()->Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, surface2.Friction()->Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -287,13 +313,39 @@ TEST(DOMfriction, SetFriction) ode1.SetFdir1(gz::math::Vector3d(1, 2, 3)); sdf::Friction friction1; friction1.SetODE(ode1); - EXPECT_EQ(nullptr, friction1.Element()); + + sdf::BulletFriction bullet; + bullet.SetFriction(0.3); + bullet.SetFriction2(0.5); + bullet.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet.SetRollingFriction(1.3); + friction1.SetBulletFriction(bullet); + + sdf::Torsional torsional; + torsional.SetCoefficient(0.5); + torsional.SetUsePatchRadius(false); + torsional.SetPatchRadius(0.1); + torsional.SetSurfaceRadius(0.3); + torsional.SetODESlip(0.2); + friction1.SetTorsional(torsional); + 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(), gz::math::Vector3d(1, 2, 3)); + + EXPECT_DOUBLE_EQ(0.3, friction1.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, friction1.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), friction1.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, friction1.BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, friction1.Torsional()->Coefficient()); + EXPECT_FALSE(friction1.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, friction1.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, friction1.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, friction1.Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -308,6 +360,21 @@ TEST(DOMfriction, CopyOperator) sdf::Friction friction1; friction1.SetODE(ode1); + sdf::BulletFriction bullet; + bullet.SetFriction(0.3); + bullet.SetFriction2(0.5); + bullet.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet.SetRollingFriction(1.3); + friction1.SetBulletFriction(bullet); + + sdf::Torsional torsional; + torsional.SetCoefficient(0.5); + torsional.SetUsePatchRadius(false); + torsional.SetPatchRadius(0.1); + torsional.SetSurfaceRadius(0.3); + torsional.SetODESlip(0.2); + friction1.SetTorsional(torsional); + sdf::Friction friction2(friction1); EXPECT_DOUBLE_EQ(friction2.ODE()->Mu(), 0.1); EXPECT_DOUBLE_EQ(friction2.ODE()->Mu2(), 0.2); @@ -315,6 +382,17 @@ TEST(DOMfriction, CopyOperator) EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); EXPECT_EQ(friction2.ODE()->Fdir1(), gz::math::Vector3d(1, 2, 3)); + + EXPECT_DOUBLE_EQ(0.3, friction2.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, friction2.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), friction2.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, friction2.BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, friction2.Torsional()->Coefficient()); + EXPECT_FALSE(friction2.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, friction2.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, friction2.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, friction2.Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -329,6 +407,21 @@ TEST(DOMfriction, CopyAssignmentOperator) sdf::Friction friction1; friction1.SetODE(ode1); + sdf::BulletFriction bullet; + bullet.SetFriction(0.3); + bullet.SetFriction2(0.5); + bullet.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet.SetRollingFriction(1.3); + friction1.SetBulletFriction(bullet); + + sdf::Torsional torsional; + torsional.SetCoefficient(0.5); + torsional.SetUsePatchRadius(false); + torsional.SetPatchRadius(0.1); + torsional.SetSurfaceRadius(0.3); + torsional.SetODESlip(0.2); + friction1.SetTorsional(torsional); + sdf::Friction friction2 = friction1; EXPECT_DOUBLE_EQ(friction2.ODE()->Mu(), 0.1); EXPECT_DOUBLE_EQ(friction2.ODE()->Mu2(), 0.2); @@ -336,6 +429,17 @@ TEST(DOMfriction, CopyAssignmentOperator) EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); EXPECT_EQ(friction2.ODE()->Fdir1(), gz::math::Vector3d(1, 2, 3)); + + EXPECT_DOUBLE_EQ(0.3, friction2.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, friction2.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), friction2.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, friction2.BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, friction2.Torsional()->Coefficient()); + EXPECT_FALSE(friction2.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, friction2.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, friction2.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, friction2.Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -359,6 +463,36 @@ TEST(DOMfriction, CopyAssignmentAfterMove) sdf::Friction friction2; friction2.SetODE(ode2); + sdf::BulletFriction bullet1; + bullet1.SetFriction(0.3); + bullet1.SetFriction2(0.5); + bullet1.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet1.SetRollingFriction(1.3); + friction1.SetBulletFriction(bullet1); + + sdf::BulletFriction bullet2; + bullet2.SetFriction(0.1); + bullet2.SetFriction2(0.2); + bullet2.SetFdir1(gz::math::Vector3d(3, 4, 5)); + bullet2.SetRollingFriction(3.1); + friction2.SetBulletFriction(bullet2); + + sdf::Torsional torsional1; + torsional1.SetCoefficient(0.5); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.1); + torsional1.SetSurfaceRadius(0.3); + torsional1.SetODESlip(0.2); + friction1.SetTorsional(torsional1); + + sdf::Torsional torsional2; + torsional2.SetCoefficient(1.5); + torsional2.SetUsePatchRadius(true); + torsional2.SetPatchRadius(1.1); + torsional2.SetSurfaceRadius(3.3); + torsional2.SetODESlip(2.2); + friction2.SetTorsional(torsional2); + sdf::Friction tmp = std::move(friction1); friction1 = friction2; friction2 = tmp; @@ -375,34 +509,26 @@ TEST(DOMfriction, CopyAssignmentAfterMove) EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); EXPECT_EQ(friction2.ODE()->Fdir1(), gz::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(), - gz::math::Vector3d(0, 0, 0)); - ode1.SetMu(0.1); - ode1.SetMu2(0.2); - ode1.SetSlip1(3); - ode1.SetSlip2(4); - ode1.SetFdir1(gz::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(), - gz::math::Vector3d(1, 2, 3)); + EXPECT_DOUBLE_EQ(0.3, friction2.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, friction2.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), friction2.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, friction2.BulletFriction()->RollingFriction()); + EXPECT_DOUBLE_EQ(0.1, friction1.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.2, friction1.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(3, 4, 5), friction1.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(3.1, friction1.BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, friction2.Torsional()->Coefficient()); + EXPECT_FALSE(friction2.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, friction2.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, friction2.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, friction2.Torsional()->ODESlip()); + EXPECT_DOUBLE_EQ(1.5, friction1.Torsional()->Coefficient()); + EXPECT_TRUE(friction1.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(1.1, friction1.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(3.3, friction1.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(2.2, friction1.Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -515,3 +641,204 @@ TEST(DOMode, Set) EXPECT_EQ(ode1.Fdir1(), gz::math::Vector3d(1, 2, 3)); } + +///////////////////////////////////////////////// +TEST(DOMbullet, DefaultValues) +{ + sdf::BulletFriction bullet; + EXPECT_EQ(nullptr, bullet.Element()); + EXPECT_DOUBLE_EQ(1.0, bullet.Friction()); + EXPECT_DOUBLE_EQ(1.0, bullet.Friction2()); + EXPECT_EQ(gz::math::Vector3d(0, 0, 0), bullet.Fdir1()); + EXPECT_DOUBLE_EQ(1.0, bullet.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMbullet, CopyOperator) +{ + sdf::BulletFriction bullet1; + bullet1.SetFriction(0.1); + bullet1.SetFriction2(0.2); + bullet1.SetFdir1(gz::math::Vector3d(1, 2, 3)); + bullet1.SetRollingFriction(4.0); + + sdf::BulletFriction bullet2(bullet1); + EXPECT_DOUBLE_EQ(0.1, bullet2.Friction()); + EXPECT_DOUBLE_EQ(0.2, bullet2.Friction2()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), bullet2.Fdir1()); + EXPECT_DOUBLE_EQ(4.0, bullet2.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMbullet, CopyAssignmentOperator) +{ + sdf::BulletFriction bullet1; + bullet1.SetFriction(0.1); + bullet1.SetFriction2(0.2); + bullet1.SetFdir1(gz::math::Vector3d(1, 2, 3)); + bullet1.SetRollingFriction(4.0); + + sdf::BulletFriction bullet2 = bullet1; + EXPECT_DOUBLE_EQ(0.1, bullet2.Friction()); + EXPECT_DOUBLE_EQ(0.2, bullet2.Friction2()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), bullet2.Fdir1()); + EXPECT_DOUBLE_EQ(4.0, bullet2.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMbullet, CopyAssignmentAfterMove) +{ + sdf::BulletFriction bullet1; + bullet1.SetFriction(0.1); + bullet1.SetFriction2(0.2); + bullet1.SetFdir1(gz::math::Vector3d(1, 2, 3)); + bullet1.SetRollingFriction(4.0); + + sdf::BulletFriction bullet2; + bullet2.SetFriction(0.2); + bullet2.SetFriction2(0.1); + bullet2.SetFdir1(gz::math::Vector3d(3, 2, 1)); + bullet2.SetRollingFriction(3.0); + + sdf::BulletFriction tmp = std::move(bullet1); + bullet1 = bullet2; + bullet2 = tmp; + + EXPECT_DOUBLE_EQ(0.2, bullet1.Friction()); + EXPECT_DOUBLE_EQ(0.1, bullet1.Friction2()); + EXPECT_EQ(gz::math::Vector3d(3, 2, 1), bullet1.Fdir1()); + EXPECT_DOUBLE_EQ(3.0, bullet1.RollingFriction()); + + EXPECT_DOUBLE_EQ(0.1, bullet2.Friction()); + EXPECT_DOUBLE_EQ(0.2, bullet2.Friction2()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), bullet2.Fdir1()); + EXPECT_DOUBLE_EQ(4.0, bullet2.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMbullet, Set) +{ + sdf::BulletFriction bullet1; + + EXPECT_DOUBLE_EQ(bullet1.Friction(), 1.0); + EXPECT_DOUBLE_EQ(bullet1.Friction2(), 1.0); + EXPECT_EQ(bullet1.Fdir1(), + gz::math::Vector3d(0, 0, 0)); + EXPECT_DOUBLE_EQ(bullet1.RollingFriction(), 1.0); + + bullet1.SetFriction(0.1); + bullet1.SetFriction2(0.2); + bullet1.SetFdir1(gz::math::Vector3d(1, 2, 3)); + bullet1.SetRollingFriction(4); + + EXPECT_DOUBLE_EQ(0.1, bullet1.Friction()); + EXPECT_DOUBLE_EQ(0.2, bullet1.Friction2()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), bullet1.Fdir1()); + EXPECT_DOUBLE_EQ(4.0, bullet1.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, DefaultValues) +{ + sdf::Torsional torsional; + EXPECT_EQ(nullptr, torsional.Element()); + EXPECT_DOUBLE_EQ(1.0, torsional.Coefficient()); + EXPECT_TRUE(torsional.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional.PatchRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional.SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional.ODESlip()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, CopyOperator) +{ + sdf::Torsional torsional1; + torsional1.SetCoefficient(0.1); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.2); + torsional1.SetSurfaceRadius(4.0); + torsional1.SetODESlip(1.0); + + sdf::Torsional torsional2(torsional1); + EXPECT_DOUBLE_EQ(0.1, torsional2.Coefficient()); + EXPECT_FALSE(torsional2.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.2, torsional2.PatchRadius()); + EXPECT_DOUBLE_EQ(4.0, torsional2.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.0, torsional2.ODESlip()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, CopyAssignmentOperator) +{ + sdf::Torsional torsional1; + torsional1.SetCoefficient(0.1); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.2); + torsional1.SetSurfaceRadius(4.0); + torsional1.SetODESlip(1.0); + + sdf::Torsional torsional2 = torsional1; + EXPECT_DOUBLE_EQ(0.1, torsional2.Coefficient()); + EXPECT_FALSE(torsional2.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.2, torsional2.PatchRadius()); + EXPECT_DOUBLE_EQ(4.0, torsional2.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.0, torsional2.ODESlip()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, CopyAssignmentAfterMove) +{ + sdf::Torsional torsional1; + torsional1.SetCoefficient(0.1); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.2); + torsional1.SetSurfaceRadius(4.0); + torsional1.SetODESlip(1.0); + + sdf::Torsional torsional2; + torsional2.SetCoefficient(1.1); + torsional2.SetUsePatchRadius(true); + torsional2.SetPatchRadius(1.2); + torsional2.SetSurfaceRadius(4.1); + torsional2.SetODESlip(1.1); + + sdf::Torsional tmp = std::move(torsional1); + torsional1 = torsional2; + torsional2 = tmp; + + EXPECT_DOUBLE_EQ(0.1, torsional2.Coefficient()); + EXPECT_FALSE(torsional2.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.2, torsional2.PatchRadius()); + EXPECT_DOUBLE_EQ(4.0, torsional2.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.0, torsional2.ODESlip()); + + EXPECT_DOUBLE_EQ(1.1, torsional1.Coefficient()); + EXPECT_TRUE(torsional1.UsePatchRadius()); + EXPECT_DOUBLE_EQ(1.2, torsional1.PatchRadius()); + EXPECT_DOUBLE_EQ(4.1, torsional1.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.1, torsional1.ODESlip()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, Set) +{ + sdf::Torsional torsional1; + + EXPECT_DOUBLE_EQ(1.0, torsional1.Coefficient()); + EXPECT_TRUE(torsional1.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional1.PatchRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional1.SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional1.ODESlip()); + + torsional1.SetCoefficient(0.1); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.2); + torsional1.SetSurfaceRadius(4); + torsional1.SetODESlip(1.0); + + EXPECT_DOUBLE_EQ(0.1, torsional1.Coefficient()); + EXPECT_FALSE(torsional1.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.2, torsional1.PatchRadius()); + EXPECT_DOUBLE_EQ(4.0, torsional1.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.0, torsional1.ODESlip()); +} diff --git a/src/Types.cc b/src/Types.cc index 6e202b936..5b363a383 100644 --- a/src/Types.cc +++ b/src/Types.cc @@ -144,13 +144,5 @@ std::string JoinName( else return _scopeName + std::string(kScopeDelimiter) + _localName; } - -///////////////////////////////////////////////// -const std::string &internal::SdfScopeDelimiter() -{ - static const gz::utils::NeverDestroyed delimiter{ - kScopeDelimiter}; - return delimiter.Access(); -} } } diff --git a/src/Types_TEST.cc b/src/Types_TEST.cc index 37b951739..3d04947be 100644 --- a/src/Types_TEST.cc +++ b/src/Types_TEST.cc @@ -200,11 +200,3 @@ TEST(Types, JoinName) EXPECT_EQ(joinedName, ""); } } - -///////////////////////////////////////////////// -TEST(Types, ScopeDelimiters) -{ - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - EXPECT_EQ(sdf::kScopeDelimiter, sdf::kSdfScopeDelimiter); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION -} diff --git a/src/Utils.cc b/src/Utils.cc index c6a89e867..a76315e0f 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -366,7 +366,8 @@ void copyChildren(ElementPtr _sdf, tinyxml2::XMLElement *_xml, for (const tinyxml2::XMLAttribute *attribute = elemXml->FirstAttribute(); attribute; attribute = attribute->Next()) { - element->AddAttribute(attribute->Name(), "string", "", 1, ""); + // Add with required == 0 to allow unrecognized attribute to be empty + element->AddAttribute(attribute->Name(), "string", "", 0, ""); element->GetAttribute(attribute->Name())->SetFromString( attribute->Value()); } diff --git a/src/World.cc b/src/World.cc index d94ff685a..e17005c9f 100644 --- a/src/World.cc +++ b/src/World.cc @@ -773,14 +773,27 @@ Actor *World::ActorByIndex(uint64_t _index) ///////////////////////////////////////////////// bool World::ActorNameExists(const std::string &_name) const { - for (auto const &a : this->dataPtr->actors) + return nullptr != this->ActorByName(_name); +} + +///////////////////////////////////////////////// +const Actor *World::ActorByName(const std::string &_name) const +{ + for (const Actor &a : this->dataPtr->actors) { if (a.Name() == _name) { - return true; + return &a; } } - return false; + return nullptr; +} + +///////////////////////////////////////////////// +Actor *World::ActorByName(const std::string &_name) +{ + return const_cast( + static_cast(this)->ActorByName(_name)); } ////////////////////////////////////////////////// diff --git a/src/World_TEST.cc b/src/World_TEST.cc index 3100ce166..ad1e0d2d5 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -64,6 +64,9 @@ TEST(DOMWorld, Construction) EXPECT_EQ(nullptr, world.ModelByName("a::b::c")); EXPECT_EQ(nullptr, world.ModelByName("::::")); + EXPECT_EQ(nullptr, world.ActorByName("")); + EXPECT_EQ(nullptr, world.ActorByName("default")); + EXPECT_EQ(0u, world.FrameCount()); EXPECT_EQ(nullptr, world.FrameByIndex(0)); EXPECT_EQ(nullptr, world.FrameByIndex(1)); @@ -458,15 +461,27 @@ TEST(DOMWorld, AddActor) EXPECT_EQ(1u, world.ActorCount()); EXPECT_FALSE(world.AddActor(actor)); EXPECT_EQ(1u, world.ActorCount()); + EXPECT_NE(nullptr, world.ActorByName("actor1")); world.ClearActors(); EXPECT_EQ(0u, world.ActorCount()); + EXPECT_EQ(nullptr, world.ActorByName("actor1")); EXPECT_TRUE(world.AddActor(actor)); EXPECT_EQ(1u, world.ActorCount()); const sdf::Actor *actorFromWorld = world.ActorByIndex(0); ASSERT_NE(nullptr, actorFromWorld); EXPECT_EQ(actorFromWorld->Name(), actor.Name()); + + const sdf::Actor *actorFromWorldByName = world.ActorByName("actor1"); + ASSERT_NE(nullptr, actorFromWorldByName); + EXPECT_EQ(actorFromWorldByName->Name(), actor.Name()); + + sdf::Actor *mutableActorFromWorldByName = world.ActorByName("actor1"); + ASSERT_NE(nullptr, mutableActorFromWorldByName); + EXPECT_EQ(mutableActorFromWorldByName->Name(), actor.Name()); + mutableActorFromWorldByName->SetName("new_name"); + EXPECT_NE(mutableActorFromWorldByName->Name(), actor.Name()); } ///////////////////////////////////////////////// diff --git a/src/XmlUtils.cc b/src/XmlUtils.cc index 4dbceab71..5d3709707 100644 --- a/src/XmlUtils.cc +++ b/src/XmlUtils.cc @@ -15,7 +15,9 @@ * */ #include +#include +#include "Utils.hh" #include "XmlUtils.hh" #include "sdf/Console.hh" @@ -27,27 +29,41 @@ inline namespace SDF_VERSION_NAMESPACE { ///////////////////////////////////////////////// tinyxml2::XMLNode *DeepClone(tinyxml2::XMLDocument *_doc, const tinyxml2::XMLNode *_src) +{ + sdf::Errors errors; + tinyxml2::XMLNode *result = DeepClone(errors, _doc, _src); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +tinyxml2::XMLNode *DeepClone(sdf::Errors &_errors, + tinyxml2::XMLDocument *_doc, + const tinyxml2::XMLNode *_src) { if (_src == nullptr) { - sdferr << "Pointer to XML node _src is NULL\n"; + _errors.push_back({sdf::ErrorCode::XML_ERROR, + "Pointer to XML node _src is NULL"}); return nullptr; } tinyxml2::XMLNode *copy = _src->ShallowClone(_doc); if (copy == nullptr) { - sdferr << "Failed to clone node " << _src->Value() << "\n"; + _errors.push_back({sdf::ErrorCode::XML_ERROR, + "Failed to clone node " + std::string(_src->Value())}); return nullptr; } for (const tinyxml2::XMLNode *node = _src->FirstChild(); node != nullptr; node = node->NextSibling()) { - auto *childCopy = DeepClone(_doc, node); + auto *childCopy = DeepClone(_errors, _doc, node); if (childCopy == nullptr) { - sdferr << "Failed to clone child " << node->Value() << "\n"; + _errors.push_back({sdf::ErrorCode::XML_ERROR, + "Failed to clone child " + std::string(node->Value())}); return nullptr; } copy->InsertEndChild(childCopy); @@ -57,11 +73,13 @@ tinyxml2::XMLNode *DeepClone(tinyxml2::XMLDocument *_doc, } ///////////////////////////////////////////////// -std::string ElementToString(const tinyxml2::XMLElement *_elem) +std::string ElementToString(sdf::Errors &_errors, + const tinyxml2::XMLElement *_elem) { if (_elem == nullptr) { - sdferr << "Pointer to XML Element _elem is nullptr\n"; + _errors.push_back({sdf::ErrorCode::XML_ERROR, + "Pointer to XML Element _elem is nullptr"}); return ""; } diff --git a/src/XmlUtils.hh b/src/XmlUtils.hh index 7ad0c30d4..522458aad 100644 --- a/src/XmlUtils.hh +++ b/src/XmlUtils.hh @@ -41,10 +41,26 @@ namespace sdf tinyxml2::XMLNode *DeepClone(tinyxml2::XMLDocument *_doc, const tinyxml2::XMLNode *_src); + /// \brief Perform a deep copy of an XML Node + /// + /// This copies an XMLNode _src and all of its decendants + /// into a specified XMLDocument. + /// + /// \param[out] _errors Vector of errors + /// \param[in] _doc Document in which to place the copied node + /// \param[in] _src The node to deep copy + /// \returns The newly copied node upon success OR + /// nullptr if an error occurs. + tinyxml2::XMLNode *DeepClone(sdf::Errors &_errors, + tinyxml2::XMLDocument *_doc, + const tinyxml2::XMLNode *_src); + /// \brief Converts the XML Element to a string + /// \param[out] _errors Vector of errors /// \param[in] _elem Element to be converted /// \return The string representation - std::string ElementToString(const tinyxml2::XMLElement *_elem); + std::string ElementToString(sdf::Errors &_errors, + const tinyxml2::XMLElement *_elem); } } #endif diff --git a/src/XmlUtils_TEST.cc b/src/XmlUtils_TEST.cc index aef79262d..4190b058b 100644 --- a/src/XmlUtils_TEST.cc +++ b/src/XmlUtils_TEST.cc @@ -37,7 +37,9 @@ TEST(XMLUtils, DeepClone) ASSERT_EQ(tinyxml2::XML_SUCCESS, ret); auto root = oldDoc.FirstChild(); - auto newRoot = sdf::DeepClone(&newDoc, root); + sdf::Errors errors; + auto newRoot = sdf::DeepClone(errors, &newDoc, root); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_STREQ("document", newRoot->ToElement()->Name()); @@ -53,3 +55,48 @@ TEST(XMLUtils, DeepClone) auto childB_text = newChildB->ToElement()->GetText(); EXPECT_STREQ("Hello World", childB_text); } + +///////////////////////////////////////////////// +TEST(XMLUtils, InvalidDeepClone) +{ + sdf::Errors errors; + auto newRoot = sdf::DeepClone(errors, nullptr, nullptr); + EXPECT_EQ(nullptr, newRoot); + EXPECT_EQ(1u, errors.size()) << errors; + ASSERT_FALSE(errors.empty()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::XML_ERROR); +} + +///////////////////////////////////////////////// +TEST(XMLUtils, ElementToString) +{ + tinyxml2::XMLDocument doc; + + std::string docXml = +R"( + + Hello World + + +)"; + + auto ret = doc.Parse(docXml.c_str()); + ASSERT_EQ(tinyxml2::XML_SUCCESS, ret); + + auto root = doc.FirstChild(); + sdf::Errors errors; + std::string docString = sdf::ElementToString(errors, root->ToElement()); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(docXml, docString); +} + +///////////////////////////////////////////////// +TEST(XMLUtils, InvalidElementToString) +{ + sdf::Errors errors; + std::string docString = sdf::ElementToString(errors, nullptr); + EXPECT_TRUE(docString.empty()); + EXPECT_EQ(1u, errors.size()) << errors; + ASSERT_FALSE(errors.empty()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::XML_ERROR); +} diff --git a/src/parser.cc b/src/parser.cc index 84a7593d6..fea9f1ff6 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1440,7 +1440,8 @@ static bool readAttributes(tinyxml2::XMLElement *_xml, ElementPtr _sdf, // attribute is found if (std::strchr(attribute->Name(), ':') != nullptr) { - _sdf->AddAttribute(attribute->Name(), "string", "", 1, ""); + // Add with required == 0 to allow custom attribute to be empty + _sdf->AddAttribute(attribute->Name(), "string", "", 0, ""); _sdf->GetAttribute(attribute->Name())->SetFromString(attribute->Value()); attribute = attribute->Next(); continue; @@ -2724,12 +2725,6 @@ bool checkPoseRelativeToGraph(sdf::Errors &_errors, const sdf::Root *_root) return result; } -////////////////////////////////////////////////// -bool checkJointParentChildLinkNames(const sdf::Root *_root) -{ - return checkJointParentChildNames(_root); -} - ////////////////////////////////////////////////// bool checkJointParentChildNames(const sdf::Root *_root) { diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index b7d846e95..e7e4b2c93 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -77,6 +77,7 @@ endif() gz_build_tests(TYPE ${TEST_TYPE} SOURCES ${tests} INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test + ENVIRONMENT SDF_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} ) diff --git a/test/integration/all_symbols_have_version.bash.in b/test/integration/all_symbols_have_version.bash.in index d69a58442..94912d34d 100644 --- a/test/integration/all_symbols_have_version.bash.in +++ b/test/integration/all_symbols_have_version.bash.in @@ -4,7 +4,13 @@ LIBPATH=$1 VERSIONED_NS=v@PROJECT_VERSION_MAJOR@ # Sanity check - there should be at least one symbol -NUM_SYMBOLS=$(nm $LIBPATH | grep -e "sdf" | wc -l) + +# nm options: +# -D to get only dynamic symbols exported +# 3 before the sdf is used by +# mangled symbols in C++ to check for the +# sdf namespace +NUM_SYMBOLS=$(nm -D $LIBPATH | grep -e "3sdf" | wc -l) if [ $NUM_SYMBOLS -eq 0 ] then @@ -13,7 +19,7 @@ then fi # There must be no unversioned symbols -UNVERSIONED_SYMBOLS=$(nm $LIBPATH | grep -e "sdf" | grep -e "$VERSIONED_NS" -v) +UNVERSIONED_SYMBOLS=$(nm -D $LIBPATH | grep -e "3sdf" | grep -e "$VERSIONED_NS" -v) UNVERSIONED_SYMBOL_CHARS=$(printf "$UNVERSIONED_SYMBOLS" | wc -m) if [ $UNVERSIONED_SYMBOL_CHARS -ne 0 ] diff --git a/test/integration/custom_elems_attrs.sdf b/test/integration/custom_elems_attrs.sdf index 8f051fc79..e936450e1 100644 --- a/test/integration/custom_elems_attrs.sdf +++ b/test/integration/custom_elems_attrs.sdf @@ -4,13 +4,13 @@ Description of this world - + L1 L2 - + transmission_interface/SimpleTransmission EffortJointInterface diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 1ed67cd3a..b080ae5d5 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -213,18 +213,18 @@ TEST(Frame, StateFrame) sdfStr << "" << "" << "" - << "" + << "" << " " << " 1 0 2 0 0 0" << " " << " 3 3 9 0 0 0" - << " " + << " " << " 111 3 0 0 0 0" - << " " - << "" - << "" + << " " + << "" + << "" << " 99 0 22 0 0 0" - << "" + << "" << "" << "" << ""; @@ -239,8 +239,8 @@ TEST(Frame, StateFrame) EXPECT_TRUE(worldElem->HasElement("state")); sdf::ElementPtr stateElem = worldElem->GetElement("state"); - EXPECT_TRUE(stateElem->HasElement("model")); - sdf::ElementPtr modelStateElem = stateElem->GetElement("model"); + EXPECT_TRUE(stateElem->HasElement("model_state")); + sdf::ElementPtr modelStateElem = stateElem->GetElement("model_state"); // model EXPECT_TRUE(modelStateElem->HasAttribute("name")); @@ -271,8 +271,8 @@ TEST(Frame, StateFrame) } // link - EXPECT_TRUE(modelStateElem->HasElement("link")); - sdf::ElementPtr linkStateElem = modelStateElem->GetElement("link"); + EXPECT_TRUE(modelStateElem->HasElement("link_state")); + sdf::ElementPtr linkStateElem = modelStateElem->GetElement("link_state"); EXPECT_TRUE(linkStateElem->HasAttribute("name")); EXPECT_EQ(linkStateElem->Get("name"), "my_link"); @@ -286,8 +286,8 @@ TEST(Frame, StateFrame) gz::math::Pose3d(111, 3, 0, 0, 0, 0)); } - EXPECT_TRUE(stateElem->HasElement("light")); - sdf::ElementPtr lightStateElem = stateElem->GetElement("light"); + EXPECT_TRUE(stateElem->HasElement("light_state")); + sdf::ElementPtr lightStateElem = stateElem->GetElement("light_state"); // light EXPECT_TRUE(lightStateElem->HasAttribute("name")); diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 0c63cbc43..54859c95e 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -114,33 +114,42 @@ TEST(NestedModel, State) sdfStr << "" << "" << "" - << "" + << "" << " 0 0 0.5 0 0 0" - << " " + << " " << " 0 0 0.5 0 0 0" - << " 0.001 0 0 0 0 0" - << " 0 0.006121 0 0.012288 0 0.001751" - << " 0 0.006121 0 0 0 0" - << " " - << " " + << " 0.001 0 0" + << " -0.1 5.0 -0.1" + << " 0 0.006121 0" + << " 0.012288 0 0.001751" + << " 0 0.006121 0" + << " 0 0 0" + << " " + << " " << " 1 0 0.5 0 0 0" - << " " + << " " << " 1.25 0 0.5 0 0 0" - << " 0 -0.001 0 0 0 0" - << " 0 0.000674 0 -0.001268 0 0" - << " 0 0.000674 0 0 0 0" - << " " - << " " + << " 0 -0.001 0" + << " 0 0 0" + << " 0 0.000674 0" + << " -0.001268 0 0" + << " 0 0.000674 0" + << " 0 0 0" + << " " + << " " << " 1 1 0.5 0 0 0" - << " " + << " " << " 1.25 1 0.5 0 0 0" - << " 0 0 0.001 0 0 0" - << " 0 0 0 0 0 0" - << " 0 0 0 0 0 0" - << " " - << " " - << " " - << "" + << " 0 0 0.001" + << " 0 0 0" + << " 0 0 0" + << " 0 0 0" + << " 0 0 0" + << " 0 0 0" + << " " + << " " + << " " + << "" << "" << "" << ""; @@ -154,9 +163,9 @@ TEST(NestedModel, State) sdf::ElementPtr worldElem = sdfParsed->Root()->GetElement("world"); EXPECT_TRUE(worldElem->HasElement("state")); sdf::ElementPtr stateElem = worldElem->GetElement("state"); - EXPECT_TRUE(stateElem->HasElement("model")); + EXPECT_TRUE(stateElem->HasElement("model_state")); - sdf::ElementPtr modelStateElem = stateElem->GetElement("model"); + sdf::ElementPtr modelStateElem = stateElem->GetElement("model_state"); // model sdf EXPECT_TRUE(modelStateElem->HasAttribute("name")); @@ -164,83 +173,126 @@ TEST(NestedModel, State) EXPECT_TRUE(modelStateElem->HasElement("pose")); EXPECT_EQ(modelStateElem->Get("pose"), gz::math::Pose3d(0, 0, 0.5, 0, 0, 0)); - EXPECT_TRUE(!modelStateElem->HasElement("joint")); + EXPECT_FALSE(modelStateElem->HasElement("joint")); + EXPECT_FALSE(modelStateElem->HasElement("joint_state")); // link sdf - EXPECT_TRUE(modelStateElem->HasElement("link")); - sdf::ElementPtr linkStateElem = modelStateElem->GetElement("link"); + EXPECT_TRUE(modelStateElem->HasElement("link_state")); + sdf::ElementPtr linkStateElem = modelStateElem->GetElement("link_state"); EXPECT_TRUE(linkStateElem->HasAttribute("name")); EXPECT_EQ(linkStateElem->Get("name"), "link_00"); EXPECT_TRUE(linkStateElem->HasElement("pose")); EXPECT_EQ(linkStateElem->Get("pose"), gz::math::Pose3d(0, 0, 0.5, 0, 0, 0)); - EXPECT_TRUE(linkStateElem->HasElement("velocity")); - EXPECT_EQ(linkStateElem->Get("velocity"), - gz::math::Pose3d(0.001, 0, 0, 0, 0, 0)); - EXPECT_TRUE(linkStateElem->HasElement("acceleration")); - EXPECT_EQ(linkStateElem->Get("acceleration"), - gz::math::Pose3d(0, 0.006121, 0, 0.012288, 0, 0.001751)); - EXPECT_TRUE(linkStateElem->HasElement("wrench")); - EXPECT_EQ(linkStateElem->Get("wrench"), - gz::math::Pose3d(0, 0.006121, 0, 0, 0, 0)); + EXPECT_FALSE(linkStateElem->HasElement("velocity")); + EXPECT_TRUE(linkStateElem->HasElement("angular_velocity")); + EXPECT_TRUE(linkStateElem->HasElement("linear_velocity")); + EXPECT_EQ(linkStateElem->Get("linear_velocity"), + gz::math::Vector3d(0.001, 0, 0)); + EXPECT_EQ(linkStateElem->Get("angular_velocity"), + gz::math::Vector3d(-0.1, 5.0, -0.1)); + EXPECT_FALSE(linkStateElem->HasElement("acceleration")); + EXPECT_TRUE(linkStateElem->HasElement("angular_acceleration")); + EXPECT_TRUE(linkStateElem->HasElement("linear_acceleration")); + EXPECT_EQ(linkStateElem->Get("linear_acceleration"), + gz::math::Vector3d(0, 0.006121, 0)); + EXPECT_EQ(linkStateElem->Get("angular_acceleration"), + gz::math::Vector3d(0.012288, 0, 0.001751)); + EXPECT_FALSE(linkStateElem->HasElement("wrench")); + EXPECT_TRUE(linkStateElem->HasElement("torque")); + EXPECT_TRUE(linkStateElem->HasElement("force")); + EXPECT_EQ(linkStateElem->Get("force"), + gz::math::Vector3d(0, 0.006121, 0)); + EXPECT_EQ(linkStateElem->Get("torque"), + gz::math::Vector3d(0, 0, 0)); // nested model sdf - EXPECT_TRUE(modelStateElem->HasElement("model")); + EXPECT_TRUE(modelStateElem->HasElement("model_state")); sdf::ElementPtr nestedModelStateElem = - modelStateElem->GetElement("model"); + modelStateElem->GetElement("model_state"); EXPECT_TRUE(nestedModelStateElem->HasAttribute("name")); EXPECT_EQ(nestedModelStateElem->Get("name"), "model_01"); EXPECT_TRUE(nestedModelStateElem->HasElement("pose")); EXPECT_EQ(nestedModelStateElem->Get("pose"), gz::math::Pose3d(1, 0, 0.5, 0, 0, 0)); - EXPECT_TRUE(!nestedModelStateElem->HasElement("joint")); + EXPECT_FALSE(nestedModelStateElem->HasElement("joint")); + EXPECT_FALSE(nestedModelStateElem->HasElement("joint_state")); // nested model's link sdf - EXPECT_TRUE(nestedModelStateElem->HasElement("link")); + EXPECT_TRUE(nestedModelStateElem->HasElement("link_state")); sdf::ElementPtr nestedLinkStateElem = - nestedModelStateElem->GetElement("link"); + nestedModelStateElem->GetElement("link_state"); EXPECT_TRUE(nestedLinkStateElem->HasAttribute("name")); EXPECT_EQ(nestedLinkStateElem->Get("name"), "link_01"); EXPECT_TRUE(nestedLinkStateElem->HasElement("pose")); EXPECT_EQ(nestedLinkStateElem->Get("pose"), gz::math::Pose3d(1.25, 0, 0.5, 0, 0, 0)); - EXPECT_TRUE(nestedLinkStateElem->HasElement("velocity")); - EXPECT_EQ(nestedLinkStateElem->Get("velocity"), - gz::math::Pose3d(0, -0.001, 0, 0, 0, 0)); - EXPECT_TRUE(nestedLinkStateElem->HasElement("acceleration")); - EXPECT_EQ(nestedLinkStateElem->Get("acceleration"), - gz::math::Pose3d(0, 0.000674, 0, -0.001268, 0, 0)); - EXPECT_TRUE(nestedLinkStateElem->HasElement("wrench")); - EXPECT_EQ(nestedLinkStateElem->Get("wrench"), - gz::math::Pose3d(0, 0.000674, 0, 0, 0, 0)); + EXPECT_FALSE(nestedLinkStateElem->HasElement("velocity")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("angular_velocity")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("linear_velocity")); + EXPECT_EQ(nestedLinkStateElem->Get("linear_velocity"), + gz::math::Vector3d(0, -0.001, 0)); + EXPECT_EQ(nestedLinkStateElem->Get("angular_velocity"), + gz::math::Vector3d(0, 0, 0)); + EXPECT_FALSE(nestedLinkStateElem->HasElement("acceleration")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("angular_acceleration")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("linear_acceleration")); + EXPECT_EQ( + nestedLinkStateElem->Get("linear_acceleration"), + gz::math::Vector3d(0, 0.000674, 0)); + EXPECT_EQ( + nestedLinkStateElem->Get("angular_acceleration"), + gz::math::Vector3d(-0.001268, 0, 0)); + EXPECT_FALSE(nestedLinkStateElem->HasElement("wrench")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("torque")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("force")); + EXPECT_EQ(nestedLinkStateElem->Get("force"), + gz::math::Vector3d(0, 0.000674, 0)); + EXPECT_EQ(nestedLinkStateElem->Get("torque"), + gz::math::Vector3d(0, 0, 0)); // double nested model sdf - EXPECT_TRUE(nestedModelStateElem->HasElement("model")); - nestedModelStateElem = nestedModelStateElem->GetElement("model"); + EXPECT_TRUE(nestedModelStateElem->HasElement("model_state")); + nestedModelStateElem = nestedModelStateElem->GetElement("model_state"); EXPECT_TRUE(nestedModelStateElem->HasAttribute("name")); EXPECT_EQ(nestedModelStateElem->Get("name"), "model_02"); EXPECT_TRUE(nestedModelStateElem->HasElement("pose")); EXPECT_EQ(nestedModelStateElem->Get("pose"), gz::math::Pose3d(1, 1, 0.5, 0, 0, 0)); - EXPECT_TRUE(!nestedModelStateElem->HasElement("joint")); + EXPECT_FALSE(nestedModelStateElem->HasElement("joint")); + EXPECT_FALSE(nestedModelStateElem->HasElement("joint_state")); // double nested model's link sdf - EXPECT_TRUE(nestedModelStateElem->HasElement("link")); - nestedLinkStateElem = nestedModelStateElem->GetElement("link"); + EXPECT_TRUE(nestedModelStateElem->HasElement("link_state")); + nestedLinkStateElem = nestedModelStateElem->GetElement("link_state"); EXPECT_TRUE(nestedLinkStateElem->HasAttribute("name")); EXPECT_EQ(nestedLinkStateElem->Get("name"), "link_02"); EXPECT_TRUE(nestedLinkStateElem->HasElement("pose")); EXPECT_EQ(nestedLinkStateElem->Get("pose"), gz::math::Pose3d(1.25, 1, 0.5, 0, 0, 0)); - EXPECT_TRUE(nestedLinkStateElem->HasElement("velocity")); - EXPECT_EQ(nestedLinkStateElem->Get("velocity"), - gz::math::Pose3d(0, 0, 0.001, 0, 0, 0)); - EXPECT_TRUE(nestedLinkStateElem->HasElement("acceleration")); - EXPECT_EQ(nestedLinkStateElem->Get("acceleration"), - gz::math::Pose3d(0, 0, 0, 0, 0, 0)); - EXPECT_TRUE(nestedLinkStateElem->HasElement("wrench")); - EXPECT_EQ(nestedLinkStateElem->Get("wrench"), - gz::math::Pose3d(0, 0, 0, 0, 0, 0)); + EXPECT_FALSE(nestedLinkStateElem->HasElement("velocity")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("angular_velocity")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("linear_velocity")); + EXPECT_EQ(nestedLinkStateElem->Get("linear_velocity"), + gz::math::Vector3d(0, 0, 0.001)); + EXPECT_EQ(nestedLinkStateElem->Get("angular_velocity"), + gz::math::Vector3d(0, 0, 0)); + EXPECT_FALSE(nestedLinkStateElem->HasElement("acceleration")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("angular_acceleration")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("linear_acceleration")); + EXPECT_EQ( + nestedLinkStateElem->Get("linear_acceleration"), + gz::math::Vector3d(0, 0, 0)); + EXPECT_EQ( + nestedLinkStateElem->Get("angular_acceleration"), + gz::math::Vector3d(0, 0, 0)); + EXPECT_FALSE(nestedLinkStateElem->HasElement("wrench")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("torque")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("force")); + EXPECT_EQ(nestedLinkStateElem->Get("force"), + gz::math::Vector3d(0, 0, 0)); + EXPECT_EQ(nestedLinkStateElem->Get("torque"), + gz::math::Vector3d(0, 0, 0)); } //////////////////////////////////////// diff --git a/test/integration/plugin_attribute.cc b/test/integration/plugin_attribute.cc index 02af21046..7f4f9f4cb 100644 --- a/test/integration/plugin_attribute.cc +++ b/test/integration/plugin_attribute.cc @@ -30,7 +30,7 @@ std::string get_sdf_string() << "" << " " << " " - << " " + << " " << " value" << " " << "" @@ -54,6 +54,8 @@ TEST(PluginAttribute, ParseAttributes) EXPECT_TRUE(user->HasAttribute("attribute")); EXPECT_EQ(user->GetAttribute("attribute")->GetAsString(), std::string("attribute")); + EXPECT_TRUE(user->HasAttribute("empty_attribute")); + EXPECT_EQ(user->GetAttribute("empty_attribute")->GetAsString(), ""); } { sdf::ElementPtr value = plugin->GetElement("value"); diff --git a/test/integration/sdf_custom.cc b/test/integration/sdf_custom.cc index 53ecedcec..ef04256b9 100644 --- a/test/integration/sdf_custom.cc +++ b/test/integration/sdf_custom.cc @@ -60,6 +60,20 @@ TEST(SDFParser, CustomElements) auto customAttrInt = link1Element->Get("mysim:custom_attr_int"); EXPECT_EQ(5, customAttrInt); + // Check empty attribute in link L2 + const sdf::Link *link2 = model->LinkByName("L2"); + ASSERT_TRUE(link2 != nullptr); + sdf::ElementPtr link2Element = link2->Element(); + ASSERT_TRUE(link2Element != nullptr); + EXPECT_TRUE(link2Element->HasAttribute("mysim:empty_attr")); + auto emptyAttrStr = link2Element->Get("mysim:empty_attr"); + EXPECT_EQ("", emptyAttrStr); + + // Ensure that mysim:empty_attr appears when calling ToString("") + auto rootToString = link2Element->ToString(""); + EXPECT_NE(std::string::npos, rootToString.find("mysim:empty_attr=''")) + << rootToString; + // Use of sdf::Model::Element() to obtain an sdf::ElementPtr object sdf::ElementPtr modelElement = model->Element(); @@ -125,7 +139,7 @@ TEST(SDFParser, ReloadCustomElements) ASSERT_NE(nullptr, customElem2); const std::string customElemStr = -R"( +R"( transmission_interface/SimpleTransmission EffortJointInterface diff --git a/test/integration/surface_dom.cc b/test/integration/surface_dom.cc index 9b3cf83aa..f1f7faf94 100644 --- a/test/integration/surface_dom.cc +++ b/test/integration/surface_dom.cc @@ -56,4 +56,22 @@ TEST(DOMSurface, Shapes) EXPECT_DOUBLE_EQ(boxCol->Surface()->Friction()->ODE()->Slip2(), 5); EXPECT_EQ(boxCol->Surface()->Friction()->ODE()->Fdir1(), gz::math::Vector3d(1.2, 3.4, 5.6)); + + EXPECT_DOUBLE_EQ(1.6, + boxCol->Surface()->Friction()->BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(1.7, + boxCol->Surface()->Friction()->BulletFriction()->Friction2()); + EXPECT_EQ(boxCol->Surface()->Friction()->BulletFriction()->Fdir1(), + gz::math::Vector3d(6.5, 4.3, 2.1)); + EXPECT_DOUBLE_EQ(1.5, + boxCol->Surface()->Friction()->BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(5.1, + boxCol->Surface()->Friction()->Torsional()->Coefficient()); + EXPECT_FALSE(boxCol->Surface()->Friction()->Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(1.3, + boxCol->Surface()->Friction()->Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(3.7, + boxCol->Surface()->Friction()->Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(3.1, boxCol->Surface()->Friction()->Torsional()->ODESlip()); } diff --git a/test/performance/CMakeLists.txt b/test/performance/CMakeLists.txt index c809bf32e..fe3e17931 100644 --- a/test/performance/CMakeLists.txt +++ b/test/performance/CMakeLists.txt @@ -4,4 +4,7 @@ set(tests parser_urdf.cc ) -gz_build_tests(TYPE ${TEST_TYPE} SOURCES ${tests} INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test) +gz_build_tests(TYPE ${TEST_TYPE} + SOURCES ${tests} + INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test + ENVIRONMENT SDF_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) diff --git a/test/sdf/basic_shapes.sdf b/test/sdf/basic_shapes.sdf index 64d4a7091..93c9fafc1 100644 --- a/test/sdf/basic_shapes.sdf +++ b/test/sdf/basic_shapes.sdf @@ -1,5 +1,5 @@ - + true diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index cd5b31e41..5a2e72d4f 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -1,5 +1,5 @@ - + @@ -14,12 +14,27 @@ - 0.6 - 0.7 - 4 - 5 - 1.2 3.4 5.6 + 0.6 + 0.7 + 4 + 5 + 1.2 3.4 5.6 + + 1.6 + 1.7 + 6.5 4.3 2.1 + 1.5 + + + 5.1 + false + 1.3 + 3.7 + + 3.1 + + diff --git a/test/sdf/shapes_world.sdf b/test/sdf/shapes_world.sdf index b0858ec75..3a7be8b91 100644 --- a/test/sdf/shapes_world.sdf +++ b/test/sdf/shapes_world.sdf @@ -1,5 +1,5 @@ - + true