diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index bd14ed689..ca5392189 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -8,7 +8,7 @@ jobs: name: Ubuntu Focal CI steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Compile and test id: ci uses: gazebo-tooling/action-gz-ci@focal @@ -21,7 +21,7 @@ jobs: name: Ubuntu Jammy CI steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Compile and test id: ci uses: gazebo-tooling/action-gz-ci@jammy diff --git a/CMakeLists.txt b/CMakeLists.txt index 531f7cd8a..f7feeffde 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(gz-physics6 VERSION 6.2.0) +project(gz-physics6 VERSION 6.3.2) #============================================================================ # Find gz-cmake diff --git a/COPYING b/COPYING deleted file mode 100644 index 4909afd04..000000000 --- a/COPYING +++ /dev/null @@ -1,178 +0,0 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - diff --git a/Changelog.md b/Changelog.md index 58add0d13..b7018b5b3 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,65 @@ ### Gazebo Physics 6.x.x (202X-XX-XX) +### Gazebo Physics 6.3.2 (2023-03-28) + +1. Fix joint index used in bookkeeping + * [Pull request #498](https://github.com/gazebosim/gz-physics/pull/498) + +1. Move joint transmitted wrench test to separate file + * [Pull request #495](https://github.com/gazebosim/gz-physics/pull/495) + * [Pull request #496](https://github.com/gazebosim/gz-physics/pull/496) + +### Gazebo Physics 6.3.1 (2023-03-22) + +1. Rename COPYING to LICENSE + * [Pull request #487](https://github.com/gazebosim/gz-physics/pull/487) + +1. Remove an extra symlink on all platforms + * [Pull request #482](https://github.com/gazebosim/gz-physics/pull/482) + +1. Fix memory corruption due to faulty refcount tracking + * [Pull request #480](https://github.com/gazebosim/gz-physics/pull/480) + +1. Infrastructure + * [Pull request #490](https://github.com/gazebosim/gz-physics/pull/490) + * [Pull request #488](https://github.com/gazebosim/gz-physics/pull/488) + +1. Forward Ports + * [Pull request #491](https://github.com/gazebosim/gz-physics/pull/491) + * [Pull request #489](https://github.com/gazebosim/gz-physics/pull/489) + * [Pull request #485](https://github.com/gazebosim/gz-physics/pull/485) + * [Pull request #476](https://github.com/gazebosim/gz-physics/pull/476) + +### Gazebo Physics 6.3.0 (2023-02-02) + +1. Fix windows warnings related to bullet + * [Pull request #473](https://github.com/gazebosim/gz-physics/pull/473) + +1. ign -> gz : Remove redundant namespace references + * [Pull request #400](https://github.com/gazebosim/gz-physics/pull/400) + +1. Resolve joints in nested models + * [Pull request #464](https://github.com/gazebosim/gz-physics/pull/464) + +1. Apply gravity external to dartsim for added mass + * [Pull request #462](https://github.com/gazebosim/gz-physics/pull/462) + +1. Refactor the VectorApprox to a single location + * [Pull request #470](https://github.com/gazebosim/gz-physics/pull/470) + +1. Port: 5 to 6 + * [Pull request #467](https://github.com/gazebosim/gz-physics/pull/467) + +1. Simplify "falling" test to not require LinkFrameSemantics + * [Pull request #461](https://github.com/gazebosim/gz-physics/pull/461) + +1. Install the common test executables to libexec + * [Pull request #458](https://github.com/gazebosim/gz-physics/pull/458) + +1. [bullet]: Fix how changed link poses are computed + * [Pull request #460](https://github.com/gazebosim/gz-physics/pull/460) + ### Gazebo Physics 6.2.0 (2022-11-28) 1. Reduce error to debug and add notes @@ -110,6 +169,40 @@ ## Gazebo Physics 5.x +### Gazebo Physics 5.3.1 (2023-02-16) + +1. Fix memory corruption due to faulty refcount tracking + * [Pull request #480](https://github.com/gazebosim/gz-physics/pull/480) + +### Gazebo Physics 5.3.0 (2023-01-09) + +1. Fix windows warnings related to bullet + * [Pull request #473](https://github.com/gazebosim/gz-physics/pull/473) + +1. Apply ign-gz after forward merge from ign-physics2 + * [Pull request #472](https://github.com/gazebosim/gz-physics/pull/472) + +1. Port: 2 to 5 + * [Pull request #471](https://github.com/gazebosim/gz-physics/pull/471) + +1. Fix build errors and warnings for DART 6.13.0 + * [Pull request #465](https://github.com/gazebosim/gz-physics/pull/465) + +1. Backport windows fix + * [Pull request #437](https://github.com/gazebosim/gz-physics/pull/437) + +1. dartsim: fix handling inertia matrix pose rotation + * [Pull request #351](https://github.com/gazebosim/gz-physics/pull/351) + +1. Add code coverage ignore file + * [Pull request #388](https://github.com/gazebosim/gz-physics/pull/388) + +1. Change IGN\_DESIGNATION to GZ\_DESIGNATION + * [Pull request #390](https://github.com/gazebosim/gz-physics/pull/390) + +1. README: Ignition -> Gazebo + * [Pull request #386](https://github.com/gazebosim/gz-physics/pull/386) + ### Gazebo Physics 5.2.0 (2022-06-29) 1. dartsim: support non-tree kinematics in AttachFixedJoint @@ -451,7 +544,53 @@ ## Gazebo Physics 2.x -### Gazebo Physics 2.x.x (20XX-XX-XX) +### Gazebo Physics 2.6.1 (2023-01-09) + +1. Fix build errors and warnings for DART 6.13.0 + * [Pull request #465](https://github.com/gazebosim/gz-physics/pull/465) + +1. Don't install CMakeLists.txt files + * [Pull request #456](https://github.com/gazebosim/gz-physics/pull/456) + +### Gazebo Physics 2.6.0 (2022-11-30) + +1. Migrate Ignition headers + * [Pull request #402](https://github.com/gazebosim/gz-physics/pull/402) + +### Gazebo Physics 2.5.1 (2022-08-16) + +1. Remove redundant namespace references + * [Pull request #400](https://github.com/gazebosim/gz-physics/pull/400) + +1. Add code coverage ignore file + * [Pull request #388](https://github.com/gazebosim/gz-physics/pull/388) + +1. Change `IGN_DESIGNATION` to `GZ_DESIGNATION` + * [Pull request #390](https://github.com/gazebosim/gz-physics/pull/390) + +1. Ignition -> Gazebo + * [Pull request #386](https://github.com/gazebosim/gz-physics/pull/386) + +1. Make `CONFIG` a CMake pass-through option for DART + * [Pull request #339](https://github.com/gazebosim/gz-physics/pull/339) + +1. Remove explicitly-defined copy constructor/operator for `Shape` + * [Pull request #328](https://github.com/gazebosim/gz-physics/pull/328) + +1. Fix `ExpectData` compiler warnings + * [Pull request #335](https://github.com/gazebosim/gz-physics/pull/335) + +1. Fix copying of `ExpectData` objects + * [Pull request #337](https://github.com/gazebosim/gz-physics/pull/337) + +1. Fix Apache license version + * [Pull request #326](https://github.com/gazebosim/gz-physics/pull/326) + +1. Tutorial fixes + * [Pull request #318](https://github.com/gazebosim/gz-physics/pull/318) + +1. Add `project()` to examples + * [Pull request #322](https://github.com/gazebosim/gz-physics/pull/322) ### Gazebo Physics 2.5.0 (2021-11-09) diff --git a/LICENSE b/LICENSE index 5f63c8824..4909afd04 100644 --- a/LICENSE +++ b/LICENSE @@ -1,15 +1,178 @@ -Software License Agreement (Apache License) + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ -Copyright 2014 Open Source Robotics Foundation + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION -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 + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS - 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. diff --git a/bullet-featherstone/CMakeLists.txt b/bullet-featherstone/CMakeLists.txt index d428dd3d6..c8ee36f41 100644 --- a/bullet-featherstone/CMakeLists.txt +++ b/bullet-featherstone/CMakeLists.txt @@ -42,5 +42,3 @@ else() EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned}) INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR}) endif() -EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned}) -INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR}) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index a7d2acfe2..5a3c1c28c 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -69,9 +69,9 @@ struct WorldInfo std::unique_ptr solver; std::unique_ptr world; - std::unordered_map modelIndexToEntityId; + std::unordered_map modelIndexToEntityId; std::unordered_map modelNameToEntityId; - std::size_t nextModelIndex = 0; + int nextModelIndex = 0; explicit WorldInfo(std::string name); }; @@ -80,7 +80,7 @@ struct ModelInfo { std::string name; Identity world; - std::size_t indexInWorld; + int indexInWorld; Eigen::Isometry3d baseInertiaToLinkFrame; std::unique_ptr body; @@ -112,7 +112,7 @@ struct ModelInfo struct LinkInfo { std::string name; - std::optional indexInModel; + std::optional indexInModel; Identity model; Eigen::Isometry3d inertiaToLinkFrame; std::unique_ptr collider = nullptr; @@ -127,12 +127,12 @@ struct CollisionInfo std::unique_ptr collider; Identity link; Eigen::Isometry3d linkToCollision; - std::size_t indexInLink = 0; + int indexInLink = 0; }; struct InternalJoint { - std::size_t indexInBtModel; + int indexInBtModel; }; struct RootJoint {}; @@ -165,21 +165,31 @@ struct JointInfo // This field gets set by AddJoint std::size_t indexInGzModel = 0; btMultiBodyJointMotor* motor = nullptr; - double effort = 0; + btScalar effort = 0; std::shared_ptr fixedContraint = nullptr; }; inline btMatrix3x3 convertMat(const Eigen::Matrix3d& mat) { - return btMatrix3x3(mat(0, 0), mat(0, 1), mat(0, 2), - mat(1, 0), mat(1, 1), mat(1, 2), - mat(2, 0), mat(2, 1), mat(2, 2)); + return btMatrix3x3( + static_cast(mat(0, 0)), static_cast(mat(0, 1)), + static_cast(mat(0, 2)), static_cast(mat(1, 0)), + static_cast(mat(1, 1)), static_cast(mat(1, 2)), + static_cast(mat(2, 0)), static_cast(mat(2, 1)), + static_cast(mat(2, 2))); } inline btVector3 convertVec(const Eigen::Vector3d& vec) { - return btVector3(vec(0), vec(1), vec(2)); + return btVector3(static_cast(vec(0)), static_cast(vec(1)), + static_cast(vec(2))); +} + +inline btVector3 convertVec(const math::Vector3d& vec) +{ + return btVector3(static_cast(vec[0]), static_cast(vec[1]), + static_cast(vec[2])); } inline btTransform convertTf(const Eigen::Isometry3d& tf) @@ -215,7 +225,7 @@ inline Eigen::Isometry3d convert(const btTransform& tf) inline btTransform GetWorldTransformOfLinkInertiaFrame( const btMultiBody &body, - const std::size_t linkIndexInModel) + const int linkIndexInModel) { const auto p = body.localPosToWorld( linkIndexInModel, btVector3(0, 0, 0)); @@ -292,7 +302,8 @@ class Base : public Implements3d> if (link->indexInModel.has_value()) { // We expect the links to be added in order - assert(*link->indexInModel+1 == model->linkEntityIds.size()); + assert(static_cast(*link->indexInModel + 1) == + model->linkEntityIds.size()); } else { @@ -312,7 +323,7 @@ class Base : public Implements3d> auto collision = std::make_shared(std::move(_collisionInfo)); this->collisions[id] = collision; auto *link = this->ReferenceInterface(_collisionInfo.link); - collision->indexInLink = link->collisionEntityIds.size(); + collision->indexInLink = static_cast(link->collisionEntityIds.size()); link->collisionEntityIds.push_back(id); link->collisionNameToEntityId[collision->name] = id; @@ -354,8 +365,10 @@ class Base : public Implements3d> public: std::unordered_map collisions; public: std::unordered_map joints; - public: std::vector> meshesGImpact; + // Note, the order of triangleMeshes and meshesGImpact is important. Reversing + // the order causes segfaults on macOS during destruction. public: std::vector> triangleMeshes; + public: std::vector> meshesGImpact; }; } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/EntityManagementFeatures.cc b/bullet-featherstone/src/EntityManagementFeatures.cc index 1344fdc72..3a491862a 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.cc +++ b/bullet-featherstone/src/EntityManagementFeatures.cc @@ -76,7 +76,7 @@ std::size_t EntityManagementFeatures::GetLinkIndex( // the rest up by one when providing an index to gazebo const auto index = this->ReferenceInterface(_linkID)->indexInModel; if (index.has_value()) - return *index+1; + return static_cast(*index+1); return 0; } @@ -183,7 +183,8 @@ const std::string &EntityManagementFeatures::GetShapeName( std::size_t EntityManagementFeatures::GetShapeIndex( const Identity &_shapeID) const { - return this->ReferenceInterface(_shapeID)->indexInLink; + return static_cast( + this->ReferenceInterface(_shapeID)->indexInLink); } ///////////////////////////////////////////////// @@ -259,7 +260,8 @@ bool EntityManagementFeatures::RemoveModelByIndex( const Identity & _worldID, std::size_t _modelIndex) { auto *world = this->ReferenceInterface(_worldID); - const auto it = world->modelIndexToEntityId.find(_modelIndex); + const auto it = + world->modelIndexToEntityId.find(static_cast(_modelIndex)); if (it == world->modelIndexToEntityId.end()) return false; @@ -367,7 +369,8 @@ Identity EntityManagementFeatures::GetModel( const Identity &_worldID, std::size_t _modelIndex) const { const auto *world = this->ReferenceInterface(_worldID); - const auto it = world->modelIndexToEntityId.find(_modelIndex); + const auto it = + world->modelIndexToEntityId.find(static_cast(_modelIndex)); if (it == world->modelIndexToEntityId.end()) return this->GenerateInvalidId(); @@ -401,7 +404,7 @@ std::size_t EntityManagementFeatures::GetModelIndex( // the rest up by one when providing an index to gazebo const auto index = this->ReferenceInterface( _modelID)->indexInWorld; - return index+1; + return static_cast(index + 1); } ///////////////////////////////////////////////// diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index 4c6773900..ba05b4982 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -84,7 +84,10 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( } if (jointInfo->motor) - jointInfo->motor->setVelocityTarget(_angularVelocity[2]); + { + jointInfo->motor->setVelocityTarget( + static_cast(_angularVelocity[2])); + } } } } diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 3d6372f2c..d71dee741 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -109,7 +109,8 @@ void JointFeatures::SetJointPosition( return; const auto *model = this->ReferenceInterface(joint->model); - model->body->getJointPosMultiDof(identifier->indexInBtModel)[_dof] = _value; + model->body->getJointPosMultiDof(identifier->indexInBtModel)[_dof] = + static_cast(_value); } ///////////////////////////////////////////////// @@ -122,7 +123,8 @@ void JointFeatures::SetJointVelocity( return; const auto *model = this->ReferenceInterface(joint->model); - model->body->getJointVelMultiDof(identifier->indexInBtModel)[_dof] = _value; + model->body->getJointVelMultiDof(identifier->indexInBtModel)[_dof] = + static_cast(_value); } ///////////////////////////////////////////////// @@ -152,7 +154,7 @@ void JointFeatures::SetJointForce( const auto *model = this->ReferenceInterface(joint->model); model->body->getJointTorqueMultiDof( - identifier->indexInBtModel)[_dof] = _value; + identifier->indexInBtModel)[_dof] = static_cast(_value); } ///////////////////////////////////////////////// @@ -168,7 +170,8 @@ std::size_t JointFeatures::GetJointDegreesOfFreedom(const Identity &_id) const } const auto *model = this->ReferenceInterface(joint->model); - return model->body->getLink(identifier->indexInBtModel).m_dofCount; + return static_cast( + model->body->getLink(identifier->indexInBtModel).m_dofCount); } ///////////////////////////////////////////////// @@ -291,7 +294,7 @@ void JointFeatures::SetJointVelocityCommand( world->world->addMultiBodyConstraint(jointInfo->motor); } - jointInfo->motor->setVelocityTarget(_value); + jointInfo->motor->setVelocityTarget(static_cast(_value)); } ///////////////////////////////////////////////// @@ -363,10 +366,7 @@ void JointFeatures::SetJointTransformFromParent( if (jointInfo->fixedContraint) { jointInfo->fixedContraint->setPivotInA( - btVector3( - _pose.translation()[0], - _pose.translation()[1], - _pose.translation()[2])); + convertVec(_pose.translation())); } } } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/LinkFeatures.cc b/bullet-featherstone/src/LinkFeatures.cc index 7fbc6b28a..b8dc44356 100644 --- a/bullet-featherstone/src/LinkFeatures.cc +++ b/bullet-featherstone/src/LinkFeatures.cc @@ -59,7 +59,7 @@ void LinkFeatures::AddLinkExternalTorqueInWorld( auto *link = this->ReferenceInterface(_id); auto *model = this->ReferenceInterface(link->model); - auto T = btVector3(_torque[0], _torque[1], _torque[2]); + const btVector3 T = convertVec(_torque); if (link->indexInModel.has_value()) { diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 73ae93656..071071c02 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -86,8 +86,7 @@ Identity SDFFeatures::ConstructSdfWorld( const WorldInfoPtr &worldInfo = this->worlds.at(worldID); - auto gravity = _sdfWorld.Gravity(); - worldInfo->world->setGravity(btVector3(gravity[0], gravity[1], gravity[2])); + worldInfo->world->setGravity(convertVec(_sdfWorld.Gravity())); for (std::size_t i = 0; i < _sdfWorld.ModelCount(); ++i) { @@ -115,7 +114,7 @@ struct Structure /// The root link of the model const ::sdf::Link *rootLink; const ::sdf::Joint *rootJoint; - double mass; + btScalar mass; btVector3 inertia; /// Is the root link fixed @@ -321,8 +320,8 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) } const auto &M = rootLink->Inertial().MassMatrix(); - const auto mass = M.Mass(); - btVector3 inertia(M.Ixx(), M.Iyy(), M.Izz()); + const auto mass = static_cast(M.Mass()); + btVector3 inertia(convertVec(M.DiagonalMoments())); for (const double &I : {M.Ixy(), M.Ixz(), M.Iyz()}) { if (std::abs(I) > 1e-3) @@ -358,7 +357,7 @@ Identity SDFFeatures::ConstructSdfModel( const auto modelID = this->AddModel( _sdfModel.Name(), _worldID, rootInertialToLink, std::make_unique( - structure.flatLinks.size(), + static_cast(structure.flatLinks.size()), structure.mass, structure.inertia, structure.fixedBase || isStatic, @@ -390,9 +389,9 @@ Identity SDFFeatures::ConstructSdfModel( std::unordered_map linkIDs; linkIDs.insert(std::make_pair(structure.rootLink, rootID)); - for (std::size_t i = 0; i < structure.flatLinks.size(); ++i) + for (int i = 0; i < static_cast(structure.flatLinks.size()); ++i) { - const auto *link = structure.flatLinks[i]; + const auto *link = structure.flatLinks[static_cast(i)]; const Eigen::Isometry3d linkToComTf = gz::math::eigen3::convert( link->Inertial().Pose()); @@ -404,8 +403,8 @@ Identity SDFFeatures::ConstructSdfModel( } const auto &M = link->Inertial().MassMatrix(); - const double mass = M.Mass(); - const auto inertia = btVector3(M.Ixx(), M.Iyy(), M.Izz()); + const btScalar mass = static_cast(M.Mass()); + const auto inertia = btVector3(convertVec(M.DiagonalMoments())); for (const double I : {M.Ixy(), M.Ixz(), M.Iyz()}) { @@ -489,7 +488,7 @@ Identity SDFFeatures::ConstructSdfModel( JointInfo{ joint->Name(), InternalJoint{i}, - model->linkEntityIds[parentIndex+1], + model->linkEntityIds[static_cast(parentIndex+1)], linkIDs.find(link)->second, poseParentLinkToJoint, poseJointToChild, @@ -531,24 +530,22 @@ Identity SDFFeatures::ConstructSdfModel( if (::sdf::JointType::REVOLUTE == joint->Type()) { - const auto axis = joint->Axis()->Xyz(); + const auto axis = convertVec(joint->Axis()->Xyz()); model->body->setupRevolute( i, mass, inertia, parentIndex, parentRotToThis, - quatRotate(offsetInBBt.getRotation(), - btVector3(axis[0], axis[1], axis[2])), + quatRotate(offsetInBBt.getRotation(), axis), offsetInABt.getOrigin(), -offsetInBBt.getOrigin(), true); } else if (::sdf::JointType::PRISMATIC == joint->Type()) { - const auto axis = joint->Axis()->Xyz(); + const auto axis = convertVec(joint->Axis()->Xyz()); model->body->setupPrismatic( i, mass, inertia, parentIndex, parentRotToThis, - quatRotate(offsetInBBt.getRotation(), - btVector3(axis[0], axis[1], axis[2])), + quatRotate(offsetInBBt.getRotation(), axis), offsetInABt.getOrigin(), -offsetInBBt.getOrigin(), true); @@ -574,16 +571,22 @@ Identity SDFFeatures::ConstructSdfModel( if (::sdf::JointType::PRISMATIC == joint->Type() || ::sdf::JointType::REVOLUTE == joint->Type()) { - model->body->getLink(i).m_jointLowerLimit = joint->Axis()->Lower(); - model->body->getLink(i).m_jointUpperLimit = joint->Axis()->Upper(); - model->body->getLink(i).m_jointDamping = joint->Axis()->Damping(); - model->body->getLink(i).m_jointFriction = joint->Axis()->Friction(); + model->body->getLink(i).m_jointLowerLimit = + static_cast(joint->Axis()->Lower()); + model->body->getLink(i).m_jointUpperLimit = + static_cast(joint->Axis()->Upper()); + model->body->getLink(i).m_jointDamping = + static_cast(joint->Axis()->Damping()); + model->body->getLink(i).m_jointFriction = + static_cast(joint->Axis()->Friction()); model->body->getLink(i).m_jointMaxVelocity = - joint->Axis()->MaxVelocity(); - model->body->getLink(i).m_jointMaxForce = joint->Axis()->Effort(); - jointInfo->effort = joint->Axis()->Effort(); - btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint( - model->body.get(), i, joint->Axis()->Lower(), joint->Axis()->Upper()); + static_cast(joint->Axis()->MaxVelocity()); + model->body->getLink(i).m_jointMaxForce = + static_cast(joint->Axis()->Effort()); + jointInfo->effort = static_cast(joint->Axis()->Effort()); + btMultiBodyConstraint *con = new btMultiBodyJointLimitConstraint( + model->body.get(), i, static_cast(joint->Axis()->Lower()), + static_cast(joint->Axis()->Upper())); world->world->addMultiBodyConstraint(con); } } @@ -613,8 +616,8 @@ Identity SDFFeatures::ConstructSdfModel( { const auto *link = structure.rootLink; const auto &M = link->Inertial().MassMatrix(); - const double mass = M.Mass(); - const auto inertia = btVector3(M.Ixx(), M.Iyy(), M.Izz()); + const btScalar mass = static_cast(M.Mass()); + const auto inertia = convertVec(M.DiagonalMoments()); for (const double I : {M.Ixy(), M.Ixz(), M.Iyz()}) { @@ -677,24 +680,25 @@ bool SDFFeatures::AddSdfCollision( else if (const auto *sphere = geom->SphereShape()) { const auto radius = sphere->Radius(); - shape = std::make_unique(radius); + shape = std::make_unique(static_cast(radius)); } else if (const auto *cylinder = geom->CylinderShape()) { - const auto radius = cylinder->Radius(); - const auto halfLength = cylinder->Length()*0.5; + const auto radius = static_cast(cylinder->Radius()); + const auto halfLength = static_cast(cylinder->Length()*0.5); shape = std::make_unique(btVector3(radius, radius, halfLength)); } else if (const auto *plane = geom->PlaneShape()) { const auto normal = convertVec(math::eigen3::convert(plane->Normal())); - shape = std::make_unique(normal, 0); + shape = std::make_unique(normal, btScalar(0)); } else if (const auto *capsule = geom->CapsuleShape()) { shape = std::make_unique( - capsule->Radius(), capsule->Length()); + static_cast(capsule->Radius()), + static_cast(capsule->Length())); } else if (const auto *ellipsoid = geom->EllipsoidShape()) { @@ -706,10 +710,12 @@ bool SDFFeatures::AddSdfCollision( for (int i = 0; i < n; i++) { btScalar p = 0.5, t = 0; - for (int j = i; j; p *= 0.5, j >>= 1) + for (int j = i; j; p *= btScalar(0.5), j >>= 1) if (j & 1) t += p; btScalar w = 2 * t - 1; - btScalar a = (SIMD_PI + 2 * i * SIMD_PI) / n; + btScalar a = + (SIMD_PI + btScalar(2.0) * static_cast(i) * SIMD_PI) / + static_cast(n); btScalar s = btSqrt(1 - w * w); *x++ = btVector3(s * btCos(a), s * btSin(a), w); } @@ -719,8 +725,7 @@ bool SDFFeatures::AddSdfCollision( vtx.resize(3 + 128); Hammersley::Generate(&vtx[0], vtx.size()); btVector3 center(0, 0, 0); - const auto radii = ellipsoid->Radii(); - btVector3 radius(radii.X(), radii.Y(), radii.Z()); + btVector3 radius = convertVec(ellipsoid->Radii()); for (int i = 0; i < vtx.size(); ++i) { vtx[i] = vtx[i] * radius + center; @@ -741,7 +746,7 @@ bool SDFFeatures::AddSdfCollision( std::make_unique( this->triangleMeshes.back().get())); this->meshesGImpact.back()->updateBound(); - this->meshesGImpact.back()->setMargin(0.001); + this->meshesGImpact.back()->setMargin(btScalar(0.001)); compoundShape->addChildShape(btTransform::getIdentity(), this->meshesGImpact.back().get()); shape = std::move(compoundShape); @@ -750,7 +755,7 @@ bool SDFFeatures::AddSdfCollision( { auto &meshManager = *gz::common::MeshManager::Instance(); auto *mesh = meshManager.Load(meshSdf->Uri()); - auto scale = meshSdf->Scale(); + const btVector3 scale = convertVec(meshSdf->Scale()); if (nullptr == mesh) { gzwarn << "Failed to load mesh from [" << meshSdf->Uri() @@ -768,13 +773,13 @@ bool SDFFeatures::AddSdfCollision( auto vertexCount = s->VertexCount(); auto indexCount = s->IndexCount(); btAlignedObjectArray convertedVerts; - convertedVerts.reserve(vertexCount); + convertedVerts.reserve(static_cast(vertexCount)); for (unsigned int i = 0; i < vertexCount; i++) { convertedVerts.push_back(btVector3( - s->Vertex(i).X() * scale.X(), - s->Vertex(i).Y() * scale.Y(), - s->Vertex(i).Z() * scale.Z())); + static_cast(s->Vertex(i).X()) * scale[0], + static_cast(s->Vertex(i).Y()) * scale[1], + static_cast(s->Vertex(i).Z()) * scale[2])); } this->triangleMeshes.push_back(std::make_unique()); @@ -790,7 +795,7 @@ bool SDFFeatures::AddSdfCollision( std::make_unique( this->triangleMeshes.back().get())); this->meshesGImpact.back()->updateBound(); - this->meshesGImpact.back()->setMargin(0.001); + this->meshesGImpact.back()->setMargin(btScalar(0.001)); compoundShape->addChildShape(btTransform::getIdentity(), this->meshesGImpact.back().get()); } @@ -846,6 +851,29 @@ bool SDFFeatures::AddSdfCollision( Eigen::Isometry3d linkFrameToCollision; if (shape != nullptr) { + { + gz::math::Pose3d gzLinkToCollision; + const auto errors = + _collision.SemanticPose().Resolve(gzLinkToCollision, linkInfo->name); + if (!errors.empty()) + { + gzerr << "An error occurred while resolving the transform of the " + << "collider [" << _collision.Name() << "] in Link [" + << linkInfo->name << "] in Model [" << model->name << "]:\n"; + for (const auto &error : errors) + { + gzerr << error << "\n"; + } + + return false; + } + + linkFrameToCollision = gz::math::eigen3::convert(gzLinkToCollision); + } + + const btTransform btInertialToCollision = + convertTf(linkInfo->inertiaToLinkFrame * linkFrameToCollision); + int linkIndexInModel = -1; if (linkInfo->indexInModel.has_value()) linkIndexInModel = *linkInfo->indexInModel; @@ -859,37 +887,14 @@ bool SDFFeatures::AddSdfCollision( auto collider = std::make_unique( model->body.get(), linkIndexInModel); - { - gz::math::Pose3d gzLinkToCollision; - const auto errors = - _collision.SemanticPose().Resolve(gzLinkToCollision, linkInfo->name); - if (!errors.empty()) - { - gzerr << "An error occurred while resolving the transform of the " - << "collider [" << _collision.Name() << "] in Link [" - << linkInfo->name << "] in Model [" << model->name << "]:\n"; - for (const auto &error : errors) - { - gzerr << error << "\n"; - } - - return false; - } - - linkFrameToCollision = gz::math::eigen3::convert(gzLinkToCollision); - } - - const btTransform btInertialToCollision = - convertTf(linkInfo->inertiaToLinkFrame * linkFrameToCollision); - linkInfo->shape->addChildShape(btInertialToCollision, shape.get()); collider->setCollisionShape(linkInfo->shape.get()); - collider->setRestitution(restitution); - collider->setRollingFriction(rollingFriction); - collider->setFriction(mu); + collider->setRestitution(static_cast(restitution)); + collider->setRollingFriction(static_cast(rollingFriction)); + collider->setFriction(static_cast(mu)); collider->setAnisotropicFriction( - btVector3(mu, mu2, 1), + btVector3(static_cast(mu), static_cast(mu2), 1), btCollisionObject::CF_ANISOTROPIC_FRICTION); linkInfo->collider = std::move(collider); @@ -928,6 +933,10 @@ bool SDFFeatures::AddSdfCollision( btBroadphaseProxy::AllFilter); } } + else if (linkInfo->shape) + { + linkInfo->shape->addChildShape(btInertialToCollision, shape.get()); + } else { // TODO(MXG): Maybe we should check if the new collider's properties diff --git a/bullet-featherstone/src/ShapeFeatures.cc b/bullet-featherstone/src/ShapeFeatures.cc index 34096292f..9e9f3107a 100644 --- a/bullet-featherstone/src/ShapeFeatures.cc +++ b/bullet-featherstone/src/ShapeFeatures.cc @@ -88,8 +88,7 @@ Identity ShapeFeatures::AttachBoxShape( const LinearVector3d &_size, const Pose3d &_pose) { - const auto size = math::eigen3::convert(_size); - const btVector3 halfExtents = btVector3(size.X(), size.Y(), size.Z()) * 0.5; + const btVector3 halfExtents = convertVec(_size * 0.5); std::unique_ptr shape = std::make_unique(halfExtents); @@ -167,8 +166,8 @@ Identity ShapeFeatures::AttachCapsuleShape( const double _length, const Pose3d &_pose) { - auto shape = - std::make_unique(_radius, _length / 2); + auto shape = std::make_unique( + static_cast(_radius), static_cast(_length / 2)); auto identity = this->AddCollision( CollisionInfo{ @@ -244,8 +243,8 @@ Identity ShapeFeatures::AttachCylinderShape( const double _height, const Pose3d &_pose) { - const auto radius = _radius; - const auto halfLength = _height * 0.5; + const auto radius = static_cast(_radius); + const auto halfLength = static_cast(_height * 0.5); auto shape = std::make_unique(btVector3(radius, radius, halfLength)); @@ -308,12 +307,12 @@ Identity ShapeFeatures::AttachEllipsoidShape( { btVector3 positions[1]; btScalar radius[1]; - positions[0] = btVector3(); + positions[0] = btVector3(0, 0, 0); radius[0] = 1; auto btSphere = std::make_unique( positions, radius, 1); - btSphere->setLocalScaling(btVector3(_radii[0], _radii[1], _radii[2])); + btSphere->setLocalScaling(convertVec(_radii)); auto shape = std::move(btSphere); auto identity = this->AddCollision( @@ -367,7 +366,7 @@ Identity ShapeFeatures::AttachSphereShape( const Pose3d &_pose) { std::unique_ptr shape = - std::make_unique(_radius); + std::make_unique(static_cast(_radius)); auto identity = this->AddCollision( CollisionInfo{ diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index 07f558438..a34a6741c 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -19,6 +19,7 @@ #include +#include #include #include @@ -42,13 +43,15 @@ void SimulationFeatures::WorldForwardStep( stepSize = dt.count(); } - worldInfo->world->stepSimulation(this->stepSize, 1, this->stepSize); + worldInfo->world->stepSimulation(static_cast(this->stepSize), 1, + static_cast(this->stepSize)); for (auto & m : this->models) { if (m.second->body) { - m.second->body->checkMotionAndSleepIfRequired(this->stepSize); + m.second->body->checkMotionAndSleepIfRequired( + static_cast(this->stepSize)); btMultiBodyLinkCollider* col = m.second->body->getBaseCollider(); if (col && col->getActivationState() != DISABLE_DEACTIVATION) col->setActivationState(ACTIVE_TAG); @@ -86,8 +89,8 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const dynamic_cast(contactManifold->getBody0()); const btMultiBodyLinkCollider* obB = dynamic_cast(contactManifold->getBody1()); - std::size_t collision1ID = -1; - std::size_t collision2ID = -1; + std::size_t collision1ID = std::numeric_limits::max(); + std::size_t collision2ID = std::numeric_limits::max(); for (const auto & link : this->links) { diff --git a/bullet-featherstone/src/WorldFeatures.cc b/bullet-featherstone/src/WorldFeatures.cc index 30250004f..c685c5784 100644 --- a/bullet-featherstone/src/WorldFeatures.cc +++ b/bullet-featherstone/src/WorldFeatures.cc @@ -31,8 +31,7 @@ void WorldFeatures::SetWorldGravity( { auto worldInfo = this->ReferenceInterface(_id); if (worldInfo) - worldInfo->world->setGravity( - btVector3(_gravity(0), _gravity(1), _gravity(2))); + worldInfo->world->setGravity(convertVec(_gravity)); } ///////////////////////////////////////////////// diff --git a/bullet/src/Base.hh b/bullet/src/Base.hh index 16de10706..743a3e853 100644 --- a/bullet/src/Base.hh +++ b/bullet/src/Base.hh @@ -117,14 +117,24 @@ struct JointInfo inline btMatrix3x3 convertMat(Eigen::Matrix3d mat) { - return btMatrix3x3(mat(0, 0), mat(0, 1), mat(0, 2), - mat(1, 0), mat(1, 1), mat(1, 2), - mat(2, 0), mat(2, 1), mat(2, 2)); + return btMatrix3x3( + static_cast(mat(0, 0)), static_cast(mat(0, 1)), + static_cast(mat(0, 2)), static_cast(mat(1, 0)), + static_cast(mat(1, 1)), static_cast(mat(1, 2)), + static_cast(mat(2, 0)), static_cast(mat(2, 1)), + static_cast(mat(2, 2))); } inline btVector3 convertVec(Eigen::Vector3d vec) { - return btVector3(vec(0), vec(1), vec(2)); + return btVector3(static_cast(vec(0)), static_cast(vec(1)), + static_cast(vec(2))); +} + +inline btVector3 convertVec(const math::Vector3d& vec) +{ + return btVector3(static_cast(vec[0]), static_cast(vec[1]), + static_cast(vec[2])); } inline Eigen::Matrix3d convert(btMatrix3x3 mat) @@ -171,15 +181,6 @@ class Base : public Implements3d> return entityCount++; } - public: inline Identity InitiateEngine(std::size_t /*_engineID*/) override - { - const auto id = this->GetNextEntity(); - assert(id == 0); - (void)id; - - return this->GenerateIdentity(0); - } - public: inline Identity AddWorld(WorldInfo _worldInfo) { const auto id = this->GetNextEntity(); diff --git a/bullet/src/JointFeatures.cc b/bullet/src/JointFeatures.cc index 8b01b03f3..1942e6582 100644 --- a/bullet/src/JointFeatures.cc +++ b/bullet/src/JointFeatures.cc @@ -332,8 +332,10 @@ void JointFeatures::SetJointForce( hinge->getRigidBodyB().getWorldTransform().getBasis() * hingeAxisLocalB; - btVector3 hingeTorqueA = _value * hingeAxisWorldA; - btVector3 hingeTorqueB = _value * hingeAxisWorldB; + btVector3 hingeTorqueA = + static_cast(_value) * hingeAxisWorldA; + btVector3 hingeTorqueB = + static_cast(_value) * hingeAxisWorldB; hinge->getRigidBodyA().applyTorque(hingeTorqueA); hinge->getRigidBodyB().applyTorque(-hingeTorqueB); @@ -374,7 +376,7 @@ void JointFeatures::SetJointVelocityCommand( link->getMotionState()->getWorldTransform(trans); btVector3 motion = quatRotate(trans.getRotation(), convertVec(gz::math::eigen3::convert(jointInfo->axis))); - btVector3 angular_vel = motion * _value; + btVector3 angular_vel = motion * static_cast(_value); link->setAngularVelocity(angular_vel); } break; diff --git a/bullet/src/SDFFeatures.cc b/bullet/src/SDFFeatures.cc index 4fced5fd7..efdb37b67 100644 --- a/bullet/src/SDFFeatures.cc +++ b/bullet/src/SDFFeatures.cc @@ -69,8 +69,7 @@ Identity SDFFeatures::ConstructSdfWorld( const WorldInfoPtr &worldInfo = this->worlds.at(worldID); - auto gravity = _sdfWorld.Gravity(); - worldInfo->world->setGravity(btVector3(gravity[0], gravity[1], gravity[2])); + worldInfo->world->setGravity(convertVec(_sdfWorld.Gravity())); for (std::size_t i=0; i < _sdfWorld.ModelCount(); ++i) { @@ -147,7 +146,7 @@ Identity SDFFeatures::ConstructSdfLink( const std::string name = _sdfLink.Name(); const math::Pose3d pose = ResolveSdfPose(_sdfLink.SemanticPose()); const gz::math::Inertiald inertial = _sdfLink.Inertial(); - double mass = inertial.MassMatrix().Mass(); + btScalar mass = static_cast(inertial.MassMatrix().Mass()); math::Pose3d inertialPose = inertial.Pose(); inertialPose.Rot() *= inertial.MassMatrix().PrincipalAxesOffset(); const auto diagonalMoments = inertial.MassMatrix().PrincipalMoments(); @@ -226,14 +225,14 @@ Identity SDFFeatures::ConstructSdfCollision( else if (geom->SphereShape()) { const auto sphere = geom->SphereShape(); - const auto radius = sphere->Radius(); + const auto radius = static_cast(sphere->Radius()); shape = std::make_shared(radius); } else if (geom->CylinderShape()) { const auto cylinder = geom->CylinderShape(); - const auto radius = cylinder->Radius(); - const auto halfLength = cylinder->Length()*0.5; + const auto radius = static_cast(cylinder->Radius()); + const auto halfLength = static_cast(cylinder->Length() * 0.5); shape = std::make_shared(btVector3(radius, radius, halfLength)); } @@ -241,27 +240,28 @@ Identity SDFFeatures::ConstructSdfCollision( { const auto plane = geom->PlaneShape(); const auto normal = convertVec(math::eigen3::convert(plane->Normal())); - shape = std::make_shared(normal, 0); + shape = std::make_shared(normal, 0.0f); } else if (geom->CapsuleShape()) { const auto capsule = geom->CapsuleShape(); shape = std::make_shared( - capsule->Radius(), capsule->Length()); + static_cast(capsule->Radius()), + static_cast(capsule->Length())); } else if (geom->EllipsoidShape()) { btVector3 positions[1]; btScalar radius[1]; - positions[0] = btVector3(); + positions[0] = btVector3(0, 0, 0); radius[0] = 1; const auto ellipsoid = geom->EllipsoidShape(); - const auto radii = ellipsoid->Radii(); + const auto radii = convertVec(ellipsoid->Radii()); shape = std::make_shared( positions, radius, 1); std::dynamic_pointer_cast(shape)->setLocalScaling( - btVector3(radii.X(), radii.Y(), radii.Z())); + radii); } // TODO(lobotuerk/blast545) Add additional friction parameters for bullet @@ -450,8 +450,9 @@ Identity SDFFeatures::ConstructSdfJoint( if (_sdfJoint.Axis(0) != nullptr) { double friction = _sdfJoint.Axis(0)->Friction(); - joint->enableAngularMotor(true, 0.0, friction); - joint->setLimit(_sdfJoint.Axis(0)->Lower(), _sdfJoint.Axis(0)->Upper()); + joint->enableAngularMotor(true, 0.0, static_cast(friction)); + joint->setLimit(static_cast(_sdfJoint.Axis(0)->Lower()), + static_cast(_sdfJoint.Axis(0)->Upper())); } else { diff --git a/bullet/src/ShapeFeatures.cc b/bullet/src/ShapeFeatures.cc index a90bff671..c6bb407bf 100644 --- a/bullet/src/ShapeFeatures.cc +++ b/bullet/src/ShapeFeatures.cc @@ -52,17 +52,17 @@ Identity ShapeFeatures::AttachMeshShape( // Create the Bullet trimesh for (unsigned int j = 0; j < numIndices; j += 3) { - btVector3 bv0(vertices[indices[j]*3+0], - vertices[indices[j]*3+1], - vertices[indices[j]*3+2]); + btVector3 bv0(static_cast(vertices[indices[j]*3+0]), + static_cast(vertices[indices[j]*3+1]), + static_cast(vertices[indices[j]*3+2])); - btVector3 bv1(vertices[indices[j+1]*3+0], - vertices[indices[j+1]*3+1], - vertices[indices[j+1]*3+2]); + btVector3 bv1(static_cast(vertices[indices[j+1]*3+0]), + static_cast(vertices[indices[j+1]*3+1]), + static_cast(vertices[indices[j+1]*3+2])); - btVector3 bv2(vertices[indices[j+2]*3+0], - vertices[indices[j+2]*3+1], - vertices[indices[j+2]*3+2]); + btVector3 bv2(static_cast(vertices[indices[j+2]*3+0]), + static_cast(vertices[indices[j+2]*3+1]), + static_cast(vertices[indices[j+2]*3+2])); mTriMesh.get()->addTriangle(bv0, bv1, bv2); } diff --git a/bullet/src/SimulationFeatures.cc b/bullet/src/SimulationFeatures.cc index a5d1b15f4..c80c1c394 100644 --- a/bullet/src/SimulationFeatures.cc +++ b/bullet/src/SimulationFeatures.cc @@ -41,7 +41,8 @@ void SimulationFeatures::WorldForwardStep( stepSize = dt.count(); } - worldInfo->world->stepSimulation(this->stepSize, 1, this->stepSize); + worldInfo->world->stepSimulation(static_cast(this->stepSize), 1, + static_cast(this->stepSize)); this->WriteRequiredData(_h); this->Write(_h.Get()); } diff --git a/bullet/src/plugin.cc b/bullet/src/plugin.cc index 21521da9c..576c95dcf 100644 --- a/bullet/src/plugin.cc +++ b/bullet/src/plugin.cc @@ -53,7 +53,15 @@ class Plugin : public virtual SDFFeatures, public virtual ShapeFeatures, public virtual JointFeatures -{}; +{ + public: Identity InitiateEngine(std::size_t /*_engineID*/) override + { + const auto id = this->GetNextEntity(); + assert(id == 0); + + return this->GenerateIdentity(0); + } +}; GZ_PHYSICS_ADD_PLUGIN(Plugin, FeaturePolicy3d, BulletFeatures) diff --git a/dartsim/CMakeLists.txt b/dartsim/CMakeLists.txt index c8756ab83..2b328b09d 100644 --- a/dartsim/CMakeLists.txt +++ b/dartsim/CMakeLists.txt @@ -83,6 +83,7 @@ gz_build_tests( SOURCES ${test_sources} LIB_DEPS ${features} + ${dartsim_plugin} gz-plugin${GZ_PLUGIN_VER}::loader gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} gz-common${GZ_COMMON_VER}::geospatial diff --git a/dartsim/include/ignition/physics/dartsim/World.hh b/dartsim/include/ignition/physics/dartsim/World.hh index 6deedbc44..1577b15a8 100644 --- a/dartsim/include/ignition/physics/dartsim/World.hh +++ b/dartsim/include/ignition/physics/dartsim/World.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/dartsim/src/AddedMassFeatures.cc b/dartsim/src/AddedMassFeatures.cc new file mode 100644 index 000000000..5e72be166 --- /dev/null +++ b/dartsim/src/AddedMassFeatures.cc @@ -0,0 +1,91 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "AddedMassFeatures.hh" +#include + +namespace gz::physics::dartsim +{ + +void AddedMassFeatures::SetLinkAddedMass(const Identity &_link, + const gz::math::Matrix6d &_addedMass) +{ + auto linkInfo = this->ReferenceInterface(_link); + auto bn = linkInfo->link; + + if (linkInfo->inertial.has_value()) + { + auto &sdfInertia = linkInfo->inertial.value(); + sdfInertia.SetFluidAddedMass(_addedMass); + + dart::dynamics::Inertia newInertia; + newInertia.setMass(sdfInertia.MassMatrix().Mass()); + + const Eigen::Matrix3d I_link = math::eigen3::convert(sdfInertia.Moi()); + newInertia.setMoment(I_link); + + const Eigen::Vector3d localCom = + math::eigen3::convert(sdfInertia.Pose().Pos()); + newInertia.setLocalCOM(localCom); + + if (sdfInertia.FluidAddedMass().has_value()) + { + // Note that the ordering of the spatial inertia matrix used in DART is + // different than the one used in Gazebo and SDF. + math::Matrix6d featherstoneMatrix; + featherstoneMatrix.SetSubmatrix(math::Matrix6d::TOP_LEFT, + sdfInertia.FluidAddedMass().value().Submatrix( + math::Matrix6d::BOTTOM_RIGHT)); + featherstoneMatrix.SetSubmatrix(math::Matrix6d::TOP_RIGHT, + sdfInertia.FluidAddedMass().value().Submatrix( + math::Matrix6d::BOTTOM_LEFT)); + featherstoneMatrix.SetSubmatrix(math::Matrix6d::BOTTOM_LEFT, + sdfInertia.FluidAddedMass().value().Submatrix( + math::Matrix6d::TOP_RIGHT)); + featherstoneMatrix.SetSubmatrix(math::Matrix6d::BOTTOM_RIGHT, + sdfInertia.FluidAddedMass().value().Submatrix( + math::Matrix6d::TOP_LEFT)); + + // If using added mass, gravity needs to be applied as a separate + // force at the center of mass using F=ma; + bn->setGravityMode(false); + + newInertia.setSpatialTensor( + newInertia.getSpatialTensor() + + math::eigen3::convert(featherstoneMatrix)); + } + bn->setInertia(newInertia); + } +} + +gz::math::Matrix6d +AddedMassFeatures::GetLinkAddedMass(const Identity &_link) const +{ + auto linkInfo = this->ReferenceInterface(_link); + + if (linkInfo->inertial.has_value() && + linkInfo->inertial->FluidAddedMass().has_value()) + { + return linkInfo->inertial->FluidAddedMass().value(); + } + else + { + return gz::math::Matrix6d::Zero; + } +} + +} // namespace gz::physics::dartsim diff --git a/dartsim/src/AddedMassFeatures.hh b/dartsim/src/AddedMassFeatures.hh new file mode 100644 index 000000000..799eb4a18 --- /dev/null +++ b/dartsim/src/AddedMassFeatures.hh @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef GZ_PHYSICS_DARTSIM_SRC_ADDEDMASSFEATURES_HH_ +#define GZ_PHYSICS_DARTSIM_SRC_ADDEDMASSFEATURES_HH_ + +#include + +#include "Base.hh" + +namespace gz::physics::dartsim +{ + +struct AddedMassFeatureList: FeatureList< + AddedMass +> { }; + +class AddedMassFeatures : + public virtual Base, + public virtual Implements3d +{ + public: void SetLinkAddedMass(const Identity &_link, + const gz::math::Matrix6d &_addedMass) override; + + public: gz::math::Matrix6d GetLinkAddedMass( + const Identity &_link) const override; +}; +} // namespace gz::physics::dartsim + +#endif // GZ_PHYSICS_DARTSIM_SRC_ADDEDMASSFEATURES_HH_ diff --git a/dartsim/src/AddedMassFeatures_TEST.cc b/dartsim/src/AddedMassFeatures_TEST.cc new file mode 100644 index 000000000..1eec689c5 --- /dev/null +++ b/dartsim/src/AddedMassFeatures_TEST.cc @@ -0,0 +1,182 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include + +struct TestFeatureList : gz::physics::FeatureList< + gz::physics::GetEntities, + gz::physics::GetBasicJointState, + gz::physics::SetBasicJointState, + gz::physics::LinkFrameSemantics, + gz::physics::dartsim::RetrieveWorld, + gz::physics::sdf::ConstructSdfLink, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::AddedMass +> { }; + +using World = gz::physics::World3d; +using WorldPtr = gz::physics::World3dPtr; +using ModelPtr = gz::physics::Model3dPtr; +using LinkPtr = gz::physics::Link3dPtr; + +///////////////////////////////////////////////// +auto LoadEngine() +{ + gz::plugin::Loader loader; + loader.LoadLib(dartsim_plugin_LIB); + + gz::plugin::PluginPtr dartsim = + loader.Instantiate("gz::physics::dartsim::Plugin"); + + auto engine = + gz::physics::RequestEngine3d::From(dartsim); + return engine; +} + +///////////////////////////////////////////////// +WorldPtr LoadWorld(const std::string &_world) +{ + auto engine = LoadEngine(); + EXPECT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors &errors = root.Load(_world); + EXPECT_EQ(0u, errors.size()); + for (const auto & error : errors) { + std::cout << error << std::endl; + } + + EXPECT_EQ(1u, root.WorldCount()); + const sdf::World *sdfWorld = root.WorldByIndex(0); + EXPECT_NE(nullptr, sdfWorld); + + auto world = engine->ConstructWorld(*sdfWorld); + EXPECT_NE(nullptr, world); + + return world; +} + +///////////////////////////////////////////////// +TEST(AddedMassFeatures, AddedMass) +{ + // Expected spatial inertia matrix. This includes inertia due to the body's + // mass and added mass. Note that the ordering of the matrix is different + // than the one used in SDF. + Eigen::Matrix6d expectedSpatialInertia; + expectedSpatialInertia << + 17, 17, 18, 4, 9, 13, + 17, 20, 20, 5, 10, 14, + 18, 20, 22, 6, 11, 15, + 4, 5, 6, 2, 2, 3, + 9, 10, 11, 2, 8, 8, + 13, 14, 15, 3, 8, 13; + + // Expected spatial inertia matrix. This includes inertia due to the body's + // mass and added mass. Note that the ordering of the matrix is different + // than the one used in SDF. + Eigen::Matrix6d expectedZeroSpatialInertia; + expectedZeroSpatialInertia << + 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1; + + auto world = LoadWorld(TEST_WORLD_DIR"/added_mass.sdf"); + ASSERT_NE(nullptr, world); + + auto dartWorld = world->GetDartsimWorld(); + ASSERT_NE(nullptr, dartWorld); + + ASSERT_EQ(3u, dartWorld->getNumSkeletons()); + + { + const auto skeleton = dartWorld->getSkeleton("body_no_added_mass"); + ASSERT_NE(skeleton, nullptr); + + ASSERT_EQ(1u, skeleton->getNumBodyNodes()); + const dart::dynamics::BodyNode *link = skeleton->getBodyNode("link"); + ASSERT_NE(link, nullptr); + + const Eigen::Matrix6d spatialInertia = link->getSpatialInertia(); + ASSERT_TRUE(expectedZeroSpatialInertia.isApprox(spatialInertia)); + + const auto linkAddedMass = + world->GetModel("body_no_added_mass")->GetLink("link")->GetAddedMass(); + ASSERT_TRUE(Eigen::Matrix6d::Zero().isApprox( + gz::math::eigen3::convert(linkAddedMass))); + } + + { + const auto skeleton = dartWorld->getSkeleton("body_zero_added_mass"); + ASSERT_NE(skeleton, nullptr); + + ASSERT_EQ(1u, skeleton->getNumBodyNodes()); + const dart::dynamics::BodyNode *link = skeleton->getBodyNode("link"); + ASSERT_NE(link, nullptr); + + const Eigen::Matrix6d spatialInertia = link->getSpatialInertia(); + ASSERT_TRUE(expectedZeroSpatialInertia.isApprox(spatialInertia)); + + auto linkAddedMass = + world->GetModel("body_zero_added_mass")->GetLink("link")->GetAddedMass(); + ASSERT_TRUE(Eigen::Matrix6d::Zero().isApprox( + gz::math::eigen3::convert(linkAddedMass))); + } + + { + const auto skeleton = dartWorld->getSkeleton("body_added_mass"); + ASSERT_NE(skeleton, nullptr); + + ASSERT_EQ(1u, skeleton->getNumBodyNodes()); + const dart::dynamics::BodyNode *link = skeleton->getBodyNode("link"); + ASSERT_NE(link, nullptr); + + const Eigen::Matrix6d spatialInertia = link->getSpatialInertia(); + ASSERT_TRUE(expectedSpatialInertia.isApprox(spatialInertia)); + + auto linkAddedMass = + world->GetModel("body_added_mass")->GetLink("link")->GetAddedMass(); + ASSERT_FALSE(Eigen::Matrix6d::Zero().isApprox( + gz::math::eigen3::convert(linkAddedMass))); + } +} diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index 2e9a630d4..6c54f9b49 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -41,6 +41,13 @@ #include +#if DART_VERSION_AT_LEAST(6, 13, 0) +// The BodyNode::getShapeNodes method was deprecated in dart 6.13.0 +// in favor of an iterator approach with BodyNode::eachShapeNode +// See https://github.com/dartsim/dart/pull/1644 for more info +#define DART_HAS_EACH_SHAPE_NODE_API +#endif + namespace gz { namespace physics { namespace dartsim { @@ -75,6 +82,12 @@ struct LinkInfo std::optional inertial; }; +struct JointInfo +{ + dart::dynamics::JointPtr joint; + dart::dynamics::SimpleFramePtr frame; +}; + struct ModelInfo { dart::dynamics::SkeletonPtr model; @@ -82,20 +95,15 @@ struct ModelInfo dart::dynamics::SimpleFramePtr frame; std::string canonicalLinkName; std::vector> links {}; + std::vector> joints {}; std::vector nestedModels = {}; }; -struct JointInfo -{ - dart::dynamics::JointPtr joint; - dart::dynamics::SimpleFramePtr frame; -}; - struct ShapeInfo { dart::dynamics::ShapeNodePtr node; - /// \brief dartsim has more strict name uniqueness rules tha Gazebo, so we + /// \brief dartsim has more strict name uniqueness rules than Gazebo, so we /// store the Gazebo-specified name of the Shape here. std::string name; @@ -183,6 +191,20 @@ struct EntityStorage return idToObject.find(_id) != idToObject.end(); } + void AddEntity(std::size_t _id, const Value1 &_value1, const Key2 &_key, + std::size_t _containerID) + { + this->idToObject[_id] = _value1; + this->objectToID[_key] = _id; + std::vector &indexInContainerToIDVector = + this->indexInContainerToID[_containerID]; + const std::size_t indexInContainer = indexInContainerToIDVector.size(); + + this->idToIndexInContainer[_id] = indexInContainer; + indexInContainerToIDVector.push_back(_id); + this->idToContainerID[_id] = _containerID; + } + bool RemoveEntity(const Key2 &_key) { auto entIter = this->objectToID.find(_key); @@ -267,19 +289,9 @@ class Base : public Implements3d> const DartWorldPtr &_world, const std::string &_name) { const std::size_t id = this->GetNextEntity(); - - this->worlds.idToObject[id] = _world; - this->worlds.objectToID[_name] = id; - - std::vector &indexInContainerToID = - this->worlds.indexInContainerToID.at(0); - - this->worlds.idToIndexInContainer[id] = indexInContainerToID.size(); - indexInContainerToID.push_back(id); - - this->worlds.idToContainerID[id] = 0; - _world->setName(_name); + this->worlds.AddEntity(id, _world, _name, 0); + this->frames[id] = dart::dynamics::Frame::World(); return id; @@ -289,23 +301,17 @@ class Base : public Implements3d> const ModelInfo &_info, const std::size_t _worldID) { const std::size_t id = this->GetNextEntity(); - this->models.idToObject[id] = std::make_shared(_info); - ModelInfo &entry = *this->models.idToObject[id]; - this->models.objectToID[_info.model] = id; + auto entry = std::make_shared(_info); const dart::simulation::WorldPtr &world = worlds[_worldID]; + world->addSkeleton(entry->model); + this->models.AddEntity(id, entry, _info.model, _worldID); + if (_info.frame) + { + this->frames[id] = _info.frame.get(); + } - std::vector &indexInContainerToID = - this->models.indexInContainerToID[_worldID]; - const std::size_t indexInWorld = indexInContainerToID.size(); - this->models.idToIndexInContainer[id] = indexInWorld; - indexInContainerToID.push_back(id); - world->addSkeleton(entry.model); - - this->models.idToContainerID[id] = _worldID; - this->frames[id] = _info.frame.get(); - - return std::forward_as_tuple(id, entry); + return std::forward_as_tuple(id, *entry); } public: inline std::tuple AddNestedModel( @@ -313,25 +319,17 @@ class Base : public Implements3d> const std::size_t _worldID) { const std::size_t id = this->GetNextEntity(); - this->models.idToObject[id] = std::make_shared(_info); - ModelInfo &entry = *this->models.idToObject[id]; - this->models.objectToID[_info.model] = id; + auto entry = std::make_shared(_info); const dart::simulation::WorldPtr &world = worlds[_worldID]; + world->addSkeleton(entry->model); - auto parentModelInfo = this->models.at(_parentID); - const std::size_t indexInModel = - parentModelInfo->nestedModels.size(); - this->models.idToIndexInContainer[id] = indexInModel; - std::vector &indexInContainerToID = - this->models.indexInContainerToID[_parentID]; - indexInContainerToID.push_back(id); - world->addSkeleton(entry.model); - - this->models.idToContainerID[id] = _parentID; + this->models.AddEntity(id, entry, _info.model, _parentID); this->frames[id] = _info.frame.get(); + + auto parentModelInfo = this->models.at(_parentID); parentModelInfo->nestedModels.push_back(id); - return {id, entry}; + return {id, *entry}; } public: inline std::size_t AddLink(DartBodyNode *_bn, @@ -340,7 +338,6 @@ class Base : public Implements3d> { const std::size_t id = this->GetNextEntity(); auto linkInfo = std::make_shared(); - this->links.idToObject[id] = linkInfo; linkInfo->link = _bn; // The name of the BodyNode during creation is assumed to be the // Gazebo-specified name. @@ -348,22 +345,12 @@ class Base : public Implements3d> // Inertial properties (if available) used when splitting nodes to close // kinematic loops. linkInfo->inertial = _inertial; - this->links.objectToID[_bn] = id; + this->links.AddEntity(id, linkInfo, _bn, _modelID); this->frames[id] = _bn; this->linksByName[_fullName] = _bn; this->models.at(_modelID)->links.push_back(linkInfo); - // Even though DART keeps track of the index of this BodyNode in the - // skeleton, the BodyNode may be moved to another skeleton when a joint is - // constructed. Thus, we store the original index here. - this->links.idToIndexInContainer[id] = _bn->getIndexInSkeleton(); - std::vector &indexInContainerToID = - this->links.indexInContainerToID[_modelID]; - indexInContainerToID.push_back(id); - - this->links.idToContainerID[id] = _modelID; - return id; } @@ -481,17 +468,21 @@ class Base : public Implements3d> } } - public: inline std::size_t AddJoint(DartJoint *_joint) + public: inline std::size_t AddJoint(DartJoint *_joint, + const std::string &_fullName, std::size_t _modelID) { const std::size_t id = this->GetNextEntity(); - this->joints.idToObject[id] = std::make_shared(); - this->joints.idToObject[id]->joint = _joint; - this->joints.objectToID[_joint] = id; + auto jointInfo = std::make_shared(); + jointInfo->joint = _joint; + this->joints.AddEntity(id, jointInfo, _joint, _modelID); + dart::dynamics::SimpleFramePtr jointFrame = dart::dynamics::SimpleFrame::createShared( _joint->getChildBodyNode(), _joint->getName() + "_frame", _joint->getTransformFromChildBodyNode()); + this->jointsByName[_fullName] = _joint; + this->models.at(_modelID)->joints.push_back(jointInfo); this->joints.idToObject[id]->frame = jointFrame; this->frames[id] = this->joints.idToObject[id]->frame.get(); @@ -525,13 +516,22 @@ class Base : public Implements3d> for (auto &jt : skel->getJoints()) { this->joints.RemoveEntity(jt); + this->jointsByName.erase(::sdf::JoinName( + world->getName(), ::sdf::JoinName(skel->getName(), jt->getName()))); } for (auto &bn : skel->getBodyNodes()) { +#ifdef DART_HAS_EACH_SHAPE_NODE_API + bn->eachShapeNode([this](dart::dynamics::ShapeNode *_sn) + { + this->shapes.RemoveEntity(_sn); + }); +#else for (auto &sn : bn->getShapeNodes()) { this->shapes.RemoveEntity(sn); } +#endif this->links.RemoveEntity(bn); this->linksByName.erase(::sdf::JoinName( world->getName(), ::sdf::JoinName(skel->getName(), bn->getName()))); @@ -573,6 +573,47 @@ class Base : public Implements3d> return this->GenerateInvalidId(); } + public: inline Identity GetModelOfLinkImpl(const Identity &_linkID) const + { + const std::size_t modelID = this->links.idToContainerID.at(_linkID); + if (this->models.HasEntity(modelID)) + { + return this->GenerateIdentity(modelID, this->models.at(modelID)); + } + else + { + return this->GenerateInvalidId(); + } + }; + + /// \brief Create a fully (world) scoped joint name. + /// \param _modelID Identity of the parent model of the joint's child link. + /// \param _name The unscoped joint name. + /// \return The fully (world) scoped joint name, or an empty string + /// if a world cannot be resolved. + public: inline std::string FullyScopedJointName( + const Identity &_modelID, + const std::string &_name) const + { + const auto modelInfo = this->ReferenceInterface(_modelID); + + auto worldID = this->GetWorldOfModelImpl(_modelID); + if (worldID == INVALID_ENTITY_ID) + { + gzerr << "World of model [" << modelInfo->model->getName() + << "] could not be found when creating joint [" << _name + << "]\n"; + return ""; + } + + auto world = this->worlds.at(worldID); + const std::string fullJointName = ::sdf::JoinName( + world->getName(), + ::sdf::JoinName(modelInfo->model->getName(), _name)); + + return fullJointName; + } + public: EntityStorage worlds; public: EntityStorage models; public: EntityStorage links; @@ -585,9 +626,82 @@ class Base : public Implements3d> /// as they move to other skeletons. public: std::unordered_map linksByName; + /// \brief Map from the fully qualified joint name (including the world name) + /// to the dart Joint object. This is useful for keeping track of + /// dart Joints even as they move to other skeletons. + public: std::unordered_map jointsByName; + /// \brief Map from welded body nodes to the LinkInfo for the original link /// they are welded to. This is useful when detaching joints. public: std::unordered_map linkByWeldedNode; + + /// \brief A debug function to list the models and their immediate + /// nested models, links and joints. + /// \return A string containing the list of model information. + public: std::string DebugModels() const + { + std::stringstream ss; + ss << "*** Models ***\n"; + for (size_t id = 0, i = 0; i < models.size(); ++id) + { + if (models.HasEntity(id)) + { + ++i; + auto modelInfo = models.at(id); + ss << "ModelID: " << id << "\n" + << "LocalName: " << modelInfo->localName << "\n" + << "NodeName: " << modelInfo->model->getName() << "\n" + << "NumModels: " << modelInfo->nestedModels.size() << "\n" + << "NumLinks: " << modelInfo->links.size() << "\n" + << "NumJoints: " << modelInfo->model->getNumJoints() << "\n"; + for (auto& joint : modelInfo->model->getJoints()) + { + ss << " Joint: " << joint->getName() << "\n"; + } + } + } + return ss.str(); + } + + /// \brief A debug function to list the links and their names. + /// \return A string containing the list of link information. + public: std::string DebugLinks() const + { + std::stringstream ss; + ss << "*** Links ***\n"; + for (size_t id = 0, i = 0; i < links.size(); ++id) + { + if (links.HasEntity(id)) + { + ++i; + auto linkInfo = links.at(id); + ss << "LinkID " << id << "\n" + << "Name: " << linkInfo->name << "\n" + << "NodeName: " << linkInfo->link->getName() << "\n"; + } + } + return ss.str(); + } + + /// \brief A debug function to list the joints and their names. + /// \return A string containing the list of joint information. + public: std::string DebugJoints() const + { + std::stringstream ss; + ss << "*** Joints ***\n"; + for (size_t id = 0, i = 0; i < joints.size(); ++id) + { + if (joints.HasEntity(id)) + { + ++i; + auto jointInfo = joints.at(id); + ss << "JointID " << id << "\n" + << "NodeName: " << jointInfo->joint->getName() << "\n"; + } + } + return ss.str(); + } + }; } diff --git a/dartsim/src/Base_TEST.cc b/dartsim/src/Base_TEST.cc index 1c038cf9f..a9f8d156a 100644 --- a/dartsim/src/Base_TEST.cc +++ b/dartsim/src/Base_TEST.cc @@ -26,6 +26,9 @@ #include #include +#include +#include + #include "dart/dynamics/BoxShape.hpp" #include "dart/dynamics/FreeJoint.hpp" #include "dart/dynamics/Skeleton.hpp" @@ -78,7 +81,12 @@ TEST(BaseClass, RemoveModel) EXPECT_EQ(pair.second->getName(), linkInfo->name); EXPECT_EQ(pair.second, linkInfo->link); - base.AddJoint(pair.first); + auto modelID = base.models.IdentityOf(modelInfo->model); + const std::string fullJointName = ::sdf::JoinName( + world->getName(), + ::sdf::JoinName(modelInfo->model->getName(), pair.first->getName())); + + base.AddJoint(pair.first, fullJointName, modelID); base.AddShape({sn, name + "_shape"}); modelIDs[name] = std::get<0>(res); @@ -148,6 +156,49 @@ TEST(BaseClass, RemoveModel) EXPECT_EQ(0u, curSize); } + +TEST(BaseClass, SdfConstructionBookkeeping) +{ + dartsim::SDFFeatures sdfFeatures; + auto engineId = sdfFeatures.InitiateEngine(0); + auto worldID = sdfFeatures.ConstructEmptyWorld(engineId, "default"); + + ::sdf::Root root; + + auto errors = root.Load(GZ_PHYSICS_RESOURCE_DIR "/rrbot.xml"); + ASSERT_TRUE(errors.empty()); + const ::sdf::Model *sdfModel = root.Model(); + ASSERT_NE(nullptr, sdfModel); + + auto modelID = sdfFeatures.ConstructSdfModel(worldID, *sdfModel); + EXPECT_EQ(1u, sdfFeatures.models.size()); + EXPECT_EQ(sdfModel->LinkCount(), sdfFeatures.links.size()); + EXPECT_EQ(sdfModel->LinkCount(), sdfFeatures.linksByName.size()); + EXPECT_EQ(sdfModel->JointCount(), sdfFeatures.joints.size()); + EXPECT_EQ(2u, sdfFeatures.shapes.size()); + + EXPECT_EQ(sdfModel->LinkCount(), sdfFeatures.GetLinkCount(modelID)); + EXPECT_EQ(sdfModel->JointCount(), sdfFeatures.GetJointCount(modelID)); + + for (uint64_t i = 0; i < sdfModel->LinkCount(); ++i) + { + EXPECT_EQ(sdfModel->LinkByIndex(i)->CollisionCount(), + sdfFeatures.GetShapeCount(sdfFeatures.GetLink(modelID, i))); + } + + for (uint64_t i =0; i < sdfModel->JointCount(); ++i) + { + auto jointID = sdfFeatures.GetJoint(modelID, i); + ASSERT_TRUE(jointID); + ASSERT_EQ(1u, sdfFeatures.joints.idToIndexInContainer.count(jointID)); + EXPECT_EQ(sdfFeatures.joints.idToIndexInContainer[jointID], i); + EXPECT_EQ(sdfFeatures.joints.idToContainerID[jointID], modelID.id); + + ASSERT_GE(sdfFeatures.joints.indexInContainerToID[modelID].size(), i); + EXPECT_EQ(sdfFeatures.joints.indexInContainerToID[modelID][i], jointID.id); + } +} + TEST(BaseClass, AddNestedModel) { dartsim::Base base; diff --git a/dartsim/src/CustomMeshShape.cc b/dartsim/src/CustomMeshShape.cc index 4e5a45805..f09fd11ae 100644 --- a/dartsim/src/CustomMeshShape.cc +++ b/dartsim/src/CustomMeshShape.cc @@ -30,11 +30,11 @@ namespace dartsim { namespace { ///////////////////////////////////////////////// unsigned int CheckNumVerticesPerFaces( - const gz::common::SubMesh &_inputSubmesh, + const common::SubMesh &_inputSubmesh, const unsigned int _submeshIndex, const std::string &_path) { - using namespace gz::common; + using namespace common; const SubMesh::PrimitiveType type = _inputSubmesh.SubMeshPrimitiveType(); @@ -76,9 +76,9 @@ unsigned int CheckNumVerticesPerFaces( ///////////////////////////////////////////////// unsigned int GetPrimitiveType( - const gz::common::SubMesh &_inputSubmesh) + const common::SubMesh &_inputSubmesh) { - using namespace gz::common; + using namespace common; const SubMesh::PrimitiveType type = _inputSubmesh.SubMeshPrimitiveType(); @@ -97,7 +97,7 @@ unsigned int GetPrimitiveType( ///////////////////////////////////////////////// CustomMeshShape::CustomMeshShape( - const gz::common::Mesh &_input, + const common::Mesh &_input, const Eigen::Vector3d &_scale) : dart::dynamics::MeshShape(_scale, nullptr) { @@ -123,7 +123,7 @@ CustomMeshShape::CustomMeshShape( // Fill in submesh contents for (unsigned int i = 0; i < numSubMeshes; ++i) { - const gz::common::SubMeshPtr &inputSubmesh = + const common::SubMeshPtr &inputSubmesh = _input.SubMeshByIndex(i).lock(); scene->mMeshes[i] = nullptr; @@ -205,11 +205,11 @@ CustomMeshShape::CustomMeshShape( for (unsigned int j = 0; j < numVertices; ++j) { - const gz::math::Vector3d &v = inputSubmesh->Vertex(j); + const math::Vector3d &v = inputSubmesh->Vertex(j); for (unsigned int k = 0; k < 3; ++k) mesh->mVertices[j][k] = static_cast(v[k]); - const gz::math::Vector3d &n = inputSubmesh->Normal(j); + const math::Vector3d &n = inputSubmesh->Normal(j); for (unsigned int k = 0; k < 3; ++k) mesh->mNormals[j][k] = static_cast(n[k]); } diff --git a/dartsim/src/CustomMeshShape.hh b/dartsim/src/CustomMeshShape.hh index 4a30b55f9..b25721bd3 100644 --- a/dartsim/src/CustomMeshShape.hh +++ b/dartsim/src/CustomMeshShape.hh @@ -26,12 +26,12 @@ namespace physics { namespace dartsim { /// \brief This class creates a custom derivative of dartsim's MeshShape class -/// which allows a gz::common::Mesh to be converted into a MeshShape that +/// which allows a common::Mesh to be converted into a MeshShape that /// can be used by dartsim. class CustomMeshShape : public dart::dynamics::MeshShape { public: CustomMeshShape( - const gz::common::Mesh &_input, + const common::Mesh &_input, const Eigen::Vector3d &_scale); }; diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 2e0d5e374..b656ea700 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -411,15 +411,18 @@ std::size_t EntityManagementFeatures::GetJointCount( Identity EntityManagementFeatures::GetJoint( const Identity &_modelID, const std::size_t _jointIndex) const { - DartJoint *const joint = - this->ReferenceInterface(_modelID)->model->getJoint( - _jointIndex); + auto modelInfo = this->ReferenceInterface(_modelID); + + if (_jointIndex >= modelInfo->joints.size()) + return this->GenerateInvalidId(); + + const auto &jointInfo = modelInfo->joints[_jointIndex]; // If the joint doesn't exist in "joints", it means the containing entity has // been removed. - if (this->joints.HasEntity(joint)) + if (this->joints.HasEntity(jointInfo->joint)) { - const std::size_t jointID = this->joints.IdentityOf(joint); + const std::size_t jointID = this->joints.IdentityOf(jointInfo->joint); return this->GenerateIdentity(jointID, this->joints.at(jointID)); } else @@ -428,6 +431,9 @@ Identity EntityManagementFeatures::GetJoint( // model that has been removed. Right now we are returning an invalid // identity, but that could cause a segfault if the use doesn't check if // returned value before using it. + gzwarn << "No joint with ID [" + << _jointIndex << "] for modelID [" << _modelID.id << "]\n"; + return this->GenerateInvalidId(); } } @@ -436,25 +442,30 @@ Identity EntityManagementFeatures::GetJoint( Identity EntityManagementFeatures::GetJoint( const Identity &_modelID, const std::string &_jointName) const { - DartJoint *const joint = - this->ReferenceInterface(_modelID)->model->getJoint( - _jointName); + const std::string fullJointName = + this->FullyScopedJointName(_modelID, _jointName); + if (fullJointName.empty()) + return this->GenerateInvalidId(); - // If the joint doesn't exist in "joints", it means the containing entity has - // been removed. - if (this->joints.HasEntity(joint)) - { - const std::size_t jointID = this->joints.IdentityOf(joint); - return this->GenerateIdentity(jointID, this->joints.at(jointID)); - } - else + auto it = this->jointsByName.find(fullJointName); + if (it != this->jointsByName.end()) { - // TODO(addisu) It's not clear what to do when `GetJoint` is called on a - // model that has been removed. Right now we are returning an invalid - // identity, but that could cause a segfault if the use doesn't check if - // returned value before using it. - return this->GenerateInvalidId(); + auto joint = it->second; + if (this->joints.HasEntity(joint)) + { + const std::size_t jointID = this->joints.IdentityOf(joint); + return this->GenerateIdentity(jointID, this->joints.at(jointID)); + } } + + // TODO(anyone) It's not clear what to do when `GetJoint` is called on a + // model that has been removed. Right now we are returning an invalid + // identity, but that could cause a segfault if the user doesn't check + // the returned value before using it. + gzwarn << "No joint named [" + << _jointName << "] for modelID [" << _modelID.id << "]\n"; + + return this->GenerateInvalidId(); } ///////////////////////////////////////////////// @@ -475,18 +486,7 @@ std::size_t EntityManagementFeatures::GetLinkIndex( Identity EntityManagementFeatures::GetModelOfLink( const Identity &_linkID) const { - const std::size_t modelID = this->links.idToContainerID.at(_linkID); - - // If the model containing the link doesn't exist in "models", it means this - // link belongs to a removed model. - if (this->models.HasEntity(modelID)) - { - return this->GenerateIdentity(modelID, this->models.at(modelID)); - } - else - { - return this->GenerateInvalidId(); - } + return GetModelOfLinkImpl(_linkID); } ///////////////////////////////////////////////// @@ -558,22 +558,19 @@ const std::string &EntityManagementFeatures::GetJointName( std::size_t EntityManagementFeatures::GetJointIndex( const Identity &_jointID) const { - return this->ReferenceInterface(_jointID) - ->joint->getJointIndexInSkeleton(); + return this->joints.idToIndexInContainer.at(_jointID); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetModelOfJoint( const Identity &_jointID) const { - const DartSkeletonPtr &model = - this->ReferenceInterface(_jointID)->joint->getSkeleton(); + const std::size_t modelID = this->joints.idToContainerID.at(_jointID); // If the model containing the joint doesn't exist in "models", it means this // joint belongs to a removed model. - if (this->models.HasEntity(model)) + if (this->models.HasEntity(modelID)) { - const std::size_t modelID = this->models.IdentityOf(model); return this->GenerateIdentity(modelID, this->models.at(modelID)); } else diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index c44c2ca59..07e056d4f 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -35,23 +35,25 @@ #include "KinematicsFeatures.hh" #include "ShapeFeatures.hh" -struct TestFeatureList : gz::physics::FeatureList< - gz::physics::dartsim::EntityManagementFeatureList, - gz::physics::dartsim::JointFeatureList, - gz::physics::dartsim::KinematicsFeatureList, - gz::physics::dartsim::ShapeFeatureList +using namespace gz; + +struct TestFeatureList : physics::FeatureList< + physics::dartsim::EntityManagementFeatureList, + physics::dartsim::JointFeatureList, + physics::dartsim::KinematicsFeatureList, + physics::dartsim::ShapeFeatureList > { }; TEST(EntityManagement_TEST, ConstructEmptyWorld) { - gz::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); - gz::plugin::PluginPtr dartsim = + plugin::PluginPtr dartsim = loader.Instantiate("gz::physics::dartsim::Plugin"); auto engine = - gz::physics::RequestEngine3d::From(dartsim); + physics::RequestEngine3d::From(dartsim); ASSERT_NE(nullptr, engine); auto world = engine->ConstructEmptyWorld("empty world"); @@ -118,7 +120,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) prismatic->SetVelocity(0, zVel); prismatic->SetAcceleration(0, zAcc); - const gz::physics::FrameData3d childData = + const physics::FrameData3d childData = child->FrameDataRelativeToWorld(); const Eigen::Vector3d childPosition = childData.pose.translation(); @@ -141,7 +143,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) childSpherePose.translate(Eigen::Vector3d(0.0, yPos, 0.0)); auto sphere = child->AttachSphereShape("child sphere", 1.0, childSpherePose); - const gz::physics::FrameData3d sphereData = + const physics::FrameData3d sphereData = sphere->FrameDataRelativeToWorld(); const Eigen::Vector3d spherePosition = sphereData.pose.translation(); @@ -159,7 +161,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) EXPECT_DOUBLE_EQ(0.0, sphereAcceleration.y()); EXPECT_DOUBLE_EQ(zAcc, sphereAcceleration.z()); - const gz::physics::FrameData3d relativeSphereData = + const physics::FrameData3d relativeSphereData = sphere->FrameDataRelativeTo(*child); const Eigen::Vector3d relativeSpherePosition = relativeSphereData.pose.translation(); @@ -170,9 +172,9 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) auto meshLink = model->ConstructEmptyLink("mesh_link"); meshLink->AttachFixedJoint(child, "fixed"); - const std::string meshFilename = gz::common::joinPaths( + const std::string meshFilename = common::joinPaths( GZ_PHYSICS_RESOURCE_DIR, "chassis.dae"); - auto &meshManager = *gz::common::MeshManager::Instance(); + auto &meshManager = *common::MeshManager::Instance(); auto *mesh = meshManager.Load(meshFilename); auto meshShape = meshLink->AttachMeshShape("chassis", *mesh); @@ -189,11 +191,11 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) EXPECT_NEAR(meshShapeSize[1], 0.3831, 1e-4); EXPECT_NEAR(meshShapeSize[2], 0.1956, 1e-4); - const gz::math::Pose3d pose(0, 0, 0.2, 0, 0, 0); - const gz::math::Vector3d scale(0.5, 1.0, 0.25); + const math::Pose3d pose(0, 0, 0.2, 0, 0, 0); + const math::Vector3d scale(0.5, 1.0, 0.25); auto meshShapeScaled = meshLink->AttachMeshShape("small_chassis", *mesh, - gz::math::eigen3::convert(pose), - gz::math::eigen3::convert(scale)); + math::eigen3::convert(pose), + math::eigen3::convert(scale)); const auto meshShapeScaledSize = meshShapeScaled->GetSize(); // Note: dartsim uses assimp for storing mesh data, and assimp by default uses @@ -210,15 +212,15 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) auto heightmapLink = model->ConstructEmptyLink("heightmap_link"); heightmapLink->AttachFixedJoint(child, "heightmap_joint"); - auto heightmapFilename = gz::common::joinPaths( + auto heightmapFilename = common::joinPaths( GZ_PHYSICS_RESOURCE_DIR, "heightmap_bowl.png"); - gz::common::ImageHeightmap data; + common::ImageHeightmap data; EXPECT_EQ(0, data.Load(heightmapFilename)); - const gz::math::Vector3d size({129, 129, 10}); + const math::Vector3d size({129, 129, 10}); auto heightmapShape = heightmapLink->AttachHeightmapShape("heightmap", data, - gz::math::eigen3::convert(pose), - gz::math::eigen3::convert(size)); + math::eigen3::convert(pose), + math::eigen3::convert(size)); EXPECT_NEAR(size.X(), heightmapShape->GetSize()[0], 1e-6); EXPECT_NEAR(size.Y(), heightmapShape->GetSize()[1], 1e-6); @@ -237,19 +239,19 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) auto demLink = model->ConstructEmptyLink("dem_link"); demLink->AttachFixedJoint(child, "dem_joint"); - auto demFilename = gz::common::joinPaths( + auto demFilename = common::joinPaths( GZ_PHYSICS_RESOURCE_DIR, "volcano.tif"); - gz::common::Dem dem; + common::Dem dem; EXPECT_EQ(0, dem.Load(demFilename)); - gz::math::Vector3d sizeDem; + math::Vector3d sizeDem; sizeDem.X(dem.WorldWidth()); sizeDem.Y(dem.WorldHeight()); sizeDem.Z(dem.MaxElevation() - dem.MinElevation()); auto demShape = demLink->AttachHeightmapShape("dem", dem, - gz::math::eigen3::convert(pose), - gz::math::eigen3::convert(sizeDem)); + math::eigen3::convert(pose), + math::eigen3::convert(sizeDem)); // there is a loss in precision with large dems since heightmaps use floats EXPECT_NEAR(sizeDem.X(), demShape->GetSize()[0], 1e-3); @@ -268,14 +270,14 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) TEST(EntityManagement_TEST, RemoveEntities) { - gz::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); - gz::plugin::PluginPtr dartsim = + plugin::PluginPtr dartsim = loader.Instantiate("gz::physics::dartsim::Plugin"); auto engine = - gz::physics::RequestEngine3d::From(dartsim); + physics::RequestEngine3d::From(dartsim); ASSERT_NE(nullptr, engine); auto world = engine->ConstructEmptyWorld("empty world"); @@ -340,14 +342,14 @@ TEST(EntityManagement_TEST, RemoveEntities) TEST(EntityManagement_TEST, ModelByIndexWithNestedModels) { - gz::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); - gz::plugin::PluginPtr dartsim = + plugin::PluginPtr dartsim = loader.Instantiate("gz::physics::dartsim::Plugin"); auto engine = - gz::physics::RequestEngine3d::From(dartsim); + physics::RequestEngine3d::From(dartsim); ASSERT_NE(nullptr, engine); auto world = engine->ConstructEmptyWorld("empty world"); diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index a125f03de..cc14bfe58 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -494,8 +494,17 @@ Identity JointFeatures::AttachFixedJoint( bn->setName(skeleton->getName() + '/' + childLinkName); } } + + // Get the model of child link and fully scoped joint name. + auto modelID = this->GetModelOfLinkImpl(_childID); + const std::string fullJointName = + this->FullyScopedJointName(modelID, _name); + if (fullJointName.empty()) + return this->GenerateInvalidId(); + const std::size_t jointID = this->AddJoint( - bn->moveTo(parentBn, properties)); + bn->moveTo(parentBn, properties), + fullJointName, modelID); if (linkInfo->weldedNodes.size() > 0) { // weld constraint needs to be updated after moving to new skeleton @@ -592,8 +601,17 @@ Identity JointFeatures::AttachRevoluteJoint( bn->setName(skeleton->getName() + '/' + linkInfo->name); } } + + // Get the model of child link and fully scoped joint name. + auto modelID = this->GetModelOfLinkImpl(_childID); + const std::string fullJointName = + this->FullyScopedJointName(modelID, _name); + if (fullJointName.empty()) + return this->GenerateInvalidId(); + const std::size_t jointID = this->AddJoint( - bn->moveTo(parentBn, properties)); + bn->moveTo(parentBn, properties), + fullJointName, modelID); // TODO(addisu) Remove incrementVersion once DART has been updated to // internally increment the BodyNode's version after moveTo. bn->incrementVersion(); @@ -661,8 +679,17 @@ Identity JointFeatures::AttachPrismaticJoint( bn->setName(skeleton->getName() + '/' + linkInfo->name); } } + + // Get the model of child link and fully scoped joint name. + auto modelID = this->GetModelOfLinkImpl(_childID); + const std::string fullJointName = + this->FullyScopedJointName(modelID, _name); + if (fullJointName.empty()) + return this->GenerateInvalidId(); + const std::size_t jointID = this->AddJoint( - bn->moveTo(parentBn, properties)); + bn->moveTo(parentBn, properties), + fullJointName, modelID); // TODO(addisu) Remove incrementVersion once DART has been updated to // internally increment the BodyNode's version after moveTo. bn->incrementVersion(); diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index c418f12e6..97e5a3ea7 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -63,6 +63,7 @@ #include #include +#include "AddedMassFeatures.hh" #include "CustomMeshShape.hh" namespace gz { @@ -164,7 +165,7 @@ static Eigen::Vector3d ConvertJointAxis( // Error while Resolving xyz. Fallback sdformat 1.6 behavior but treat // xyz_expressed_in = "__model__" as the old use_parent_model_frame - const Eigen::Vector3d axis = gz::math::eigen3::convert(_sdfAxis->Xyz()); + const Eigen::Vector3d axis = math::eigen3::convert(_sdfAxis->Xyz()); if (_sdfAxis->XyzExpressedIn().empty()) return axis; @@ -373,6 +374,7 @@ static ShapeAndTransform ConstructGeometry( } // namespace +///////////////////////////////////////////////// dart::dynamics::BodyNode *SDFFeatures::FindBodyNode( const std::string &_worldName, const std::string &_jointModelName, const std::string &_linkRelativeName) @@ -401,7 +403,7 @@ Identity SDFFeatures::ConstructSdfWorld( const dart::simulation::WorldPtr &world = this->worlds.at(worldID); - world->setGravity(gz::math::eigen3::convert(_sdfWorld.Gravity())); + world->setGravity(math::eigen3::convert(_sdfWorld.Gravity())); // TODO(MXG): Add a Physics class to the SDFormat DOM and then parse that // information here. For now, we'll just use dartsim's default physics @@ -609,7 +611,7 @@ Identity SDFFeatures::ConstructSdfLink( dart::dynamics::BodyNode::Properties bodyProperties; bodyProperties.mName = _sdfLink.Name(); - const gz::math::Inertiald &sdfInertia = _sdfLink.Inertial(); + const math::Inertiald &sdfInertia = _sdfLink.Inertial(); bodyProperties.mInertia.setMass(sdfInertia.MassMatrix().Mass()); const Eigen::Matrix3d I_link = math::eigen3::convert(sdfInertia.Moi()); @@ -621,31 +623,6 @@ Identity SDFFeatures::ConstructSdfLink( bodyProperties.mInertia.setLocalCOM(localCom); - // If added mass is provided, add that to DART's computed spatial tensor - // TODO(chapulina) Put in another feature - if (sdfInertia.FluidAddedMass().has_value()) - { - // Note that the ordering of the spatial inertia matrix used in DART is - // different than the one used in Gazebo and SDF. - math::Matrix6d featherstoneMatrix; - featherstoneMatrix.SetSubmatrix(math::Matrix6d::TOP_LEFT, - sdfInertia.FluidAddedMass().value().Submatrix( - math::Matrix6d::BOTTOM_RIGHT)); - featherstoneMatrix.SetSubmatrix(math::Matrix6d::TOP_RIGHT, - sdfInertia.FluidAddedMass().value().Submatrix( - math::Matrix6d::BOTTOM_LEFT)); - featherstoneMatrix.SetSubmatrix(math::Matrix6d::BOTTOM_LEFT, - sdfInertia.FluidAddedMass().value().Submatrix( - math::Matrix6d::TOP_RIGHT)); - featherstoneMatrix.SetSubmatrix(math::Matrix6d::BOTTOM_RIGHT, - sdfInertia.FluidAddedMass().value().Submatrix( - math::Matrix6d::TOP_LEFT)); - - bodyProperties.mInertia.setSpatialTensor( - bodyProperties.mInertia.getSpatialTensor() + - math::eigen3::convert(featherstoneMatrix) - ); - } dart::dynamics::FreeJoint::Properties jointProperties; jointProperties.mName = bodyProperties.mName + "_FreeJoint"; @@ -680,10 +657,26 @@ Identity SDFFeatures::ConstructSdfLink( world->getName(), ::sdf::JoinName(modelInfo.model->getName(), bn->getName())); const std::size_t linkID = this->AddLink(bn, fullName, _modelID, sdfInertia); - this->AddJoint(joint); auto linkIdentity = this->GenerateIdentity(linkID, this->links.at(linkID)); + if (sdfInertia.FluidAddedMass().has_value()) + { + auto* amf = dynamic_cast(this); + + if (nullptr == amf) + { + gzwarn << "Link [" << _sdfLink.Name() << "] in model [" + << modelInfo.model->getName() << + "] has added mass specified in SDF, but AddedMassFeatures" << + "was not available on this engine. Added mass will not be applied.\n"; + } + else + { + amf->SetLinkAddedMass(linkIdentity, sdfInertia.FluidAddedMass().value()); + } + } + if (_sdfLink.Name() == modelInfo.canonicalLinkName || (modelInfo.canonicalLinkName.empty() && modelInfo.model->getNumBodyNodes() == 1)) @@ -988,7 +981,7 @@ Identity SDFFeatures::ConstructSdfVisual( // intended for the physics? if (_visual.Material()) { - const gz::math::Color &color = _visual.Material()->Ambient(); + const math::Color &color = _visual.Material()->Ambient(); node->getVisualAspect()->setColor( Eigen::Vector4d(color.R(), color.G(), color.B(), color.A())); } @@ -1158,14 +1151,30 @@ Identity SDFFeatures::ConstructSdfJoint( joint = _child->moveTo(_parent); } - joint->setName(_sdfJoint.Name()); + const std::string jointName = _sdfJoint.Name(); + joint->setName(jointName); const Eigen::Isometry3d child_T_postjoint = T_child.inverse() * T_joint; const Eigen::Isometry3d parent_T_prejoint_init = T_parent.inverse() * T_joint; joint->setTransformFromParentBodyNode(parent_T_prejoint_init); joint->setTransformFromChildBodyNode(child_T_postjoint); - const std::size_t jointID = this->AddJoint(joint); + auto modelID = this->models.IdentityOf(_modelInfo.model); + auto worldID = this->GetWorldOfModelImpl(modelID); + if (worldID == INVALID_ENTITY_ID) + { + gzerr << "World of model [" << _modelInfo.model->getName() + << "] could not be found when creating joint [" << jointName + << "]\n"; + return this->GenerateInvalidId(); + } + + auto world = this->worlds.at(worldID); + const std::string fullJointName = ::sdf::JoinName( + world->getName(), + ::sdf::JoinName(_modelInfo.model->getName(), jointName)); + + const std::size_t jointID = this->AddJoint(joint, fullJointName, modelID); // Increment BodyNode version since the child could be moved to a new skeleton // when a joint is created. // TODO(azeey) Remove incrementVersion once DART has been updated to diff --git a/dartsim/src/SDFFeatures.hh b/dartsim/src/SDFFeatures.hh index 4dcdfda84..0882c0258 100644 --- a/dartsim/src/SDFFeatures.hh +++ b/dartsim/src/SDFFeatures.hh @@ -18,6 +18,7 @@ #ifndef GZ_PHYSICS_DARTSIM_SRC_SDFFEATURES_HH_ #define GZ_PHYSICS_DARTSIM_SRC_SDFFEATURES_HH_ +#include #include #include diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 1942d2a6d..b1c7218b5 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -54,36 +54,38 @@ #include -struct TestFeatureList : gz::physics::FeatureList< - gz::physics::GetEntities, - gz::physics::GetBasicJointState, - gz::physics::SetBasicJointState, - gz::physics::LinkFrameSemantics, - gz::physics::dartsim::RetrieveWorld, - gz::physics::sdf::ConstructSdfCollision, - gz::physics::sdf::ConstructSdfJoint, - gz::physics::sdf::ConstructSdfLink, - gz::physics::sdf::ConstructSdfModel, - gz::physics::sdf::ConstructSdfNestedModel, - gz::physics::sdf::ConstructSdfWorld +using namespace gz; + +struct TestFeatureList : physics::FeatureList< + physics::GetEntities, + physics::GetBasicJointState, + physics::SetBasicJointState, + physics::LinkFrameSemantics, + physics::dartsim::RetrieveWorld, + physics::sdf::ConstructSdfCollision, + physics::sdf::ConstructSdfJoint, + physics::sdf::ConstructSdfLink, + physics::sdf::ConstructSdfModel, + physics::sdf::ConstructSdfNestedModel, + physics::sdf::ConstructSdfWorld > { }; -using World = gz::physics::World3d; -using WorldPtr = gz::physics::World3dPtr; -using ModelPtr = gz::physics::Model3dPtr; -using LinkPtr = gz::physics::Link3dPtr; +using World = physics::World3d; +using WorldPtr = physics::World3dPtr; +using ModelPtr = physics::Model3dPtr; +using LinkPtr = physics::Link3dPtr; ///////////////////////////////////////////////// auto LoadEngine() { - gz::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); - gz::plugin::PluginPtr dartsim = + plugin::PluginPtr dartsim = loader.Instantiate("gz::physics::dartsim::Plugin"); auto engine = - gz::physics::RequestEngine3d::From(dartsim); + physics::RequestEngine3d::From(dartsim); return engine; } @@ -537,6 +539,45 @@ TEST_P(SDFFeatures_TEST, WorldIsParentOrChild) } ///////////////////////////////////////////////// +// The model, link and joint structure in the +// test world `world_with_nested_model.sdf`: +// +// model and link tree: +// parent_model +// nested_model +// nested_link1 +// nested_link2 +// link1 +// nested_model2 +// nested_link1 +// nested_model3 +// link1 +// parent_model2 +// child_model +// grand_child_model +// link1 +// +// models: +// parent_model +// parent_model::nested_model +// parent_model::nested_model2 +// parent_model::nested_model3 +// parent_model2 +// parent_model2::child_model +// parent_model2::child_model::grand_child_model +// +// links: +// parent_model::nested_link1::nested_link1 +// parent_model::nested_link1::nested_link2 +// parent_model::link1 +// parent_model::nested_model2::nested_link1 +// parent_model::nested_model3::link1 +// parent_model2::child_model::grand_child_model::link1 +// +// joints: +// parent_model::nested_model::nested_joint +// parent_model::joint1 +// TEST_P(SDFFeatures_TEST, WorldWithNestedModel) { WorldPtr world = @@ -559,8 +600,14 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModel) auto nestedModel = parentModel->GetNestedModel("nested_model"); ASSERT_NE(nullptr, nestedModel); + // DART associates the nested joint with the skeleton of the top level + // model when the nested model is joined to the parent model, but Gazebo + // should not find grandchild joints when querying a parent model. auto nestedJoint = parentModel->GetJoint("nested_joint"); - EXPECT_NE(nullptr, nestedJoint); + EXPECT_EQ(nullptr, nestedJoint); + + // The nested_joint should be found when querying the nested model. + EXPECT_NE(nullptr, nestedModel->GetJoint("nested_joint")); EXPECT_EQ(1u, parentModel->GetLinkCount()); EXPECT_NE(nullptr, parentModel->GetLink("link1")); @@ -753,7 +800,7 @@ TEST_P(SDFFeatures_FrameSemantics, LinkRelativeTo) dartWorld->step(); // Step once and check - EXPECT_TRUE(gz::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( expWorldPose, link2->getWorldTransform(), 1e-3)); } @@ -784,13 +831,13 @@ TEST_P(SDFFeatures_FrameSemantics, CollisionRelativeTo) Eigen::Isometry3d expPose; expPose = Eigen::Translation3d(0, 0, -1); - EXPECT_TRUE(gz::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( expPose, collision->getRelativeTransform(), 1e-5)); // Step once and check, the relative pose should still be the same dartWorld->step(); - EXPECT_TRUE(gz::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( expPose, collision->getRelativeTransform(), 1e-5)); } @@ -821,7 +868,7 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitFramesWithLinks) Eigen::Isometry3d link1ExpPose; link1ExpPose = Eigen::Translation3d(1, 0, 1); - EXPECT_TRUE(gz::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( link1ExpPose, link1->getWorldTransform(), 1e-5)); // Expect the world pose of L2 to be the same as the world pose of F2, which @@ -829,15 +876,15 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitFramesWithLinks) Eigen::Isometry3d link2ExpPose; link2ExpPose = Eigen::Translation3d(1, 0, 0); - EXPECT_TRUE(gz::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( link2ExpPose, link2->getWorldTransform(), 1e-5)); // Step once and check dartWorld->step(); - EXPECT_TRUE(gz::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( link1ExpPose, link1->getWorldTransform(), 1e-5)); - EXPECT_TRUE(gz::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( link2ExpPose, link2->getWorldTransform(), 1e-5)); } @@ -868,13 +915,13 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitFramesWithCollision) Eigen::Isometry3d expPose; expPose = Eigen::Translation3d(0, 0, 1); - EXPECT_TRUE(gz::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( expPose, collision->getRelativeTransform(), 1e-5)); // Step once and check dartWorld->step(); - EXPECT_TRUE(gz::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( expPose, collision->getRelativeTransform(), 1e-5)); } @@ -905,13 +952,13 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitWorldFrames) // Since we can't get the skeleton's world transform, we use the world // transform of L1 which is at the origin of the model frame. - EXPECT_TRUE(gz::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( expPose, link1->getWorldTransform(), 1e-5)); // Step once and check dartWorld->step(); - EXPECT_TRUE(gz::physics::test::Equal( + EXPECT_TRUE(physics::test::Equal( expPose, link1->getWorldTransform(), 1e-5)); } @@ -935,36 +982,3 @@ TEST_P(SDFFeatures_TEST, Shapes) ASSERT_EQ(1u, skeleton->getNumBodyNodes()); } } - -///////////////////////////////////////////////// -TEST_P(SDFFeatures_TEST, AddedMass) -{ - // Expected spatial inertia matrix. This includes inertia due to the body's - // mass and added mass. Note that the ordering of the matrix is different - // than the one used in SDF. - Eigen::Matrix6d expectedSpatialInertia; - expectedSpatialInertia << - 17, 17, 18, 4, 9, 13, - 17, 20, 20, 5, 10, 14, - 18, 20, 22, 6, 11, 15, - 4, 5, 6, 2, 2, 3, - 9, 10, 11, 2, 8, 8, - 13, 14, 15, 3, 8, 13; - - auto world = this->LoadWorld(TEST_WORLD_DIR"/added_mass.sdf"); - ASSERT_NE(nullptr, world); - - auto dartWorld = world->GetDartsimWorld(); - ASSERT_NE(nullptr, dartWorld); - - ASSERT_EQ(1u, dartWorld->getNumSkeletons()); - const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton("body"); - ASSERT_NE(skeleton, nullptr); - - ASSERT_EQ(1u, skeleton->getNumBodyNodes()); - const dart::dynamics::BodyNode *link = skeleton->getBodyNode("link"); - ASSERT_NE(link, nullptr); - - const Eigen::Matrix6d spatialInertia = link->getSpatialInertia(); - ASSERT_TRUE(expectedSpatialInertia.isApprox(spatialInertia)); -} diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index c5fbfaa63..dfbf01277 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -422,7 +422,7 @@ LinearVector3d ShapeFeatures::GetMeshShapeScale( Identity ShapeFeatures::AttachMeshShape( const Identity &_linkID, const std::string &_name, - const gz::common::Mesh &_mesh, + const common::Mesh &_mesh, const Pose3d &_pose, const LinearVector3d &_scale) { diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index 204bfc097..0c4d5370e 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -193,7 +193,7 @@ class ShapeFeatures : public: Identity AttachMeshShape( const Identity &_linkID, const std::string &_name, - const gz::common::Mesh &_mesh, + const common::Mesh &_mesh, const Pose3d &_pose, const LinearVector3d &_scale) override; diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 2e8faa626..a622c8f7c 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -38,6 +38,15 @@ #include "SimulationFeatures.hh" +#if DART_VERSION_AT_LEAST(6, 13, 0) +// The ContactSurfaceParams class was first added to version 6.10 of our fork +// of dart, and then merged upstream and released in version 6.13.0 with +// different public member variable names. +// See https://github.com/dartsim/dart/pull/1626 and +// https://github.com/gazebo-forks/dart/pull/22 for more info. +#define DART_HAS_UPSTREAM_FRICTION_VARIABLE_NAMES +#endif + namespace gz { namespace physics { namespace dartsim { @@ -65,6 +74,21 @@ void SimulationFeatures::WorldForwardStep( } } + for (const auto &[id, info] : this->links.idToObject) + { + if (info && info->inertial->FluidAddedMass().has_value()) + { + auto com = Eigen::Vector3d(info->inertial->Pose().Pos().X(), + info->inertial->Pose().Pos().Y(), + info->inertial->Pose().Pos().Z()); + + auto mass = info->inertial->MassMatrix().Mass(); + auto g = world->getGravity(); + + info->link->addExtForce(mass * g, com, false, true); + } + } + // TODO(MXG): Parse input world->step(); this->WriteRequiredData(_h); @@ -233,9 +257,17 @@ dart::constraint::ContactSurfaceParams GzContactSurfaceHandler::createParams( typedef FeaturePolicy3d P; typename F::ContactSurfaceParams

pGz; +#ifdef DART_HAS_UPSTREAM_FRICTION_VARIABLE_NAMES + pGz.frictionCoeff = pDart.mPrimaryFrictionCoeff; +#else pGz.frictionCoeff = pDart.mFrictionCoeff; +#endif pGz.secondaryFrictionCoeff = pDart.mSecondaryFrictionCoeff; +#ifdef DART_HAS_UPSTREAM_FRICTION_VARIABLE_NAMES + pGz.slipCompliance = pDart.mPrimarySlipCompliance; +#else pGz.slipCompliance = pDart.mSlipCompliance; +#endif pGz.secondarySlipCompliance = pDart.mSecondarySlipCompliance; pGz.restitutionCoeff = pDart.mRestitutionCoeff; pGz.firstFrictionalDirection = pDart.mFirstFrictionalDirection; @@ -248,11 +280,23 @@ dart::constraint::ContactSurfaceParams GzContactSurfaceHandler::createParams( _numContactsOnCollisionObject, pGz); if (pGz.frictionCoeff) + { +#ifdef DART_HAS_UPSTREAM_FRICTION_VARIABLE_NAMES + pDart.mPrimaryFrictionCoeff = pGz.frictionCoeff.value(); +#else pDart.mFrictionCoeff = pGz.frictionCoeff.value(); +#endif + } if (pGz.secondaryFrictionCoeff) pDart.mSecondaryFrictionCoeff = pGz.secondaryFrictionCoeff.value(); if (pGz.slipCompliance) + { +#ifdef DART_HAS_UPSTREAM_FRICTION_VARIABLE_NAMES + pDart.mPrimarySlipCompliance = pGz.slipCompliance.value(); +#else pDart.mSlipCompliance = pGz.slipCompliance.value(); +#endif + } if (pGz.secondarySlipCompliance) pDart.mSecondarySlipCompliance = pGz.secondarySlipCompliance.value(); if (pGz.restitutionCoeff) diff --git a/dartsim/src/WorldFeatures_TEST.cc b/dartsim/src/WorldFeatures_TEST.cc index c802dfbda..459460ba2 100644 --- a/dartsim/src/WorldFeatures_TEST.cc +++ b/dartsim/src/WorldFeatures_TEST.cc @@ -47,29 +47,7 @@ using namespace gz; using TestEnginePtr = physics::Engine3dPtr; using TestWorldPtr = physics::World3dPtr; - -// A predicate-formatter for asserting that two vectors are approximately equal. -class AssertVectorApprox -{ - public: explicit AssertVectorApprox(double _tol = 1e-6) : tol(_tol) - { - } - - public: ::testing::AssertionResult operator()( - const char *_mExpr, const char *_nExpr, Eigen::Vector3d _m, - Eigen::Vector3d _n) - { - if (gz::physics::test::Equal(_m, _n, this->tol)) - return ::testing::AssertionSuccess(); - - return ::testing::AssertionFailure() - << _mExpr << " and " << _nExpr << " ([" << _m.transpose() - << "] and [" << _n.transpose() << "]" - << ") are not equal"; - } - - private: double tol; -}; +using AssertVectorApprox = physics::test::AssertVectorApprox; ////////////////////////////////////////////////// class WorldFeaturesFixture : public ::testing::Test diff --git a/dartsim/src/plugin.cc b/dartsim/src/plugin.cc index 284b99de2..8aa14b0f9 100644 --- a/dartsim/src/plugin.cc +++ b/dartsim/src/plugin.cc @@ -18,6 +18,7 @@ #include #include "Base.hh" +#include "AddedMassFeatures.hh" #include "CustomFeatures.hh" #include "JointFeatures.hh" #include "KinematicsFeatures.hh" @@ -34,6 +35,7 @@ namespace physics { namespace dartsim { struct DartsimFeatures : FeatureList< + AddedMassFeatureList, CustomFeatureList, EntityManagementFeatureList, FreeGroupFeatureList, @@ -48,6 +50,7 @@ struct DartsimFeatures : FeatureList< class Plugin : public virtual Base, + public virtual AddedMassFeatures, public virtual CustomFeatures, public virtual EntityManagementFeatures, public virtual FreeGroupFeatures, diff --git a/dartsim/worlds/added_mass.sdf b/dartsim/worlds/added_mass.sdf index 199e9a069..1ae18cf78 100644 --- a/dartsim/worlds/added_mass.sdf +++ b/dartsim/worlds/added_mass.sdf @@ -1,7 +1,62 @@ - + + + + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + + + + + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + 1 diff --git a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh index 2ef199d38..3e17702f4 100644 --- a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2023 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. diff --git a/include/gz/physics/AddedMass.hh b/include/gz/physics/AddedMass.hh new file mode 100644 index 000000000..bfe279585 --- /dev/null +++ b/include/gz/physics/AddedMass.hh @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef GZ_PHYSICS_ADDED_MASS_HH_ +#define GZ_PHYSICS_ADDED_MASS_HH_ + +#include + +#include + + +namespace gz::physics +{ + +class AddedMass: public virtual Feature +{ + public: template + class Link : public virtual Feature::Link + { + public: void SetAddedMass(const gz::math::Matrix6d &_addedMass); + public: gz::math::Matrix6d GetAddedMass() const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: virtual void SetLinkAddedMass(const Identity &_link, + const gz::math::Matrix6d &_addedMass) = 0; + public: virtual gz::math::Matrix6d GetLinkAddedMass( + const Identity &_link) const = 0; + }; +}; + +template +auto AddedMass::Link::SetAddedMass( + const gz::math::Matrix6d &_addedMass) -> void +{ + this->template Interface() + ->SetLinkAddedMass(this->identity, _addedMass); +} +template + +auto AddedMass::Link::GetAddedMass( +) const -> gz::math::Matrix6d +{ + return this->template Interface() + ->GetLinkAddedMass(this->identity); +} + +} // namespace gz::physics + +#endif // GZ_PHYSICS_ADDED_MASS_HH_) diff --git a/include/ignition/physics/BoxShape.hh b/include/ignition/physics/BoxShape.hh index 5fe8c193b..af204b26f 100644 --- a/include/ignition/physics/BoxShape.hh +++ b/include/ignition/physics/BoxShape.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/CanReadData.hh b/include/ignition/physics/CanReadData.hh index 8b9aefd77..90f8c743f 100644 --- a/include/ignition/physics/CanReadData.hh +++ b/include/ignition/physics/CanReadData.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/CanWriteData.hh b/include/ignition/physics/CanWriteData.hh index 9b979a037..bdbfd3074 100644 --- a/include/ignition/physics/CanWriteData.hh +++ b/include/ignition/physics/CanWriteData.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/CapsuleShape.hh b/include/ignition/physics/CapsuleShape.hh index b426bd50f..830a8f422 100644 --- a/include/ignition/physics/CapsuleShape.hh +++ b/include/ignition/physics/CapsuleShape.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/Cloneable.hh b/include/ignition/physics/Cloneable.hh index 22a5541f0..6c4491951 100644 --- a/include/ignition/physics/Cloneable.hh +++ b/include/ignition/physics/Cloneable.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/CompositeData.hh b/include/ignition/physics/CompositeData.hh index 1798c1860..be8bf1809 100644 --- a/include/ignition/physics/CompositeData.hh +++ b/include/ignition/physics/CompositeData.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/ConstructEmpty.hh b/include/ignition/physics/ConstructEmpty.hh index 25d26f378..768bcaa1a 100644 --- a/include/ignition/physics/ConstructEmpty.hh +++ b/include/ignition/physics/ConstructEmpty.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/ContactProperties.hh b/include/ignition/physics/ContactProperties.hh index de17a1be3..7d4a6c61f 100644 --- a/include/ignition/physics/ContactProperties.hh +++ b/include/ignition/physics/ContactProperties.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/CylinderShape.hh b/include/ignition/physics/CylinderShape.hh index 651c733ca..7a6b2e4c3 100644 --- a/include/ignition/physics/CylinderShape.hh +++ b/include/ignition/physics/CylinderShape.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/DataStatusMask.hh b/include/ignition/physics/DataStatusMask.hh index 21f6d0e24..3171d2d7a 100644 --- a/include/ignition/physics/DataStatusMask.hh +++ b/include/ignition/physics/DataStatusMask.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/EllipsoidShape.hh b/include/ignition/physics/EllipsoidShape.hh index 07c5290e7..261c28be8 100644 --- a/include/ignition/physics/EllipsoidShape.hh +++ b/include/ignition/physics/EllipsoidShape.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/Entity.hh b/include/ignition/physics/Entity.hh index 5e24bde01..bd61f1964 100644 --- a/include/ignition/physics/Entity.hh +++ b/include/ignition/physics/Entity.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/Feature.hh b/include/ignition/physics/Feature.hh index f2477f298..bf4138666 100644 --- a/include/ignition/physics/Feature.hh +++ b/include/ignition/physics/Feature.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/FeatureList.hh b/include/ignition/physics/FeatureList.hh index 5a8c468a7..2174ebac9 100644 --- a/include/ignition/physics/FeatureList.hh +++ b/include/ignition/physics/FeatureList.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/FeaturePolicy.hh b/include/ignition/physics/FeaturePolicy.hh index 4267c772c..48b630adc 100644 --- a/include/ignition/physics/FeaturePolicy.hh +++ b/include/ignition/physics/FeaturePolicy.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/FindFeatures.hh b/include/ignition/physics/FindFeatures.hh index e0fbb1ced..514e816f1 100644 --- a/include/ignition/physics/FindFeatures.hh +++ b/include/ignition/physics/FindFeatures.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/FixedJoint.hh b/include/ignition/physics/FixedJoint.hh index cc8eae27d..e48b1d242 100644 --- a/include/ignition/physics/FixedJoint.hh +++ b/include/ignition/physics/FixedJoint.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/ForwardStep.hh b/include/ignition/physics/ForwardStep.hh index 0f88a0ffc..072bab4d8 100644 --- a/include/ignition/physics/ForwardStep.hh +++ b/include/ignition/physics/ForwardStep.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/FrameData.hh b/include/ignition/physics/FrameData.hh index f0abf26a3..a5174bccc 100644 --- a/include/ignition/physics/FrameData.hh +++ b/include/ignition/physics/FrameData.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/FrameID.hh b/include/ignition/physics/FrameID.hh index 4cc591440..58aaf8075 100644 --- a/include/ignition/physics/FrameID.hh +++ b/include/ignition/physics/FrameID.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/FrameSemantics.hh b/include/ignition/physics/FrameSemantics.hh index 2450b28b9..ea8f02de4 100644 --- a/include/ignition/physics/FrameSemantics.hh +++ b/include/ignition/physics/FrameSemantics.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/FreeGroup.hh b/include/ignition/physics/FreeGroup.hh index 2c7c127d3..b7308bea0 100644 --- a/include/ignition/physics/FreeGroup.hh +++ b/include/ignition/physics/FreeGroup.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/FreeJoint.hh b/include/ignition/physics/FreeJoint.hh index 70c109e82..7fc7e3ea0 100644 --- a/include/ignition/physics/FreeJoint.hh +++ b/include/ignition/physics/FreeJoint.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/GetBoundingBox.hh b/include/ignition/physics/GetBoundingBox.hh index a73bd2647..be763687f 100644 --- a/include/ignition/physics/GetBoundingBox.hh +++ b/include/ignition/physics/GetBoundingBox.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/GetContacts.hh b/include/ignition/physics/GetContacts.hh index 0ca5c9f26..6ad36e460 100644 --- a/include/ignition/physics/GetContacts.hh +++ b/include/ignition/physics/GetContacts.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/GetEntities.hh b/include/ignition/physics/GetEntities.hh index 622bd560e..bbeb2efa3 100644 --- a/include/ignition/physics/GetEntities.hh +++ b/include/ignition/physics/GetEntities.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/Implements.hh b/include/ignition/physics/Implements.hh index 4d4e269a4..77f5b83b8 100644 --- a/include/ignition/physics/Implements.hh +++ b/include/ignition/physics/Implements.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/Joint.hh b/include/ignition/physics/Joint.hh index 9d535ede2..a7f638638 100644 --- a/include/ignition/physics/Joint.hh +++ b/include/ignition/physics/Joint.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/Link.hh b/include/ignition/physics/Link.hh index cd676b071..7bfd12cc1 100644 --- a/include/ignition/physics/Link.hh +++ b/include/ignition/physics/Link.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/OperateOnSpecifiedData.hh b/include/ignition/physics/OperateOnSpecifiedData.hh index 29fa5fbed..7a7da3838 100644 --- a/include/ignition/physics/OperateOnSpecifiedData.hh +++ b/include/ignition/physics/OperateOnSpecifiedData.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/PlaneShape.hh b/include/ignition/physics/PlaneShape.hh index 790b22499..cc3c1b9b2 100644 --- a/include/ignition/physics/PlaneShape.hh +++ b/include/ignition/physics/PlaneShape.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/PrismaticJoint.hh b/include/ignition/physics/PrismaticJoint.hh index 9d39acf09..7b0c89462 100644 --- a/include/ignition/physics/PrismaticJoint.hh +++ b/include/ignition/physics/PrismaticJoint.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/RelativeQuantity.hh b/include/ignition/physics/RelativeQuantity.hh index fe8c94933..edc82a026 100644 --- a/include/ignition/physics/RelativeQuantity.hh +++ b/include/ignition/physics/RelativeQuantity.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/RemoveEntities.hh b/include/ignition/physics/RemoveEntities.hh index 187646eec..31764705b 100644 --- a/include/ignition/physics/RemoveEntities.hh +++ b/include/ignition/physics/RemoveEntities.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/RequestFeatures.hh b/include/ignition/physics/RequestFeatures.hh index bde1ccbdd..92a9ef8a9 100644 --- a/include/ignition/physics/RequestFeatures.hh +++ b/include/ignition/physics/RequestFeatures.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/RevoluteJoint.hh b/include/ignition/physics/RevoluteJoint.hh index 059c0e53d..bf963c55e 100644 --- a/include/ignition/physics/RevoluteJoint.hh +++ b/include/ignition/physics/RevoluteJoint.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/Shape.hh b/include/ignition/physics/Shape.hh index 657046820..8e371c7c3 100644 --- a/include/ignition/physics/Shape.hh +++ b/include/ignition/physics/Shape.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/SpecifyData.hh b/include/ignition/physics/SpecifyData.hh index d3c50f60e..c3b127507 100644 --- a/include/ignition/physics/SpecifyData.hh +++ b/include/ignition/physics/SpecifyData.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/SphereShape.hh b/include/ignition/physics/SphereShape.hh index e9ea56955..23f317478 100644 --- a/include/ignition/physics/SphereShape.hh +++ b/include/ignition/physics/SphereShape.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/include/ignition/physics/World.hh b/include/ignition/physics/World.hh index 7b3691d19..297bb860a 100644 --- a/include/ignition/physics/World.hh +++ b/include/ignition/physics/World.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/mesh/include/ignition/physics/mesh/MeshShape.hh b/mesh/include/ignition/physics/mesh/MeshShape.hh index a3f7bd4c3..0fe8ee2c2 100644 --- a/mesh/include/ignition/physics/mesh/MeshShape.hh +++ b/mesh/include/ignition/physics/mesh/MeshShape.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/sdf/include/ignition/physics/sdf/ConstructCollision.hh b/sdf/include/ignition/physics/sdf/ConstructCollision.hh index 1dd8b784b..4ffe1df05 100644 --- a/sdf/include/ignition/physics/sdf/ConstructCollision.hh +++ b/sdf/include/ignition/physics/sdf/ConstructCollision.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/sdf/include/ignition/physics/sdf/ConstructJoint.hh b/sdf/include/ignition/physics/sdf/ConstructJoint.hh index c5f49f05f..4466e303c 100644 --- a/sdf/include/ignition/physics/sdf/ConstructJoint.hh +++ b/sdf/include/ignition/physics/sdf/ConstructJoint.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/sdf/include/ignition/physics/sdf/ConstructLink.hh b/sdf/include/ignition/physics/sdf/ConstructLink.hh index 8ae7bd1ef..eff229df7 100644 --- a/sdf/include/ignition/physics/sdf/ConstructLink.hh +++ b/sdf/include/ignition/physics/sdf/ConstructLink.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/sdf/include/ignition/physics/sdf/ConstructModel.hh b/sdf/include/ignition/physics/sdf/ConstructModel.hh index 1b5196371..001180922 100644 --- a/sdf/include/ignition/physics/sdf/ConstructModel.hh +++ b/sdf/include/ignition/physics/sdf/ConstructModel.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/sdf/include/ignition/physics/sdf/ConstructNestedModel.hh b/sdf/include/ignition/physics/sdf/ConstructNestedModel.hh index bb290b6ff..fdb934599 100644 --- a/sdf/include/ignition/physics/sdf/ConstructNestedModel.hh +++ b/sdf/include/ignition/physics/sdf/ConstructNestedModel.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/sdf/include/ignition/physics/sdf/ConstructVisual.hh b/sdf/include/ignition/physics/sdf/ConstructVisual.hh index 6229f4167..97a2c3957 100644 --- a/sdf/include/ignition/physics/sdf/ConstructVisual.hh +++ b/sdf/include/ignition/physics/sdf/ConstructVisual.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/sdf/include/ignition/physics/sdf/ConstructWorld.hh b/sdf/include/ignition/physics/sdf/ConstructWorld.hh index 26d67ea75..a1444adc6 100644 --- a/sdf/include/ignition/physics/sdf/ConstructWorld.hh +++ b/sdf/include/ignition/physics/sdf/ConstructWorld.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/src/Cloneable_TEST.cc b/src/Cloneable_TEST.cc index ab5e6f630..08f8bf7c5 100644 --- a/src/Cloneable_TEST.cc +++ b/src/Cloneable_TEST.cc @@ -20,8 +20,9 @@ #include "gz/physics/Cloneable.hh" #include "utils/TestDataTypes.hh" -using gz::physics::Cloneable; -using gz::physics::MakeCloneable; +using namespace gz; +using physics::Cloneable; +using physics::MakeCloneable; ///////////////////////////////////////////////// TEST(Cloneable_TEST, Construct) diff --git a/src/CompositeData_TEST.cc b/src/CompositeData_TEST.cc index 4c0ece9ec..4e76776d0 100644 --- a/src/CompositeData_TEST.cc +++ b/src/CompositeData_TEST.cc @@ -20,7 +20,9 @@ #include "gz/physics/CompositeData.hh" #include "utils/TestDataTypes.hh" -using gz::physics::CompositeData; +using namespace gz; + +using physics::CompositeData; ///////////////////////////////////////////////// TEST(CompositeData_TEST, DestructorCoverage) @@ -136,7 +138,7 @@ TEST(CompositeData_TEST, InsertOrAssign) ///////////////////////////////////////////////// TEST(CompositeData_TEST, CopyMoveOperators) { - gz::physics::CompositeData data = + physics::CompositeData data = CreateSomeData(); EXPECT_EQ(3u, data.EntryCount()); @@ -144,7 +146,7 @@ TEST(CompositeData_TEST, CopyMoveOperators) EXPECT_TRUE(data.Has()); EXPECT_TRUE(data.Has()); - gz::physics::CompositeData emptyData; + physics::CompositeData emptyData; data = emptyData; EXPECT_EQ(0u, data.EntryCount()); @@ -167,10 +169,10 @@ struct zzzzzzzzz ///////////////////////////////////////////////// TEST(CompositeData_TEST, CopyFunction) { - gz::physics::CompositeData data = + physics::CompositeData data = CreateSomeData(); - gz::physics::CompositeData otherData = + physics::CompositeData otherData = CreateSomeData(); EXPECT_TRUE(data.Has()); @@ -206,7 +208,7 @@ TEST(CompositeData_TEST, CopyFunction) // The next section is used for implementation line coverage to ensure that // we can correctly insert data entries in the center of the data map. - gz::physics::CompositeData zzzData = CreateSomeData(); + physics::CompositeData zzzData = CreateSomeData(); EXPECT_TRUE(zzzData.Has()); zzzData.Copy(otherData); @@ -218,7 +220,7 @@ TEST(CompositeData_TEST, CopyFunction) ///////////////////////////////////////////////// TEST(CompositeData_TEST, CopyFunctionWithRequirements) { - gz::physics::CompositeData data = + physics::CompositeData data = CreateSomeData(); EXPECT_FALSE(data.Requires()); @@ -233,7 +235,7 @@ TEST(CompositeData_TEST, CopyFunctionWithRequirements) EXPECT_FALSE(data.Requires()); - gz::physics::CompositeData otherData = + physics::CompositeData otherData = CreateSomeData(); otherData.MakeRequired(); @@ -294,10 +296,10 @@ TEST(CompositeData_TEST, CopyFunctionWithRequirements) ///////////////////////////////////////////////// TEST(CompositeData_TEST, MergeFunction) { - gz::physics::CompositeData data = + physics::CompositeData data = CreateSomeData(); - gz::physics::CompositeData otherData = + physics::CompositeData otherData = CreateSomeData(); EXPECT_TRUE(data.Has()); @@ -318,7 +320,7 @@ TEST(CompositeData_TEST, MergeFunction) ///////////////////////////////////////////////// TEST(CompositeData_TEST, MergeFunctionWithRequirements) { - gz::physics::CompositeData data = + physics::CompositeData data = CreateSomeData(); EXPECT_FALSE(data.Requires()); @@ -333,7 +335,7 @@ TEST(CompositeData_TEST, MergeFunctionWithRequirements) EXPECT_FALSE(data.Requires()); - gz::physics::CompositeData otherData = + physics::CompositeData otherData = CreateSomeData(); otherData.MakeRequired(); @@ -374,7 +376,7 @@ TEST(CompositeData_TEST, MergeFunctionWithRequirements) ///////////////////////////////////////////////// TEST(CompositeData_TEST, Remove) { - gz::physics::CompositeData data; + physics::CompositeData data; // try to remove data from an empty container // it should return true because the container does not have it now @@ -409,7 +411,7 @@ TEST(CompositeData_TEST, Remove) ///////////////////////////////////////////////// TEST(CompositeData_TEST, Requirements) { - gz::physics::CompositeData requiredData; + physics::CompositeData requiredData; // If StringData was not already created, we should create a new one when it // gets marked as required, using the arguments passed in by MarkRequired @@ -438,7 +440,7 @@ TEST(CompositeData_TEST, Requirements) // When we copy from a blank object, we should retain the required data and // lose everything else. - requiredData = gz::physics::CompositeData(); + requiredData = physics::CompositeData(); EXPECT_TRUE(requiredData.Has()); EXPECT_TRUE(requiredData.Has()); EXPECT_FALSE(requiredData.Has()); @@ -449,7 +451,7 @@ TEST(CompositeData_TEST, Queries) { // test queries on empty container { - gz::physics::CompositeData data; + physics::CompositeData data; EXPECT_EQ(nullptr, data.Query()); EXPECT_EQ(nullptr, data.Query()); EXPECT_EQ(nullptr, data.Query()); @@ -460,7 +462,7 @@ TEST(CompositeData_TEST, Queries) // copy/move operators and copy/move constructors, perhaps using return value // optimization. That gives us the wrong query behavior. If we instead did // - // gz::physics::CompositeData data; + // physics::CompositeData data; // data = CreateSomeData(); // // we would get the correct query behavior. I feel like this is a bug in the @@ -471,7 +473,7 @@ TEST(CompositeData_TEST, Queries) // considered good practice to call ResetQueries() before returning a // CompositeData from a function. - gz::physics::CompositeData data = + physics::CompositeData data = CreateSomeData(true); std::set unqueried, all; @@ -562,7 +564,7 @@ TEST(CompositeData_TEST, Queries) // Make sure that the const-qualified version of query also works - EXPECT_NE(nullptr, static_cast( + EXPECT_NE(nullptr, static_cast( data).Query()); EXPECT_EQ(1u, data.UnqueriedEntryCount()); EXPECT_EQ(4u, data.EntryCount()); diff --git a/src/FeatureList_TEST.cc b/src/FeatureList_TEST.cc index d187eccf9..bef23ef34 100644 --- a/src/FeatureList_TEST.cc +++ b/src/FeatureList_TEST.cc @@ -218,7 +218,7 @@ TEST(FeatureList_TEST, ConflictsAndRequirements) // This is a regression test. We need to make sure that this FeatureList // compiles. - gz::physics::FeatureList(); + FeatureList(); } TEST(FeatureList_TEST, Hierarchy) diff --git a/src/Feature_TEST.cc b/src/Feature_TEST.cc index 4aee812c2..d918f9d1b 100644 --- a/src/Feature_TEST.cc +++ b/src/Feature_TEST.cc @@ -20,7 +20,8 @@ #include #include -using namespace gz::physics; +using namespace gz; +using namespace physics; ///////////////////////////////////////////////// class EngineMockFeature : public virtual Feature @@ -110,7 +111,7 @@ TEST(Feature_TEST, SimpleMock) // a total of 3 feature names. std::set missing = RequestEngine3d::MissingFeatureNames( - gz::plugin::PluginPtr()); + plugin::PluginPtr()); EXPECT_EQ(3u, missing.size()); } diff --git a/src/FilterTuple_TEST.cc b/src/FilterTuple_TEST.cc index 5f287e47a..2748356ee 100644 --- a/src/FilterTuple_TEST.cc +++ b/src/FilterTuple_TEST.cc @@ -19,19 +19,20 @@ #include -class ClassA : public gz::physics::Feature { }; -class ClassB : public gz::physics::Feature { }; -class ClassC : public gz::physics::Feature { }; +using namespace gz; +class ClassA : public physics::Feature { }; +class ClassB : public physics::Feature { }; +class ClassC : public physics::Feature { }; class ClassD : public ClassB { }; using Filter = std::tuple; using Input = std::tuple; -using Result = gz::physics::detail::SubtractTuple +using Result = physics::detail::SubtractTuple ::From::type; using UnfilteredResult = - gz::physics::detail::SubtractTuple>::From::type; + physics::detail::SubtractTuple>::From::type; TEST(FilterTuple_TEST, FilterTupleResult) { @@ -46,19 +47,19 @@ TEST(FilterTuple_TEST, FilterTupleResult) using SingleCombineLists = - gz::physics::detail::CombineListsImpl< + physics::detail::CombineListsImpl< std::tuple<>, ClassA>::type; using SingleCombineListsInitial = - gz::physics::detail::CombineListsImpl< + physics::detail::CombineListsImpl< std::tuple<>, ClassA>::InitialResult; using SingleCombineListsPartial = - gz::physics::detail::CombineListsImpl< + physics::detail::CombineListsImpl< std::tuple<>, ClassA>::PartialResult; using SingleCombineListsChildFilter = - gz::physics::detail::CombineListsImpl< + physics::detail::CombineListsImpl< std::tuple<>, ClassA>::ChildFilter; TEST(FilterTuple_TEST, CombineListsResult) diff --git a/src/FindFeatures_TEST.cc b/src/FindFeatures_TEST.cc index 158236b6c..165e3f42f 100644 --- a/src/FindFeatures_TEST.cc +++ b/src/FindFeatures_TEST.cc @@ -26,6 +26,8 @@ #include "TestUtilities.hh" +using namespace gz; + TEST(FindFeatures_TEST, ForwardStep) { // List of plugin names that are known to provide this test feature. @@ -36,14 +38,14 @@ TEST(FindFeatures_TEST, ForwardStep) }; using TestFeatures = - gz::physics::FeatureList; + physics::FeatureList; - gz::plugin::Loader loader; + plugin::Loader loader; PrimeTheLoader(loader); const std::set allPlugins = loader.AllPlugins(); const std::set foundPlugins = - gz::physics::FindFeatures3d::From(loader); + physics::FindFeatures3d::From(loader); for (const std::string &acceptable : knownAcceptablePlugins) { @@ -57,13 +59,13 @@ TEST(FindFeatures_TEST, ForwardStep) TEST(FindFeatures_TEST, Unimplemented) { using TestFeatures = - gz::physics::FeatureList; + physics::FeatureList; - gz::plugin::Loader loader; + plugin::Loader loader; PrimeTheLoader(loader); const std::set foundPlugins = - gz::physics::FindFeatures3d::From(loader); + physics::FindFeatures3d::From(loader); // No plugins should ever have implemented this spoofed feature list EXPECT_EQ(0u, foundPlugins.size()); diff --git a/src/SpecifyData_TEST.cc b/src/SpecifyData_TEST.cc index e85d353ae..4b46f28c4 100644 --- a/src/SpecifyData_TEST.cc +++ b/src/SpecifyData_TEST.cc @@ -23,6 +23,8 @@ #include "gz/physics/SpecifyData.hh" #include "gz/math/Vector3.hh" +using namespace gz; + ///////////////////////////////////////////////// TEST(SpecifyData, RequirementsAccessConstruction) { @@ -221,11 +223,11 @@ TEST(SpecifyData, RequirementsAccessConstruction) ///////////////////////////////////////////////// TEST(SpecifyData, QueryCounting) { - const gz::physics::CompositeData::QueryMode normal = - gz::physics::CompositeData::QueryMode::NORMAL; + const physics::CompositeData::QueryMode normal = + physics::CompositeData::QueryMode::NORMAL; - const gz::physics::CompositeData::QueryMode silent = - gz::physics::CompositeData::QueryMode::SILENT; + const physics::CompositeData::QueryMode silent = + physics::CompositeData::QueryMode::SILENT; RequireStringBoolChar data; @@ -444,7 +446,7 @@ TEST(SpecifyData, QueryCounting) // Test StatusOf on an existing queried required type usedExpectedDataAccess = false; - gz::physics::CompositeData::DataStatus status = + physics::CompositeData::DataStatus status = data.StatusOf(); EXPECT_TRUE(usedExpectedDataAccess); EXPECT_TRUE(status.exists); @@ -506,11 +508,11 @@ TEST(SpecifyData, QueryCounting) ///////////////////////////////////////////////// TEST(SpecifyData, ConstQueryCounting) { - const gz::physics::CompositeData::QueryMode normal = - gz::physics::CompositeData::QueryMode::NORMAL; + const physics::CompositeData::QueryMode normal = + physics::CompositeData::QueryMode::NORMAL; - const gz::physics::CompositeData::QueryMode silent = - gz::physics::CompositeData::QueryMode::SILENT; + const physics::CompositeData::QueryMode silent = + physics::CompositeData::QueryMode::SILENT; RequireStringBoolChar data; @@ -611,16 +613,16 @@ TEST(SpecifyData, Remove) ///////////////////////////////////////////////// TEST(SpecifyData, CountData) { - EXPECT_EQ(1u, gz::physics::CountUpperLimitOfRequiredData< + EXPECT_EQ(1u, physics::CountUpperLimitOfRequiredData< RequireString>()); - EXPECT_EQ(1u, gz::physics::CountUpperLimitOfExpectedData< + EXPECT_EQ(1u, physics::CountUpperLimitOfExpectedData< RequireString>()); - EXPECT_EQ(3u, gz::physics::CountUpperLimitOfRequiredData< + EXPECT_EQ(3u, physics::CountUpperLimitOfRequiredData< RequireStringBoolChar>()); - EXPECT_EQ(5u, gz::physics::CountUpperLimitOfExpectedData< + EXPECT_EQ(5u, physics::CountUpperLimitOfExpectedData< RequireStringBoolChar>()); // Note that there are only 3 unique requirements and 5 unique expectations @@ -628,34 +630,34 @@ TEST(SpecifyData, CountData) // of constexpr, we only provide an upper limit of the count which will count // repeated data specifications once for each repeat. In the future, if we can // find a way to push these tests down to 3u and 5u, that would be ideal. - EXPECT_EQ(4u, gz::physics::CountUpperLimitOfRequiredData< + EXPECT_EQ(4u, physics::CountUpperLimitOfRequiredData< RedundantSpec>()); - EXPECT_EQ(6u, gz::physics::CountUpperLimitOfExpectedData< + EXPECT_EQ(6u, physics::CountUpperLimitOfExpectedData< RedundantSpec>()); } ///////////////////////////////////////////////// TEST(SpecifyData, OtherDataTypes) { - gz::physics::RequireData stringData; + physics::RequireData stringData; EXPECT_TRUE(stringData.Has()); EXPECT_EQ("", stringData.Get()); stringData.Get() = "my_new_string"; EXPECT_EQ("my_new_string", stringData.Get()); - gz::physics::RequireData vector3dData; + physics::RequireData vector3dData; EXPECT_FALSE(vector3dData.Has()); - EXPECT_TRUE(vector3dData.Has()); - EXPECT_EQ(gz::math::Vector3d(), - vector3dData.Get()); + EXPECT_TRUE(vector3dData.Has()); + EXPECT_EQ(math::Vector3d(), + vector3dData.Get()); // We can add a string to the vector3d composite data. vector3dData.Get() = "my_new_string"; EXPECT_EQ("my_new_string", vector3dData.Get()); // We can also set the vector3d data - vector3dData.Get().Set(1, 2, 3); - EXPECT_EQ(gz::math::Vector3d(1, 2, 3), - vector3dData.Get()); + vector3dData.Get().Set(1, 2, 3); + EXPECT_EQ(math::Vector3d(1, 2, 3), + vector3dData.Get()); } ///////////////////////////////////////////////// @@ -688,11 +690,11 @@ TEST(SpecifyData, Copy) ///////////////////////////////////////////////// TEST(SpecifyData, CopyExpectData) { - gz::physics::ExpectData data; + physics::ExpectData data; data.Get().myString = "old_string"; EXPECT_EQ("old_string", data.Get().myString); - gz::physics::ExpectDatacopyCtor(data); + physics::ExpectDatacopyCtor(data); EXPECT_EQ("old_string", copyCtor.Get().myString); // Modify the original and check that the copy is not affected @@ -700,7 +702,7 @@ TEST(SpecifyData, CopyExpectData) EXPECT_EQ("old_string", copyCtor.Get().myString); } -class ExpectString : public virtual gz::physics::ExpectData +class ExpectString : public virtual physics::ExpectData { }; diff --git a/src/TestUtilities.hh b/src/TestUtilities.hh index 8b66b2a86..05ed29fbc 100644 --- a/src/TestUtilities.hh +++ b/src/TestUtilities.hh @@ -25,9 +25,10 @@ #include +using namespace gz; namespace test { - class UnimplementedFeature : public virtual gz::physics::Feature + class UnimplementedFeature : public virtual physics::Feature { public: template class Implementation : public virtual Feature::Implementation @@ -39,10 +40,10 @@ namespace test }; } -void PrimeTheLoader(gz::plugin::Loader &_loader) +void PrimeTheLoader(plugin::Loader &_loader) { for (const std::string &library - : gz::physics::test::g_PhysicsPluginLibraries) + : physics::test::g_PhysicsPluginLibraries) { if (!library.empty()) _loader.LoadLib(library); diff --git a/test/Utils.hh b/test/Utils.hh index 30f664601..5755c6618 100644 --- a/test/Utils.hh +++ b/test/Utils.hh @@ -18,221 +18,249 @@ #ifndef GZ_PHYSICS_TEST_UTILS_HH_ #define GZ_PHYSICS_TEST_UTILS_HH_ +#include + #include #include #include -namespace gz +namespace gz::physics::test +{ +///////////////////////////////////////////////// +template +VectorType RandomVector(const double range) +{ + VectorType v; + for (std::size_t i = 0; i < VectorType::RowsAtCompileTime; ++i) + v[i] = math::Rand::DblUniform(-range, range); + + return v; +} + +///////////////////////////////////////////////// +template +struct Rotation { - namespace physics + /// \brief Randomize the orientation of a 3D pose + static void Randomize(gz::physics::Pose &_pose) + { + for (std::size_t i = 0; i < 3; ++i) + { + Vector axis = Vector::Zero(); + axis[i] = 1.0; + _pose.rotate(Eigen::AngleAxis( + static_cast(math::Rand::DblUniform(0, 2*GZ_PI)), axis)); + } + } + + static bool Equal( + const Eigen::Matrix &_R1, + const Eigen::Matrix &_R2, + const double _tolerance) { - namespace test + Eigen::AngleAxis R(_R1.transpose() * _R2); + if (std::abs(R.angle()) > _tolerance) { - ///////////////////////////////////////////////// - template - VectorType RandomVector(const double range) - { - VectorType v; - for (std::size_t i = 0; i < VectorType::RowsAtCompileTime; ++i) - v[i] = math::Rand::DblUniform(-range, range); - - return v; - } - - ///////////////////////////////////////////////// - template - struct Rotation - { - /// \brief Randomize the orientation of a 3D pose - static void Randomize(gz::physics::Pose &_pose) - { - for (std::size_t i = 0; i < 3; ++i) - { - Vector axis = Vector::Zero(); - axis[i] = 1.0; - _pose.rotate(Eigen::AngleAxis( - static_cast(math::Rand::DblUniform(0, 2*GZ_PI)), axis)); - } - } - - static bool Equal( - const Eigen::Matrix &_R1, - const Eigen::Matrix &_R2, - const double _tolerance) - { - Eigen::AngleAxis R(_R1.transpose() * _R2); - if (std::abs(R.angle()) > _tolerance) - { - std::cout << "Difference in angle: " << R.angle() << std::endl; - return false; - } - - return true; - } - - static AngularVector Apply( - const Eigen::Matrix &_R, - const AngularVector &_input) - { - // In 3D simulation, this is a normal multiplication - return _R * _input; - } - }; - - ///////////////////////////////////////////////// - template - struct Rotation - { - /// \brief Randomize the orientation of a 2D pose - static void Randomize(gz::physics::Pose &_pose) - { - _pose.rotate(Eigen::Rotation2D( - math::Rand::DblUniform(0, 2*GZ_PI))); - } - - static bool Equal( - const Eigen::Matrix &_R1, - const Eigen::Matrix &_R2, - const double _tolerance) - { - // Choose the largest of either 1.0 or the size of the larger angle. - const double scale = - std::max( - static_cast(1.0), - std::max( - Eigen::Rotation2D(_R1).angle(), - Eigen::Rotation2D(_R2).angle())); - - const Eigen::Rotation2D R(_R1.transpose() * _R2); - if (std::abs(R.angle()/scale) > _tolerance) - { - std::cout << "Scaled difference in angle: " - << R.angle()/scale << " | Difference: " << R.angle() - << " | Scale: " << scale - << " | (Tolerance: " << _tolerance << ")" << std::endl; - return false; - } - - return true; - } - - static AngularVector Apply( - const Eigen::Matrix &, - const AngularVector &_input) - { - // Angular vectors cannot be rotated in 2D simulations, so we just - // pass back the value that was given. - return _input; - } - }; - - ///////////////////////////////////////////////// - template - FrameData RandomFrameData() - { - using LinearVector = LinearVector; - using AngularVector = AngularVector; - - FrameData data; - data.pose.translation() = RandomVector(100.0); - Rotation::Randomize(data.pose); - data.linearVelocity = RandomVector(10.0); - data.angularVelocity = RandomVector(10.0); - data.linearAcceleration = RandomVector(1.0); - data.angularAcceleration = RandomVector(1.0); - - return data; - } - - ///////////////////////////////////////////////// - template - bool Equal(const Vector &_vec1, - const Vector &_vec2, - const double _tolerance, - const std::string &_label = "vectors") - { - // Choose the largest of either 1.0 or the length of the longer vector. - const double scale = std::max(static_cast(1.0), - std::max(_vec1.norm(), _vec2.norm())); - const double diff = (_vec1 - _vec2).norm(); - if (diff/scale <= _tolerance) - return true; - - std::cout << "Scaled difference in " << _label << ": " << diff/scale - << " | Difference: " << diff << " | Scale: " << scale - << " | (Tolerance: " << _tolerance << ")" << std::endl; - - return false; - } - - ///////////////////////////////////////////////// - template - bool Equal(const AlignedBox &_box1, - const AlignedBox &_box2, - const double _tolerance) - { - bool min = Equal(_box1.min(), _box2.min(), _tolerance, "box minimums"); - bool max = Equal(_box1.max(), _box2.max(), _tolerance, "box maximums"); - return min && max; - } - - ///////////////////////////////////////////////// - template - bool Equal(const Pose &_T1, - const Pose &_T2, - const double _tolerance) - { - bool result = true; - result &= Equal(Vector(_T1.translation()), - Vector(_T2.translation()), - _tolerance, "position"); - - result &= Rotation::Equal( - _T1.linear(), _T2.linear(), _tolerance); - - return result; - } - - ///////////////////////////////////////////////// - template - bool Equal(const FrameData &_data1, - const FrameData &_data2, - const double _tolerance) - { - bool result = true; - result &= Equal(_data1.pose, _data2.pose, _tolerance); - - result &= Equal(_data1.linearVelocity, _data2.linearVelocity, - _tolerance, "linear velocity"); - - result &= Equal(_data1.angularVelocity, _data2.angularVelocity, - _tolerance, "angular velocity"); - - result &= Equal(_data1.linearAcceleration, _data2.linearAcceleration, - _tolerance, "linear acceleration"); - - result &= Equal(_data1.angularAcceleration, - _data2.angularAcceleration, - _tolerance, "angular acceleration"); - - return result; - } - - ///////////////////////////////////////////////// - template - bool Equal(const Wrench &_data1, - const Wrench &_data2, - const double _tolerance) - { - bool result = true; - result &= Equal(_data1.torque, _data2.torque, _tolerance, "torque"); - result &= Equal(_data1.force, _data2.force, _tolerance, "force"); - - return result; - } + std::cout << "Difference in angle: " << R.angle() << std::endl; + return false; } + + return true; } + + static AngularVector Apply( + const Eigen::Matrix &_R, + const AngularVector &_input) + { + // In 3D simulation, this is a normal multiplication + return _R * _input; + } +}; + +///////////////////////////////////////////////// +template +struct Rotation +{ + /// \brief Randomize the orientation of a 2D pose + static void Randomize(gz::physics::Pose &_pose) + { + _pose.rotate(Eigen::Rotation2D( + math::Rand::DblUniform(0, 2 * GZ_PI))); + } + + static bool Equal( + const Eigen::Matrix &_R1, + const Eigen::Matrix &_R2, + const double _tolerance) + { + // Choose the largest of either 1.0 or the size of the larger angle. + const double scale = + std::max( + static_cast(1.0), + std::max( + Eigen::Rotation2D(_R1).angle(), + Eigen::Rotation2D(_R2).angle())); + + const Eigen::Rotation2D R(_R1.transpose() * _R2); + if (std::abs(R.angle()/scale) > _tolerance) + { + std::cout << "Scaled difference in angle: " + << R.angle()/scale << " | Difference: " << R.angle() + << " | Scale: " << scale + << " | (Tolerance: " << _tolerance << ")" << std::endl; + return false; + } + + return true; + } + + static AngularVector Apply( + const Eigen::Matrix &, + const AngularVector &_input) + { + // Angular vectors cannot be rotated in 2D simulations, so we just + // pass back the value that was given. + return _input; + } +}; + +///////////////////////////////////////////////// +template +FrameData RandomFrameData() +{ + using LinearVector = LinearVector; + using AngularVector = AngularVector; + + FrameData data; + data.pose.translation() = RandomVector(100.0); + Rotation::Randomize(data.pose); + data.linearVelocity = RandomVector(10.0); + data.angularVelocity = RandomVector(10.0); + data.linearAcceleration = RandomVector(1.0); + data.angularAcceleration = RandomVector(1.0); + + return data; +} + +///////////////////////////////////////////////// +template +bool Equal(const Vector &_vec1, + const Vector &_vec2, + const double _tolerance, + const std::string &_label = "vectors") +{ + // Choose the largest of either 1.0 or the length of the longer vector. + const double scale = std::max(static_cast(1.0), + std::max(_vec1.norm(), _vec2.norm())); + const double diff = (_vec1 - _vec2).norm(); + if (diff / scale <= _tolerance) + return true; + + std::cout << "Scaled difference in " << _label << ": " << diff/scale + << " | Difference: " << diff << " | Scale: " << scale + << " | (Tolerance: " << _tolerance << ")" << std::endl; + + return false; +} + +///////////////////////////////////////////////// +template +bool Equal(const AlignedBox &_box1, + const AlignedBox &_box2, + const double _tolerance) +{ + bool min = Equal(_box1.min(), _box2.min(), _tolerance, "box minimums"); + bool max = Equal(_box1.max(), _box2.max(), _tolerance, "box maximums"); + return min && max; +} + +///////////////////////////////////////////////// +template +bool Equal(const Pose &_T1, + const Pose &_T2, + const double _tolerance) +{ + bool result = true; + result &= Equal(Vector(_T1.translation()), + Vector(_T2.translation()), + _tolerance, "position"); + + result &= Rotation::Equal( + _T1.linear(), _T2.linear(), _tolerance); + + return result; } +///////////////////////////////////////////////// +template +bool Equal(const FrameData &_data1, + const FrameData &_data2, + const double _tolerance) +{ + bool result = true; + result &= Equal(_data1.pose, _data2.pose, _tolerance); + + result &= Equal(_data1.linearVelocity, _data2.linearVelocity, + _tolerance, "linear velocity"); + + result &= Equal(_data1.angularVelocity, _data2.angularVelocity, + _tolerance, "angular velocity"); + + result &= Equal(_data1.linearAcceleration, _data2.linearAcceleration, + _tolerance, "linear acceleration"); + + result &= Equal(_data1.angularAcceleration, + _data2.angularAcceleration, + _tolerance, "angular acceleration"); + + return result; +} + +///////////////////////////////////////////////// +template +bool Equal(const Wrench &_data1, + const Wrench &_data2, + const double _tolerance) +{ + bool result = true; + result &= Equal(_data1.torque, _data2.torque, _tolerance, "torque"); + result &= Equal(_data1.force, _data2.force, _tolerance, "force"); + + return result; +} + +/// \brief A predicate-formatter for asserting that two vectors are approximately equal. +class AssertVectorApprox +{ + /// \brief Constructor + /// \param[in] _tol Tolerance for comparison + public: explicit AssertVectorApprox(double _tol = 1e-6) : tol(_tol) + { + } + + /// \brief Compare two vectors + /// \param[in] _mExpr left hand side expression + /// \param[in] _nExpr right hand side expression + /// \param[in] _m left hand side value + /// \param[in] _n right hand side value + /// \return Gtest assertion result of the comparison + public: ::testing::AssertionResult operator()( + const char *_mExpr, const char *_nExpr, Eigen::Vector3d _m, + Eigen::Vector3d _n) + { + if (Equal(_m, _n, this->tol)) + return ::testing::AssertionSuccess(); + + return ::testing::AssertionFailure() + << _mExpr << " and " << _nExpr << " ([" << _m.transpose() + << "] and [" << _n.transpose() << "]" + << ") are not equal"; + } + + /// \brief Tolerance for comparison + private: double tol; +}; + +} // namespace gz::physics::test -#endif +#endif // GZ_PHYSICS_TEST_UTILS_HH_ diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 30bc561c3..c09e61f77 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -1,12 +1,15 @@ set(TEST_TYPE "COMMON_TEST") set(tests + added_mass addexternalforcetorque basic_test collisions construct_empty_world free_joint_features joint_features + joint_mimic_features + joint_transmitted_wrench_features kinematic_features link_features shape_features diff --git a/test/common_test/TestLibLoader.hh b/test/common_test/TestLibLoader.hh index 87d73054a..5d5910eea 100644 --- a/test/common_test/TestLibLoader.hh +++ b/test/common_test/TestLibLoader.hh @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -56,7 +57,7 @@ class TestLibLoader /// \brief Get Physics Engine name based on the plugin name /// \param[in] _name Plugin name /// \return Name of the Physics Engine - std::string PhysicsEngineName(std::string _name) + static std::string PhysicsEngineName(std::string _name) { std::vector tokens = gz::common::split(_name, "::"); if (tokens.size() == 4) @@ -76,6 +77,19 @@ class TestLibLoader return ""; } + /// \brief Check if the physics engine provided by the plugin is in the + /// provided list. + /// \param[in] _pluginName Plugin name. PhysicsEngineName will first be called + /// to determine the engine name. + /// \param[in] _engineList List of engine names. + /// \return True if the engine is in the list. + public: + static bool EngineInList(const std::string &_pluginName, + const std::unordered_set &_engineList) + { + return _engineList.count(PhysicsEngineName(_pluginName)) != 0; + } + private: static std::string& LibToTest() { static std::string libToTest = ""; @@ -85,4 +99,25 @@ class TestLibLoader } } +/// \brief Check that the current engine being tested is supported. +/// If the engine is not in the set of passed arguments, the test is skipped +/// Adapted from +/// https://github.com/gazebosim/gz-rendering/blob/c2e72ee51a7e4dba5156faa96c972c63ca5ab437/test/common_test/CommonRenderingTest.hh#L127-L138 +/// Example: +/// Skip test if engine is not dart or bullet +/// CHECK_SUPPORTED_ENGINE(name, "dart", "bullet"); +#define CHECK_SUPPORTED_ENGINE(engineToTest, ...) \ + if (!gz::physics::TestLibLoader::EngineInList(engineToTest, {__VA_ARGS__})) \ + GTEST_SKIP() << "Engine '" << engineToTest << "' unsupported"; + +/// \brief Check that the current engine being tested is unsupported +/// If the engine is in the set of passed arguments, the test is skipped +/// Adapted from +/// https://github.com/gazebosim/gz-rendering/blob/c2e72ee51a7e4dba5156faa96c972c63ca5ab437/test/common_test/CommonRenderingTest.hh#L127-L138 +/// Example: +/// Skip test if engine is bullet-featherstone +/// CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone"); +#define CHECK_UNSUPPORTED_ENGINE(engineToTest, ...) \ + if (gz::physics::TestLibLoader::EngineInList(engineToTest, {__VA_ARGS__})) \ + GTEST_SKIP() << "Engine '" << (engineToTest) << "' unsupported"; #endif diff --git a/test/common_test/added_mass.cc b/test/common_test/added_mass.cc new file mode 100644 index 000000000..f46468bab --- /dev/null +++ b/test/common_test/added_mass.cc @@ -0,0 +1,174 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include + +#include +#include + +#include "TestLibLoader.hh" +#include "../Utils.hh" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +using AssertVectorApprox = gz::physics::test::AssertVectorApprox; + +// The features that an engine must have to be loaded by this loader. +using AddedMassFeatures = gz::physics::FeatureList< + gz::physics::AddedMass, + gz::physics::GetEngineInfo, + gz::physics::Gravity, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::LinkFrameSemantics, + gz::physics::GetModelFromWorld, + gz::physics::GetLinkFromModel, + gz::physics::ForwardStep +>; + +class AddedMassFeaturesTest: + public testing::Test, public gz::physics::TestLibLoader +{ + // Documentation inherited + public: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + + auto plugins = loader.LoadLib(AddedMassFeaturesTest::GetLibToTest()); + + // TODO(ahcorde): We should also run the 3f, 2d, and 2f variants of + // FindFeatures + pluginNames = gz::physics::FindFeatures3d::From(loader); + if (pluginNames.empty()) + { + std::cerr << "No plugins with required features found in " << GetLibToTest(); + GTEST_SKIP(); + } + } + + public: std::set pluginNames; + public: gz::plugin::Loader loader; +}; + +///////////////////////////////////////////////// +TEST_F(AddedMassFeaturesTest, Gravity) +{ + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + EXPECT_TRUE(engine->GetName().find(this->PhysicsEngineName(name)) != + std::string::npos); + + sdf::Root root; + const sdf::Errors errors = root.Load( + gz::common::joinPaths(TEST_WORLD_DIR, "falling_added_mass.world")); + EXPECT_TRUE(errors.empty()) << errors; + const sdf::World *sdfWorld = root.WorldByIndex(0); + EXPECT_NE(nullptr, sdfWorld); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + EXPECT_NE(nullptr, world); + + auto graphErrors = sdfWorld->ValidateGraphs(); + EXPECT_EQ(0u, graphErrors.size()) << graphErrors; + + AssertVectorApprox vectorPredicate6(1e-6); + + // Set gravity to a nice round number + world->SetGravity({0, 0, -10}); + + // Link poses + const Eigen::Vector3d initialLinkPosition(0, 0, 2); + const Eigen::Vector3d finalLinkPosition(0, 0, -3.005); + const Eigen::Vector3d finalLinkPositionAddedMass(0, 0, -0.5025); + + // This tests that the physics plugin correctly considers added mass. + for (auto modelName: {"sphere", "sphere_zero_added_mass", "sphere_added_mass", "heavy_sphere"}) + { + auto model = world->GetModel(modelName); + ASSERT_NE(nullptr, model); + + auto link = model->GetLink(0); + ASSERT_NE(nullptr, link); + + Eigen::Vector3d pos = link->FrameDataRelativeToWorld().pose.translation(); + EXPECT_PRED_FORMAT2(vectorPredicate6, + initialLinkPosition, + pos); + } + + gz::physics::ForwardStep::Input input; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Output output; + + const size_t numSteps = 1000; + for (size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + } + + // Confirm that the models with zero added mass behave consistently + for (auto modelName: {"sphere", "sphere_zero_added_mass", "heavy_sphere"}) + { + auto model = world->GetModel(modelName); + ASSERT_NE(nullptr, model); + + auto link = model->GetLink(0); + ASSERT_NE(nullptr, link); + + Eigen::Vector3d pos = link->FrameDataRelativeToWorld().pose.translation(); + EXPECT_PRED_FORMAT2(vectorPredicate6, + finalLinkPosition, + pos); + } + + for (auto modelName: {"sphere_added_mass"}) + { + auto model = world->GetModel(modelName); + ASSERT_NE(nullptr, model); + + auto link = model->GetLink(0); + ASSERT_NE(nullptr, link); + + Eigen::Vector3d pos = link->FrameDataRelativeToWorld().pose.translation(); + EXPECT_PRED_FORMAT2(vectorPredicate6, + finalLinkPositionAddedMass, + pos); + } + } + +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + if (!AddedMassFeaturesTest::init(argc, argv)) + return -1; + return RUN_ALL_TESTS(); +} diff --git a/test/common_test/addexternalforcetorque.cc b/test/common_test/addexternalforcetorque.cc index fab35842b..93409f345 100644 --- a/test/common_test/addexternalforcetorque.cc +++ b/test/common_test/addexternalforcetorque.cc @@ -78,28 +78,7 @@ class LinkFeaturesTest: public: gz::plugin::Loader loader; }; -// A predicate-formatter for asserting that two vectors are approximately equal. -class AssertVectorApprox -{ - public: explicit AssertVectorApprox(double _tol = 1e-6) : tol(_tol) - { - } - - public: ::testing::AssertionResult operator()( - const char *_mExpr, const char *_nExpr, Eigen::Vector3d _m, - Eigen::Vector3d _n) - { - if (gz::physics::test::Equal(_m, _n, this->tol)) - return ::testing::AssertionSuccess(); - - return ::testing::AssertionFailure() - << _mExpr << " and " << _nExpr << " ([" << _m.transpose() - << "] and [" << _n.transpose() << "]" - << ") are not equal"; - } - - private: double tol; -}; +using AssertVectorApprox = gz::physics::test::AssertVectorApprox; struct LinkFeaturesList : gz::physics::FeatureList< gz::physics::AddLinkExternalForceTorque, @@ -116,7 +95,7 @@ struct LinkFeaturesList : gz::physics::FeatureList< using LinkFeaturesTestTypes = ::testing::Types; TYPED_TEST_SUITE(LinkFeaturesTest, - LinkFeaturesTestTypes); + LinkFeaturesTestTypes,); TYPED_TEST(LinkFeaturesTest, JointSetCommand) { diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 985afbb69..5c644272c 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1148,6 +1148,11 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) { for (const std::string &name : this->pluginNames) { +#ifdef _WIN32 + // On windows, there's a tolerance issue with bullet-featherstone on the + // last body2LinearVelocity expectation. Disabling until it's resolved. + CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone") +#endif std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); @@ -1483,689 +1488,6 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachSpawnedModel) EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); } -template -class JointTransmittedWrenchFixture : public JointFeaturesTest -{ - public: using WorldPtr = gz::physics::World3dPtr; - public: using ModelPtr = gz::physics::Model3dPtr; - public: using JointPtr = gz::physics::Joint3dPtr; - public: using LinkPtr = gz::physics::Link3dPtr; - - protected: void SetUp() override - { - JointFeaturesTest::SetUp(); - for (const std::string &name : this->pluginNames) - { - std::cout << "Testing plugin: " << name << std::endl; - gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); - - auto engine = gz::physics::RequestEngine3d::From(plugin); - ASSERT_NE(nullptr, engine); - - sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "pendulum_joint_wrench.sdf")); - ASSERT_TRUE(errors.empty()) << errors.front(); - - this->world = engine->ConstructWorld(*root.WorldByIndex(0)); - ASSERT_NE(nullptr, this->world); - - this->model = this->world->GetModel("pendulum"); - ASSERT_NE(nullptr, this->model); - this->motorJoint = this->model->GetJoint("motor_joint"); - ASSERT_NE(nullptr, this->motorJoint); - this->sensorJoint = this->model->GetJoint("sensor_joint"); - ASSERT_NE(nullptr, this->sensorJoint); - this->armLink = this->model->GetLink("arm"); - ASSERT_NE(nullptr, this->armLink); - } - } - - public: void Step(int _iters) - { - for (int i = 0; i < _iters; ++i) - { - this->world->Step(this->output, this->state, this->input); - } - } - - public: gz::physics::ForwardStep::Output output; - public: gz::physics::ForwardStep::State state; - public: gz::physics::ForwardStep::Input input; - public: WorldPtr world; - public: ModelPtr model; - public: JointPtr motorJoint; - public: JointPtr sensorJoint; - public: LinkPtr armLink; - - // From SDFormat file - static constexpr double kGravity = 9.8; - static constexpr double kArmLinkMass = 6.0; - static constexpr double kSensorLinkMass = 0.4; - // MOI in the z-axis - static constexpr double kSensorLinkMOI = 0.02; - static constexpr double kArmLength = 1.0; -}; - -struct JointTransmittedWrenchFeatureList : gz::physics::FeatureList< - gz::physics::ForwardStep, - gz::physics::FreeGroupFrameSemantics, - gz::physics::GetBasicJointState, - gz::physics::GetEngineInfo, - gz::physics::GetJointFromModel, - gz::physics::GetJointTransmittedWrench, - gz::physics::GetLinkFromModel, - gz::physics::GetModelFromWorld, - gz::physics::LinkFrameSemantics, - gz::physics::SetBasicJointState, - gz::physics::SetFreeGroupWorldPose, - gz::physics::sdf::ConstructSdfWorld -> { }; - -using JointTransmittedWrenchFeaturesTestTypes = - ::testing::Types; -TYPED_TEST_SUITE(JointTransmittedWrenchFixture, - JointTransmittedWrenchFeaturesTestTypes); - -TYPED_TEST(JointTransmittedWrenchFixture, PendulumAtZeroAngle) -{ - // Run a few steps for the constraint forces to stabilize - this->Step(10); - - // Test wrench expressed in different frames - { - auto wrenchAtMotorJoint = this->motorJoint->GetTransmittedWrench(); - gz::physics::Wrench3d expectedWrenchAtMotorJoint{ - gz::physics::Vector3d::Zero(), - {-this->kGravity * (this->kArmLinkMass + this->kSensorLinkMass), 0, 0}}; - - EXPECT_TRUE( - gz::physics::test::Equal(expectedWrenchAtMotorJoint, wrenchAtMotorJoint, 1e-4)); - } - { - auto wrenchAtMotorJointInWorld = this->motorJoint->GetTransmittedWrench( - this->motorJoint->GetFrameID(), gz::physics::FrameID::World()); - gz::physics::Wrench3d expectedWrenchAtMotorJointInWorld{ - gz::physics::Vector3d::Zero(), - {0, 0, this->kGravity * (this->kArmLinkMass + this->kSensorLinkMass)}}; - - EXPECT_TRUE(gz::physics::test::Equal(expectedWrenchAtMotorJointInWorld, - wrenchAtMotorJointInWorld, 1e-4)); - } - { - auto wrenchAtMotorJointInArm = this->motorJoint->GetTransmittedWrench( - this->armLink->GetFrameID(), this->armLink->GetFrameID()); - // The arm frame is rotated by 90° in the Y-axis of the joint frame. - gz::physics::Wrench3d expectedWrenchAtMotorJointInArm{ - gz::physics::Vector3d::Zero(), - {0, 0, this->kGravity * (this->kArmLinkMass + this->kSensorLinkMass)}}; - - EXPECT_TRUE(gz::physics::test::Equal(expectedWrenchAtMotorJointInArm, - wrenchAtMotorJointInArm, 1e-4)); - } -} - -TYPED_TEST(JointTransmittedWrenchFixture, PendulumInMotion) -{ - // Start pendulum at 90° (parallel to the ground) and stop at about 40° - // so that we have non-trivial test expectations. - this->motorJoint->SetPosition(0, GZ_DTOR(90.0)); - this->Step(350); - - // Given the position (θ), velocity (ω), and acceleration (α) of the joint - // and distance from the joint to the COM (r), the reaction forces in - // the tangent direction (Ft) and normal direction (Fn) are given by: - // - // Ft = m * α * r + (m * g * sin(θ)) = m * (α * r + g * sin(θ)) - // Fn = -m * ω² * r - (m * g * cos(θ)) = -m * (ω² * r + g * cos(θ)) - { - const double theta = this->motorJoint->GetPosition(0); - const double omega = this->motorJoint->GetVelocity(0); - // In order to get the math to work out, we need to use the joint - // acceleration and transmitted wrench from the current time step with the - // joint position and velocity from the previous time step. That is, we need - // the position and velocity before they are integrated. - this->Step(1); - const double alpha = this->motorJoint->GetAcceleration(0); - - auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); - - const double armTangentForce = - this->kArmLinkMass * ((alpha * this->kArmLength / 2.0) + (this->kGravity * sin(theta))); - - const double motorLinkTangentForce = - this->kSensorLinkMass * this->kGravity * sin(theta); - - const double armNormalForce = - -this->kArmLinkMass * - ((std::pow(omega, 2) * this->kArmLength / 2.0) + (this->kGravity * cos(theta))); - - const double motorLinkNormalForce = - -this->kSensorLinkMass * this->kGravity * cos(theta); - - const double tangentForce = armTangentForce + motorLinkTangentForce; - const double normalForce = armNormalForce + motorLinkNormalForce; - - // The orientation of the joint frame is such that the normal force is - // parallel to the x-axis and the tangent force is parallel to the y-axis. - gz::physics::Wrench3d expectedWrenchAtMotorJointInJoint{ - gz::physics::Vector3d::Zero(), {normalForce, tangentForce, 0}}; - - EXPECT_TRUE(gz::physics::test::Equal(expectedWrenchAtMotorJointInJoint, - wrenchAtMotorJointInJoint, 1e-4)); - } - - // Test Wrench expressed in different frames - { - auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); - // This is just a rotation of the wrench to be expressed in the world's - // coordinate frame - auto wrenchAtMotorJointInWorld = this->motorJoint->GetTransmittedWrench( - this->motorJoint->GetFrameID(), gz::physics::FrameID::World()); - // The joint frame is rotated by 90° along the world's y-axis - Eigen::Quaterniond R_WJ = - Eigen::AngleAxisd(GZ_PI_2, Eigen::Vector3d(0, 1, 0)) * - Eigen::AngleAxisd(this->motorJoint->GetPosition(0), - Eigen::Vector3d(0, 0, 1)); - - gz::physics::Wrench3d expectedWrenchAtMotorJointInWorld{ - gz::physics::Vector3d::Zero(), R_WJ * wrenchAtMotorJointInJoint.force}; - EXPECT_TRUE(gz::physics::test::Equal(expectedWrenchAtMotorJointInWorld, - wrenchAtMotorJointInWorld, 1e-4)); - - // This moves the point of application and changes the coordinate frame - gz::physics::Wrench3d wrenchAtArmInArm = this->motorJoint->GetTransmittedWrench( - this->armLink->GetFrameID(), this->armLink->GetFrameID()); - - // Notation: arm link (A), joint (J) - Eigen::Isometry3d X_AJ; - // Pose of joint (J) in arm link (A) as specified in the SDFormat file. - X_AJ = Eigen::AngleAxisd(GZ_PI_2, Eigen::Vector3d(0, 1, 0)); - X_AJ.translation() = gz::physics::Vector3d(0, 0, this->kArmLength / 2.0); - gz::physics::Wrench3d expectedWrenchAtArmInArm; - - expectedWrenchAtArmInArm.force = - X_AJ.linear() * wrenchAtMotorJointInJoint.force; - - expectedWrenchAtArmInArm.torque = - X_AJ.linear() * wrenchAtMotorJointInJoint.torque + - X_AJ.translation().cross(expectedWrenchAtArmInArm.force); - - EXPECT_TRUE(gz::physics::test::Equal(expectedWrenchAtArmInArm, wrenchAtArmInArm, 1e-4)); - } -} - -TYPED_TEST(JointTransmittedWrenchFixture, ValidateWrenchWithSecondaryJoint) -{ - // Start pendulum at 90° (parallel to the ground) and stop at about 40° - // so that we have non-trivial test expectations. - this->motorJoint->SetPosition(0, GZ_DTOR(90.0)); - this->Step(350); - const double theta = this->motorJoint->GetPosition(0); - // In order to get the math to work out, we need to use the joint - // acceleration and transmitted wrench from the current time step with the - // joint position and velocity from the previous time step. That is, we need - // the position and velocity before they are integrated. - this->Step(1); - const double alpha = this->motorJoint->GetAcceleration(0); - - auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); - auto wrenchAtSensorInSensor = this->sensorJoint->GetTransmittedWrench(); - - // Since sensor_link has moment of inertia, the fixed joint will transmit a - // torque necessary to rotate the sensor. This is not detected by the motor - // joint because no force is transmitted along the revolute axis. On the - // other hand, the mass of sensor_link will contribute to the constraint - // forces on the motor joint, but these won't be detected by the sensor - // joint. - gz::physics::Vector3d expectedTorqueDiff{0, 0, this->kSensorLinkMOI * alpha}; - gz::physics::Vector3d expectedForceDiff{-this->kSensorLinkMass * this->kGravity * cos(theta), - this->kSensorLinkMass * this->kGravity * sin(theta), 0}; - - gz::physics::Vector3d torqueDiff = - wrenchAtMotorJointInJoint.torque - wrenchAtSensorInSensor.torque; - gz::physics::Vector3d forceDiff = - wrenchAtMotorJointInJoint.force - wrenchAtSensorInSensor.force; - EXPECT_TRUE(gz::physics::test::Equal(expectedTorqueDiff, torqueDiff, 1e-4)); - EXPECT_TRUE(gz::physics::test::Equal(expectedForceDiff, forceDiff, 1e-4)); -} - -TYPED_TEST(JointTransmittedWrenchFixture, JointLosses) -{ - // // Get DART joint pointer to set joint friction, damping, etc. - // auto dartWorld = this->world->GetDartsimWorld(); - // ASSERT_NE(nullptr, dartWorld); - // auto dartModel = dartWorld->getSkeleton(this->model->GetIndex()); - // ASSERT_NE(nullptr, dartModel); - // auto dartJoint = dartModel->getJoint(this->motorJoint->GetIndex()); - // ASSERT_NE(nullptr, dartJoint); - // - // // Joint friction - // { - // this->motorJoint->SetPosition(0, GZ_DTOR(90.0)); - // this->motorJoint->SetVelocity(0, 0); - // const double kFrictionCoef = 0.5; - // dartJoint->setCoulombFriction(0, kFrictionCoef); - // this->Step(10); - // auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); - // EXPECT_NEAR(kFrictionCoef, wrenchAtMotorJointInJoint.torque.z(), 1e-4); - // dartJoint->setCoulombFriction(0, 0.0); - // } - // - // // Joint damping - // { - // this->motorJoint->SetPosition(0, GZ_DTOR(90.0)); - // this->motorJoint->SetVelocity(0, 0); - // const double kDampingCoef = 0.2; - // dartJoint->setDampingCoefficient(0, kDampingCoef); - // this->Step(100); - // const double omega = this->motorJoint->GetVelocity(0); - // this->Step(1); - // auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); - // EXPECT_NEAR(-omega * kDampingCoef, wrenchAtMotorJointInJoint.torque.z(), - // 1e-3); - // dartJoint->setDampingCoefficient(0, 0.0); - // } - // - // // Joint stiffness - // { - // // Note: By default, the spring reference position is 0. - // this->motorJoint->SetPosition(0, GZ_DTOR(30.0)); - // this->motorJoint->SetVelocity(0, 0); - // const double kSpringStiffness = 0.7; - // dartJoint->setSpringStiffness(0, kSpringStiffness); - // this->Step(1); - // const double theta = this->motorJoint->GetPosition(0); - // this->Step(1); - // auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); - // EXPECT_NEAR(-theta * kSpringStiffness, wrenchAtMotorJointInJoint.torque.z(), - // 1e-3); - // dartJoint->setSpringStiffness(0, 0.0); - // } -} - -// Check that the transmitted wrench is affected by contact forces -TYPED_TEST(JointTransmittedWrenchFixture, ContactForces) -{ - auto box = this->world->GetModel("box"); - ASSERT_NE(nullptr, box); - auto boxFreeGroup = box->FindFreeGroup(); - ASSERT_NE(nullptr, boxFreeGroup); - gz::physics::Pose3d X_WB(Eigen::Translation3d(0, 1, 1)); - boxFreeGroup->SetWorldPose(X_WB); - - this->motorJoint->SetPosition(0, GZ_DTOR(90.0)); - // After this many steps, the pendulum is in contact with the box - this->Step(1000); - const double theta = this->motorJoint->GetPosition(0); - // Sanity check that the pendulum is at rest - EXPECT_NEAR(0.0, this->motorJoint->GetVelocity(0), 1e-3); - - auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); - - // To compute the reaction forces, we consider the pivot on the contact point - // between the pendulum and the box and the fact that the sum of moments about - // the pivot is zero. We also note that all forces, including the reaction - // forces, are in the vertical (world's z-axis) direction. - // - // Notation: - // Fp_z: Reaction force at pendulum joint (pin) in the world's z-axis - // M_b: Moment about the contact point between box and pendulum - // - // Fp_z = √(Fn² + Ft²) // Since all of the reaction force is in the world's - // z-axis - // - // ∑M_b = 0 = -Fp_z * sin(θ) * (2*r) + m₁*g*sin(θ)*r + m₂*g*sin(θ)*(2*r) - // - // Fp_z = 0.5 * g * (m₁ + 2*m₂) - // - // We can then compute the tangential (Ft) and normal (Fn) components as - // - // Ft = Fp_z * sin(θ) - // Fn = -Fp_z * cos(θ) - - const double reactionForceAtP = - 0.5 * this->kGravity * (this->kArmLinkMass + 2 * this->kSensorLinkMass); - - gz::physics::Wrench3d expectedWrenchAtMotorJointInJoint{ - gz::physics::Vector3d::Zero(), - {-reactionForceAtP * cos(theta), reactionForceAtP * sin(theta), 0}}; - - EXPECT_TRUE(gz::physics::test::Equal(expectedWrenchAtMotorJointInJoint, - wrenchAtMotorJointInJoint, 1e-4)); -} - -struct JointMimicFeatureList : gz::physics::FeatureList< - gz::physics::ForwardStep, - gz::physics::GetBasicJointProperties, - gz::physics::GetBasicJointState, - gz::physics::GetEngineInfo, - gz::physics::GetJointFromModel, - gz::physics::GetLinkFromModel, - gz::physics::GetModelFromWorld, - gz::physics::LinkFrameSemantics, - gz::physics::SetBasicJointState, - gz::physics::SetJointVelocityCommandFeature, - gz::physics::SetMimicConstraintFeature, - gz::physics::sdf::ConstructSdfWorld>{}; - -template -class JointMimicFeatureFixture : - public JointFeaturesTest{}; -using JointMimicFeatureTestTypes = - ::testing::Types; -TYPED_TEST_SUITE(JointMimicFeatureFixture, - JointMimicFeatureTestTypes); - -// Here, we test mimic constraints on various combinations of -// prismatic and revolute joints using a chain of constraints. -TYPED_TEST(JointMimicFeatureFixture, PrismaticRevoluteMimicTest) -{ - // This test contains 5 joints : 3 prismatic and 2 revolute. - // They are connected as follows : - // prismatic_joint_1 : Free joint - // prismatic_joint_2 : Mimics prismatic_joint_1 - // revolute_joint_1 : Mimics prismatic_joint_1 - // revolute_joint_2 : Mimics revolute_joint_1 - // prismatic_joint_3 : Mimics revolute_joint_1 - for (const std::string &name : this->pluginNames) - { - if(this->PhysicsEngineName(name) != "dartsim") - { - GTEST_SKIP(); - } - - std::cout << "Testing plugin: " << name << std::endl; - gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); - - auto engine = gz::physics::RequestEngine3d::From(plugin); - ASSERT_NE(nullptr, engine); - - sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, - "mimic_prismatic_world.sdf")); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = engine->ConstructWorld(*root.WorldByIndex(0)); - - auto model = world->GetModel("prismatic_model"); - - auto parentJoint = model->GetJoint("prismatic_joint_1"); - auto prismaticChildJoint1 = model->GetJoint("prismatic_joint_2"); - auto revoluteChildJoint1 = model->GetJoint("revolute_joint_1"); - auto revoluteChildJoint2 = model->GetJoint("revolute_joint_2"); - auto prismaticChildJoint2 = model->GetJoint("prismatic_joint_3"); - - // Ensure both joints start from zero angle. - EXPECT_EQ(parentJoint->GetPosition(0), 0); - EXPECT_EQ(prismaticChildJoint1->GetPosition(0), 0); - EXPECT_EQ(revoluteChildJoint1->GetPosition(0), 0); - EXPECT_EQ(revoluteChildJoint2->GetPosition(0), 0); - EXPECT_EQ(prismaticChildJoint2->GetPosition(0), 0); - - gz::physics::ForwardStep::Output output; - gz::physics::ForwardStep::State state; - gz::physics::ForwardStep::Input input; - - // Case : Without mimic constraint - - // Let the simulation run without mimic constraint. - // The positions of joints should not be equal. - double prismaticJointPrevPos = 0; - double revoluteJointPrevPos = 0; - for (int i = 0; i < 10; i++) - { - world->Step(output, state, input); - if (i > 5) - { - EXPECT_NE(prismaticJointPrevPos, prismaticChildJoint1->GetPosition(0)); - EXPECT_NE(prismaticJointPrevPos, revoluteChildJoint1->GetPosition(0)); - EXPECT_NE(revoluteJointPrevPos, revoluteChildJoint2->GetPosition(0)); - EXPECT_NE(revoluteJointPrevPos, prismaticChildJoint2->GetPosition(0)); - } - - // Update previous positions. - prismaticJointPrevPos = parentJoint->GetPosition(0); - revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0); - } - - auto testMimicFcn = [&](double multiplier, double offset, double reference) - { - // Set mimic joint constraints. - // Parent --> Child - // prismatic_joint_1 -> prismatic_joint_2 - // prismatic_joint_1 -> revolute_joint_1 - // revolute_joint_1 --> revolute_joint_2 - // revolute_joint_1 --> prismatic_joint_3 - prismaticChildJoint1->SetMimicConstraint("prismatic_joint_1", multiplier, - offset, reference); - revoluteChildJoint1->SetMimicConstraint("prismatic_joint_1", multiplier, - offset, reference); - revoluteChildJoint2->SetMimicConstraint("revolute_joint_1", multiplier, - offset, reference); - prismaticChildJoint2->SetMimicConstraint("revolute_joint_1", multiplier, - offset, reference); - - // Reset positions and run a few iterations so the positions reach nontrivial values. - parentJoint->SetPosition(0, 0); - prismaticChildJoint1->SetPosition(0, 0); - prismaticChildJoint2->SetPosition(0, 0); - revoluteChildJoint1->SetPosition(0, 0); - revoluteChildJoint2->SetPosition(0, 0); - for (int _ = 0; _ < 10; _++) - world->Step(output, state, input); - - // Child joint's position should be equal to that of parent joint in previous timestep, - // considering the offsets and multipliers. - prismaticJointPrevPos = parentJoint->GetPosition(0); - revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0); - for (int _ = 0; _ < 10; _++) - { - world->Step(output, state, input); - // Check for prismatic -> prismatic mimicking. - EXPECT_FLOAT_EQ(multiplier * (prismaticJointPrevPos - reference) + offset, - prismaticChildJoint1->GetPosition(0)); - // Check for prismatic -> revolute mimicking. - EXPECT_FLOAT_EQ(multiplier * (prismaticJointPrevPos - reference) + offset, - revoluteChildJoint1->GetPosition(0)); - // Check for revolute -> revolute mimicking. - EXPECT_FLOAT_EQ(multiplier * (revoluteJointPrevPos - reference) + offset, - revoluteChildJoint2->GetPosition(0)); - // Check for revolute --> prismatic mimicking. - EXPECT_FLOAT_EQ(multiplier * (revoluteJointPrevPos - reference) + offset, - prismaticChildJoint2->GetPosition(0)); - - // Update previous positions. - prismaticJointPrevPos = parentJoint->GetPosition(0); - revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0); - } - }; - - // Testing with different (multiplier, offset, reference) combinations. - testMimicFcn(1, 0, 0); - testMimicFcn(-1, 0, 0); - testMimicFcn(1, 0.1, 0); - testMimicFcn(1, 0.05, 0.05); - testMimicFcn(-1, 0.2, 0); - testMimicFcn(-1, 0.05, -0.05); - testMimicFcn(-2, 0, 0); - testMimicFcn(2, 0.1, 0); - } -} - -// Here, we test the mimic constraint for a pair of universal joints. -TYPED_TEST(JointMimicFeatureFixture, UniversalMimicTest) -{ - for (const std::string &name : this->pluginNames) - { - if(this->PhysicsEngineName(name) != "dartsim") - { - GTEST_SKIP(); - } - - std::cout << "Testing plugin: " << name << std::endl; - gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); - - auto engine = gz::physics::RequestEngine3d::From(plugin); - ASSERT_NE(nullptr, engine); - - sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, - "mimic_universal_world.sdf")); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = engine->ConstructWorld(*root.WorldByIndex(0)); - - // Test mimic constraint between two revolute joints. - auto model = world->GetModel("universal_model"); - auto parentJoint = model->GetJoint("universal_joint_1"); - auto childJoint = model->GetJoint("universal_joint_2"); - - // Ensure both joints start from zero angle. - EXPECT_EQ(parentJoint->GetPosition(0), 0); - EXPECT_EQ(childJoint->GetPosition(0), 0); - - gz::physics::ForwardStep::Output output; - gz::physics::ForwardStep::State state; - gz::physics::ForwardStep::Input input; - - // Case : Without mimic constraint - - // Let the simulation run without mimic constraint. - // The positions of joints should not be equal. - double parentJointPrevPosAxis1 = 0; - double parentJointPrevPosAxis2 = 0; - for (int _ = 0; _ < 10; _++) - { - world->Step(output, state, input); - EXPECT_NE(parentJointPrevPosAxis1, childJoint->GetPosition(0)); - EXPECT_NE(parentJointPrevPosAxis2, childJoint->GetPosition(1)); - parentJointPrevPosAxis1 = parentJoint->GetPosition(0); - parentJointPrevPosAxis2 = parentJoint->GetPosition(1); - } - - auto testMimicFcn = [&](double multiplier, double offset, double reference) - { - // Set mimic joint constraint. - childJoint->SetMimicConstraint("universal_joint_1", multiplier, offset, reference); - // Reset positions and run a few iterations so the positions reach nontrivial values. - parentJoint->SetPosition(0, 0); - parentJoint->SetPosition(1, 0); - childJoint->SetPosition(0, 0); - childJoint->SetPosition(1, 0); - for (int _ = 0; _ < 10; _++) - world->Step(output, state, input); - - // Child joint's position should be equal to that of parent joint in previous timestep. - parentJointPrevPosAxis1 = parentJoint->GetPosition(0); - parentJointPrevPosAxis2 = parentJoint->GetPosition(1); - for (int _ = 0; _ < 10; _++) - { - world->Step(output, state, input); - EXPECT_FLOAT_EQ(multiplier * (parentJointPrevPosAxis1 - reference) + offset, - childJoint->GetPosition(0)); - EXPECT_FLOAT_EQ(multiplier * (parentJointPrevPosAxis2 - reference) + offset, - childJoint->GetPosition(1)); - parentJointPrevPosAxis1 = parentJoint->GetPosition(0); - parentJointPrevPosAxis2 = parentJoint->GetPosition(1); - } - }; - - // Testing with different (multiplier, offset, reference) combinations. - testMimicFcn(1, 0, 0); - testMimicFcn(-1, 0, 0); - testMimicFcn(1, 0.1, 0); - testMimicFcn(1, 0.2, 0.1); - testMimicFcn(-1, 0.2, 0); - testMimicFcn(-2, 0, 0); - testMimicFcn(2, 0.1, 0); - testMimicFcn(2, 0.3, -0.1); - } -} - -// In this test, we have 2 pendulums of different lengths. -// Originally, their time periods are different, but after applying -// the mimic constraint, they follow the same velocity, effectively -// violating some laws. (Work done = Change in kinetic energy, for instance) -TYPED_TEST(JointMimicFeatureFixture, PendulumMimicTest) -{ - for (const std::string &name : this->pluginNames) - { - if(this->PhysicsEngineName(name) != "dartsim") - { - GTEST_SKIP(); - } - - std::cout << "Testing plugin: " << name << std::endl; - gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); - - auto engine = gz::physics::RequestEngine3d::From(plugin); - ASSERT_NE(nullptr, engine); - - sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, - "mimic_pendulum_world.sdf")); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = engine->ConstructWorld(*root.WorldByIndex(0)); - - // Test mimic constraint between two revolute joints. - auto model = world->GetModel("pendulum_with_base"); - auto parentJoint = model->GetJoint("upper_joint_1"); - auto childJoint = model->GetJoint("upper_joint_2"); - - // Ensure both joints start from zero angle. - EXPECT_EQ(parentJoint->GetPosition(0), 0); - EXPECT_EQ(childJoint->GetPosition(0), 0); - - gz::physics::ForwardStep::Output output; - gz::physics::ForwardStep::State state; - gz::physics::ForwardStep::Input input; - - // Case : Without mimic constraint - - // Let the simulation run without mimic constraint. - // The positions of joints should not be equal. - double parentJointPrevPosAxis = 0; - for (int _ = 0; _ < 10; _++) - { - world->Step(output, state, input); - EXPECT_NE(parentJointPrevPosAxis, childJoint->GetPosition(0)); - parentJointPrevPosAxis = parentJoint->GetPosition(0); - } - - auto testMimicFcn = [&](double multiplier, double offset, double reference) - { - // Set mimic joint constraint. - childJoint->SetMimicConstraint("upper_joint_1", multiplier, offset, reference); - // Reset positions and run a few iterations so the positions reach nontrivial values. - parentJoint->SetPosition(0, 0); - childJoint->SetPosition(0, 0); - for (int _ = 0; _ < 10; _++) - world->Step(output, state, input); - - // Child joint's position should be equal to that of parent joint in previous timestep. - parentJointPrevPosAxis = parentJoint->GetPosition(0); - for (int _ = 0; _ < 10; _++) - { - world->Step(output, state, input); - EXPECT_FLOAT_EQ(multiplier * (parentJointPrevPosAxis - reference) + offset, - childJoint->GetPosition(0)); - parentJointPrevPosAxis = parentJoint->GetPosition(0); - } - }; - - // Testing with different (multiplier, offset, reference) combinations. - testMimicFcn(1, 0, 0); - testMimicFcn(-1, 0, 0); - testMimicFcn(1, 0.1, 0); - testMimicFcn(1, 0.2, 0.1); - testMimicFcn(-1, 0.2, 0); - testMimicFcn(-2, 0, 0); - testMimicFcn(2, 0.1, 0); - testMimicFcn(2, 0.3, -0.1); - } -} - int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/common_test/joint_mimic_features.cc b/test/common_test/joint_mimic_features.cc new file mode 100644 index 000000000..b5eb6cdce --- /dev/null +++ b/test/common_test/joint_mimic_features.cc @@ -0,0 +1,420 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include + +#include + +#include +#include + +#include +#include + +#include "TestLibLoader.hh" +#include "../Utils.hh" + +#include "gz/physics/FrameSemantics.hh" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +template +class JointMimicFeaturesTest: + public testing::Test, public gz::physics::TestLibLoader +{ + // Documentation inherited + public: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + + std::cerr << "JointMimicFeaturesTest::GetLibToTest() " + << JointMimicFeaturesTest::GetLibToTest() << '\n'; + + loader.LoadLib(JointMimicFeaturesTest::GetLibToTest()); + + // TODO(ahcorde): We should also run the 3f, 2d, and 2f variants of + // FindFeatures + pluginNames = gz::physics::FindFeatures3d::From(loader); + if (pluginNames.empty()) + { + std::cerr << "No plugins with required features found in " + << GetLibToTest() << std::endl; + GTEST_SKIP(); + } + for (const std::string &name : this->pluginNames) + { + if(this->PhysicsEngineName(name) == "tpe") + { + GTEST_SKIP(); + } + } + } + + public: std::set pluginNames; + public: gz::plugin::Loader loader; +}; + +struct JointMimicFeatureList : gz::physics::FeatureList< + gz::physics::ForwardStep, + gz::physics::GetBasicJointProperties, + gz::physics::GetBasicJointState, + gz::physics::GetEngineInfo, + gz::physics::GetJointFromModel, + gz::physics::GetLinkFromModel, + gz::physics::GetModelFromWorld, + gz::physics::LinkFrameSemantics, + gz::physics::SetBasicJointState, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::SetMimicConstraintFeature, + gz::physics::sdf::ConstructSdfWorld>{}; + +template +class JointMimicFeatureFixture : + public JointFeaturesTest{}; +using JointMimicFeatureTest = + JointMimicFeatureFixture; + +// Here, we test mimic constraints on various combinations of +// prismatic and revolute joints using a chain of constraints. +TEST_F(JointMimicFeatureTest, PrismaticRevoluteMimicTest) +{ + // This test contains 5 joints : 3 prismatic and 2 revolute. + // They are connected as follows : + // prismatic_joint_1 : Free joint + // prismatic_joint_2 : Mimics prismatic_joint_1 + // revolute_joint_1 : Mimics prismatic_joint_1 + // revolute_joint_2 : Mimics revolute_joint_1 + // prismatic_joint_3 : Mimics revolute_joint_1 + for (const std::string &name : this->pluginNames) + { + if(this->PhysicsEngineName(name) != "dartsim") + { + GTEST_SKIP(); + } + + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, + "mimic_prismatic_world.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + auto model = world->GetModel("prismatic_model"); + + auto parentJoint = model->GetJoint("prismatic_joint_1"); + auto prismaticChildJoint1 = model->GetJoint("prismatic_joint_2"); + auto revoluteChildJoint1 = model->GetJoint("revolute_joint_1"); + auto revoluteChildJoint2 = model->GetJoint("revolute_joint_2"); + auto prismaticChildJoint2 = model->GetJoint("prismatic_joint_3"); + + // Ensure both joints start from zero angle. + EXPECT_EQ(parentJoint->GetPosition(0), 0); + EXPECT_EQ(prismaticChildJoint1->GetPosition(0), 0); + EXPECT_EQ(revoluteChildJoint1->GetPosition(0), 0); + EXPECT_EQ(revoluteChildJoint2->GetPosition(0), 0); + EXPECT_EQ(prismaticChildJoint2->GetPosition(0), 0); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + // Case : Without mimic constraint + + // Let the simulation run without mimic constraint. + // The positions of joints should not be equal. + double prismaticJointPrevPos = 0; + double revoluteJointPrevPos = 0; + for (int i = 0; i < 10; i++) + { + world->Step(output, state, input); + if (i > 5) + { + EXPECT_NE(prismaticJointPrevPos, prismaticChildJoint1->GetPosition(0)); + EXPECT_NE(prismaticJointPrevPos, revoluteChildJoint1->GetPosition(0)); + EXPECT_NE(revoluteJointPrevPos, revoluteChildJoint2->GetPosition(0)); + EXPECT_NE(revoluteJointPrevPos, prismaticChildJoint2->GetPosition(0)); + } + + // Update previous positions. + prismaticJointPrevPos = parentJoint->GetPosition(0); + revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0); + } + + auto testMimicFcn = [&](double multiplier, double offset, double reference) + { + // Set mimic joint constraints. + // Parent --> Child + // prismatic_joint_1 -> prismatic_joint_2 + // prismatic_joint_1 -> revolute_joint_1 + // revolute_joint_1 --> revolute_joint_2 + // revolute_joint_1 --> prismatic_joint_3 + prismaticChildJoint1->SetMimicConstraint("prismatic_joint_1", multiplier, + offset, reference); + revoluteChildJoint1->SetMimicConstraint("prismatic_joint_1", multiplier, + offset, reference); + revoluteChildJoint2->SetMimicConstraint("revolute_joint_1", multiplier, + offset, reference); + prismaticChildJoint2->SetMimicConstraint("revolute_joint_1", multiplier, + offset, reference); + + // Reset positions and run a few iterations so the positions reach nontrivial values. + parentJoint->SetPosition(0, 0); + prismaticChildJoint1->SetPosition(0, 0); + prismaticChildJoint2->SetPosition(0, 0); + revoluteChildJoint1->SetPosition(0, 0); + revoluteChildJoint2->SetPosition(0, 0); + for (int _ = 0; _ < 10; _++) + world->Step(output, state, input); + + // Child joint's position should be equal to that of parent joint in previous timestep, + // considering the offsets and multipliers. + prismaticJointPrevPos = parentJoint->GetPosition(0); + revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0); + for (int _ = 0; _ < 10; _++) + { + world->Step(output, state, input); + // Check for prismatic -> prismatic mimicking. + EXPECT_FLOAT_EQ(multiplier * (prismaticJointPrevPos - reference) + offset, + prismaticChildJoint1->GetPosition(0)); + // Check for prismatic -> revolute mimicking. + EXPECT_FLOAT_EQ(multiplier * (prismaticJointPrevPos - reference) + offset, + revoluteChildJoint1->GetPosition(0)); + // Check for revolute -> revolute mimicking. + EXPECT_FLOAT_EQ(multiplier * (revoluteJointPrevPos - reference) + offset, + revoluteChildJoint2->GetPosition(0)); + // Check for revolute --> prismatic mimicking. + EXPECT_FLOAT_EQ(multiplier * (revoluteJointPrevPos - reference) + offset, + prismaticChildJoint2->GetPosition(0)); + + // Update previous positions. + prismaticJointPrevPos = parentJoint->GetPosition(0); + revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0); + } + }; + + // Testing with different (multiplier, offset, reference) combinations. + testMimicFcn(1, 0, 0); + testMimicFcn(-1, 0, 0); + testMimicFcn(1, 0.1, 0); + testMimicFcn(1, 0.05, 0.05); + testMimicFcn(-1, 0.2, 0); + testMimicFcn(-1, 0.05, -0.05); + testMimicFcn(-2, 0, 0); + testMimicFcn(2, 0.1, 0); + } +} + +// Here, we test the mimic constraint for a pair of universal joints. +TEST_F(JointMimicFeatureTest, UniversalMimicTest) +{ + for (const std::string &name : this->pluginNames) + { + if(this->PhysicsEngineName(name) != "dartsim") + { + GTEST_SKIP(); + } + + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, + "mimic_universal_world.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + // Test mimic constraint between two revolute joints. + auto model = world->GetModel("universal_model"); + auto parentJoint = model->GetJoint("universal_joint_1"); + auto childJoint = model->GetJoint("universal_joint_2"); + + // Ensure both joints start from zero angle. + EXPECT_EQ(parentJoint->GetPosition(0), 0); + EXPECT_EQ(childJoint->GetPosition(0), 0); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + // Case : Without mimic constraint + + // Let the simulation run without mimic constraint. + // The positions of joints should not be equal. + double parentJointPrevPosAxis1 = 0; + double parentJointPrevPosAxis2 = 0; + for (int _ = 0; _ < 10; _++) + { + world->Step(output, state, input); + EXPECT_NE(parentJointPrevPosAxis1, childJoint->GetPosition(0)); + EXPECT_NE(parentJointPrevPosAxis2, childJoint->GetPosition(1)); + parentJointPrevPosAxis1 = parentJoint->GetPosition(0); + parentJointPrevPosAxis2 = parentJoint->GetPosition(1); + } + + auto testMimicFcn = [&](double multiplier, double offset, double reference) + { + // Set mimic joint constraint. + childJoint->SetMimicConstraint("universal_joint_1", multiplier, offset, reference); + // Reset positions and run a few iterations so the positions reach nontrivial values. + parentJoint->SetPosition(0, 0); + parentJoint->SetPosition(1, 0); + childJoint->SetPosition(0, 0); + childJoint->SetPosition(1, 0); + for (int _ = 0; _ < 10; _++) + world->Step(output, state, input); + + // Child joint's position should be equal to that of parent joint in previous timestep. + parentJointPrevPosAxis1 = parentJoint->GetPosition(0); + parentJointPrevPosAxis2 = parentJoint->GetPosition(1); + for (int _ = 0; _ < 10; _++) + { + world->Step(output, state, input); + EXPECT_FLOAT_EQ(multiplier * (parentJointPrevPosAxis1 - reference) + offset, + childJoint->GetPosition(0)); + EXPECT_FLOAT_EQ(multiplier * (parentJointPrevPosAxis2 - reference) + offset, + childJoint->GetPosition(1)); + parentJointPrevPosAxis1 = parentJoint->GetPosition(0); + parentJointPrevPosAxis2 = parentJoint->GetPosition(1); + } + }; + + // Testing with different (multiplier, offset, reference) combinations. + testMimicFcn(1, 0, 0); + testMimicFcn(-1, 0, 0); + testMimicFcn(1, 0.1, 0); + testMimicFcn(1, 0.2, 0.1); + testMimicFcn(-1, 0.2, 0); + testMimicFcn(-2, 0, 0); + testMimicFcn(2, 0.1, 0); + testMimicFcn(2, 0.3, -0.1); + } +} + +// In this test, we have 2 pendulums of different lengths. +// Originally, their time periods are different, but after applying +// the mimic constraint, they follow the same velocity, effectively +// violating some laws. (Work done = Change in kinetic energy, for instance) +TEST_F(JointMimicFeatureTest, PendulumMimicTest) +{ + for (const std::string &name : this->pluginNames) + { + if(this->PhysicsEngineName(name) != "dartsim") + { + GTEST_SKIP(); + } + + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, + "mimic_pendulum_world.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + // Test mimic constraint between two revolute joints. + auto model = world->GetModel("pendulum_with_base"); + auto parentJoint = model->GetJoint("upper_joint_1"); + auto childJoint = model->GetJoint("upper_joint_2"); + + // Ensure both joints start from zero angle. + EXPECT_EQ(parentJoint->GetPosition(0), 0); + EXPECT_EQ(childJoint->GetPosition(0), 0); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + // Case : Without mimic constraint + + // Let the simulation run without mimic constraint. + // The positions of joints should not be equal. + double parentJointPrevPosAxis = 0; + for (int _ = 0; _ < 10; _++) + { + world->Step(output, state, input); + EXPECT_NE(parentJointPrevPosAxis, childJoint->GetPosition(0)); + parentJointPrevPosAxis = parentJoint->GetPosition(0); + } + + auto testMimicFcn = [&](double multiplier, double offset, double reference) + { + // Set mimic joint constraint. + childJoint->SetMimicConstraint("upper_joint_1", multiplier, offset, reference); + // Reset positions and run a few iterations so the positions reach nontrivial values. + parentJoint->SetPosition(0, 0); + childJoint->SetPosition(0, 0); + for (int _ = 0; _ < 10; _++) + world->Step(output, state, input); + + // Child joint's position should be equal to that of parent joint in previous timestep. + parentJointPrevPosAxis = parentJoint->GetPosition(0); + for (int _ = 0; _ < 10; _++) + { + world->Step(output, state, input); + EXPECT_FLOAT_EQ(multiplier * (parentJointPrevPosAxis - reference) + offset, + childJoint->GetPosition(0)); + parentJointPrevPosAxis = parentJoint->GetPosition(0); + } + }; + + // Testing with different (multiplier, offset, reference) combinations. + testMimicFcn(1, 0, 0); + testMimicFcn(-1, 0, 0); + testMimicFcn(1, 0.1, 0); + testMimicFcn(1, 0.2, 0.1); + testMimicFcn(-1, 0.2, 0); + testMimicFcn(-2, 0, 0); + testMimicFcn(2, 0.1, 0); + testMimicFcn(2, 0.3, -0.1); + } +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + if (!JointMimicFeaturesTest< + JointMimicFeatureList>::init(argc, argv)) + return -1; + return RUN_ALL_TESTS(); +} diff --git a/test/common_test/joint_transmitted_wrench_features.cc b/test/common_test/joint_transmitted_wrench_features.cc new file mode 100644 index 000000000..563b2569a --- /dev/null +++ b/test/common_test/joint_transmitted_wrench_features.cc @@ -0,0 +1,442 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include + +#include + +#include +#include + +#include +#include + +#include "TestLibLoader.hh" +#include "../Utils.hh" + +#include "gz/physics/FrameSemantics.hh" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +template +class JointTransmittedWrenchFeaturesTest: + public testing::Test, public gz::physics::TestLibLoader +{ + // Documentation inherited + public: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + + std::cerr << "JointTransmittedWrenchFeaturesTest::GetLibToTest() " + << JointTransmittedWrenchFeaturesTest::GetLibToTest() << '\n'; + + loader.LoadLib(JointTransmittedWrenchFeaturesTest::GetLibToTest()); + + // TODO(ahcorde): We should also run the 3f, 2d, and 2f variants of + // FindFeatures + pluginNames = gz::physics::FindFeatures3d::From(loader); + if (pluginNames.empty()) + { + std::cerr << "No plugins with required features found in " + << GetLibToTest() << std::endl; + GTEST_SKIP(); + } + for (const std::string &name : this->pluginNames) + { + if(this->PhysicsEngineName(name) == "tpe") + { + GTEST_SKIP(); + } + } + } + + public: std::set pluginNames; + public: gz::plugin::Loader loader; +}; + +template +class JointTransmittedWrenchFixture : + public JointTransmittedWrenchFeaturesTest +{ + public: using WorldPtr = gz::physics::World3dPtr; + public: using ModelPtr = gz::physics::Model3dPtr; + public: using JointPtr = gz::physics::Joint3dPtr; + public: using LinkPtr = gz::physics::Link3dPtr; + + protected: void SetUp() override + { + JointTransmittedWrenchFeaturesTest::SetUp(); + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "pendulum_joint_wrench.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + this->world = engine->ConstructWorld(*root.WorldByIndex(0)); + ASSERT_NE(nullptr, this->world); + + this->model = this->world->GetModel("pendulum"); + ASSERT_NE(nullptr, this->model); + this->motorJoint = this->model->GetJoint("motor_joint"); + ASSERT_NE(nullptr, this->motorJoint); + this->sensorJoint = this->model->GetJoint("sensor_joint"); + ASSERT_NE(nullptr, this->sensorJoint); + this->armLink = this->model->GetLink("arm"); + ASSERT_NE(nullptr, this->armLink); + } + } + + public: void Step(int _iters) + { + for (int i = 0; i < _iters; ++i) + { + this->world->Step(this->output, this->state, this->input); + } + } + + public: gz::physics::ForwardStep::Output output; + public: gz::physics::ForwardStep::State state; + public: gz::physics::ForwardStep::Input input; + public: WorldPtr world; + public: ModelPtr model; + public: JointPtr motorJoint; + public: JointPtr sensorJoint; + public: LinkPtr armLink; + + // From SDFormat file + static constexpr double kGravity = 9.8; + static constexpr double kArmLinkMass = 6.0; + static constexpr double kSensorLinkMass = 0.4; + // MOI in the z-axis + static constexpr double kSensorLinkMOI = 0.02; + static constexpr double kArmLength = 1.0; +}; + +struct JointTransmittedWrenchFeatureList : gz::physics::FeatureList< + gz::physics::ForwardStep, + gz::physics::FreeGroupFrameSemantics, + gz::physics::GetBasicJointState, + gz::physics::GetEngineInfo, + gz::physics::GetJointFromModel, + gz::physics::GetJointTransmittedWrench, + gz::physics::GetLinkFromModel, + gz::physics::GetModelFromWorld, + gz::physics::LinkFrameSemantics, + gz::physics::SetBasicJointState, + gz::physics::SetFreeGroupWorldPose, + gz::physics::sdf::ConstructSdfWorld +> { }; + +using JointTransmittedWrenchFeaturesTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(JointTransmittedWrenchFixture, + JointTransmittedWrenchFeaturesTestTypes); + +TYPED_TEST(JointTransmittedWrenchFixture, PendulumAtZeroAngle) +{ + // Run a few steps for the constraint forces to stabilize + this->Step(10); + + // Test wrench expressed in different frames + { + auto wrenchAtMotorJoint = this->motorJoint->GetTransmittedWrench(); + gz::physics::Wrench3d expectedWrenchAtMotorJoint{ + gz::physics::Vector3d::Zero(), + {-this->kGravity * (this->kArmLinkMass + this->kSensorLinkMass), 0, 0}}; + + EXPECT_TRUE( + gz::physics::test::Equal(expectedWrenchAtMotorJoint, wrenchAtMotorJoint, 1e-4)); + } + { + auto wrenchAtMotorJointInWorld = this->motorJoint->GetTransmittedWrench( + this->motorJoint->GetFrameID(), gz::physics::FrameID::World()); + gz::physics::Wrench3d expectedWrenchAtMotorJointInWorld{ + gz::physics::Vector3d::Zero(), + {0, 0, this->kGravity * (this->kArmLinkMass + this->kSensorLinkMass)}}; + + EXPECT_TRUE(gz::physics::test::Equal(expectedWrenchAtMotorJointInWorld, + wrenchAtMotorJointInWorld, 1e-4)); + } + { + auto wrenchAtMotorJointInArm = this->motorJoint->GetTransmittedWrench( + this->armLink->GetFrameID(), this->armLink->GetFrameID()); + // The arm frame is rotated by 90° in the Y-axis of the joint frame. + gz::physics::Wrench3d expectedWrenchAtMotorJointInArm{ + gz::physics::Vector3d::Zero(), + {0, 0, this->kGravity * (this->kArmLinkMass + this->kSensorLinkMass)}}; + + EXPECT_TRUE(gz::physics::test::Equal(expectedWrenchAtMotorJointInArm, + wrenchAtMotorJointInArm, 1e-4)); + } +} + +TYPED_TEST(JointTransmittedWrenchFixture, PendulumInMotion) +{ + // Start pendulum at 90° (parallel to the ground) and stop at about 40° + // so that we have non-trivial test expectations. + this->motorJoint->SetPosition(0, GZ_DTOR(90.0)); + this->Step(350); + + // Given the position (θ), velocity (ω), and acceleration (α) of the joint + // and distance from the joint to the COM (r), the reaction forces in + // the tangent direction (Ft) and normal direction (Fn) are given by: + // + // Ft = m * α * r + (m * g * sin(θ)) = m * (α * r + g * sin(θ)) + // Fn = -m * ω² * r - (m * g * cos(θ)) = -m * (ω² * r + g * cos(θ)) + { + const double theta = this->motorJoint->GetPosition(0); + const double omega = this->motorJoint->GetVelocity(0); + // In order to get the math to work out, we need to use the joint + // acceleration and transmitted wrench from the current time step with the + // joint position and velocity from the previous time step. That is, we need + // the position and velocity before they are integrated. + this->Step(1); + const double alpha = this->motorJoint->GetAcceleration(0); + + auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); + + const double armTangentForce = + this->kArmLinkMass * ((alpha * this->kArmLength / 2.0) + (this->kGravity * sin(theta))); + + const double motorLinkTangentForce = + this->kSensorLinkMass * this->kGravity * sin(theta); + + const double armNormalForce = + -this->kArmLinkMass * + ((std::pow(omega, 2) * this->kArmLength / 2.0) + (this->kGravity * cos(theta))); + + const double motorLinkNormalForce = + -this->kSensorLinkMass * this->kGravity * cos(theta); + + const double tangentForce = armTangentForce + motorLinkTangentForce; + const double normalForce = armNormalForce + motorLinkNormalForce; + + // The orientation of the joint frame is such that the normal force is + // parallel to the x-axis and the tangent force is parallel to the y-axis. + gz::physics::Wrench3d expectedWrenchAtMotorJointInJoint{ + gz::physics::Vector3d::Zero(), {normalForce, tangentForce, 0}}; + + EXPECT_TRUE(gz::physics::test::Equal(expectedWrenchAtMotorJointInJoint, + wrenchAtMotorJointInJoint, 1e-4)); + } + + // Test Wrench expressed in different frames + { + auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); + // This is just a rotation of the wrench to be expressed in the world's + // coordinate frame + auto wrenchAtMotorJointInWorld = this->motorJoint->GetTransmittedWrench( + this->motorJoint->GetFrameID(), gz::physics::FrameID::World()); + // The joint frame is rotated by 90° along the world's y-axis + Eigen::Quaterniond R_WJ = + Eigen::AngleAxisd(GZ_PI_2, Eigen::Vector3d(0, 1, 0)) * + Eigen::AngleAxisd(this->motorJoint->GetPosition(0), + Eigen::Vector3d(0, 0, 1)); + + gz::physics::Wrench3d expectedWrenchAtMotorJointInWorld{ + gz::physics::Vector3d::Zero(), R_WJ * wrenchAtMotorJointInJoint.force}; + EXPECT_TRUE(gz::physics::test::Equal(expectedWrenchAtMotorJointInWorld, + wrenchAtMotorJointInWorld, 1e-4)); + + // This moves the point of application and changes the coordinate frame + gz::physics::Wrench3d wrenchAtArmInArm = this->motorJoint->GetTransmittedWrench( + this->armLink->GetFrameID(), this->armLink->GetFrameID()); + + // Notation: arm link (A), joint (J) + Eigen::Isometry3d X_AJ; + // Pose of joint (J) in arm link (A) as specified in the SDFormat file. + X_AJ = Eigen::AngleAxisd(GZ_PI_2, Eigen::Vector3d(0, 1, 0)); + X_AJ.translation() = gz::physics::Vector3d(0, 0, this->kArmLength / 2.0); + gz::physics::Wrench3d expectedWrenchAtArmInArm; + + expectedWrenchAtArmInArm.force = + X_AJ.linear() * wrenchAtMotorJointInJoint.force; + + expectedWrenchAtArmInArm.torque = + X_AJ.linear() * wrenchAtMotorJointInJoint.torque + + X_AJ.translation().cross(expectedWrenchAtArmInArm.force); + + EXPECT_TRUE(gz::physics::test::Equal(expectedWrenchAtArmInArm, wrenchAtArmInArm, 1e-4)); + } +} + +TYPED_TEST(JointTransmittedWrenchFixture, ValidateWrenchWithSecondaryJoint) +{ + // Start pendulum at 90° (parallel to the ground) and stop at about 40° + // so that we have non-trivial test expectations. + this->motorJoint->SetPosition(0, GZ_DTOR(90.0)); + this->Step(350); + const double theta = this->motorJoint->GetPosition(0); + // In order to get the math to work out, we need to use the joint + // acceleration and transmitted wrench from the current time step with the + // joint position and velocity from the previous time step. That is, we need + // the position and velocity before they are integrated. + this->Step(1); + const double alpha = this->motorJoint->GetAcceleration(0); + + auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); + auto wrenchAtSensorInSensor = this->sensorJoint->GetTransmittedWrench(); + + // Since sensor_link has moment of inertia, the fixed joint will transmit a + // torque necessary to rotate the sensor. This is not detected by the motor + // joint because no force is transmitted along the revolute axis. On the + // other hand, the mass of sensor_link will contribute to the constraint + // forces on the motor joint, but these won't be detected by the sensor + // joint. + gz::physics::Vector3d expectedTorqueDiff{0, 0, this->kSensorLinkMOI * alpha}; + gz::physics::Vector3d expectedForceDiff{-this->kSensorLinkMass * this->kGravity * cos(theta), + this->kSensorLinkMass * this->kGravity * sin(theta), 0}; + + gz::physics::Vector3d torqueDiff = + wrenchAtMotorJointInJoint.torque - wrenchAtSensorInSensor.torque; + gz::physics::Vector3d forceDiff = + wrenchAtMotorJointInJoint.force - wrenchAtSensorInSensor.force; + EXPECT_TRUE(gz::physics::test::Equal(expectedTorqueDiff, torqueDiff, 1e-4)); + EXPECT_TRUE(gz::physics::test::Equal(expectedForceDiff, forceDiff, 1e-4)); +} + +TYPED_TEST(JointTransmittedWrenchFixture, JointLosses) +{ + // // Get DART joint pointer to set joint friction, damping, etc. + // auto dartWorld = this->world->GetDartsimWorld(); + // ASSERT_NE(nullptr, dartWorld); + // auto dartModel = dartWorld->getSkeleton(this->model->GetIndex()); + // ASSERT_NE(nullptr, dartModel); + // auto dartJoint = dartModel->getJoint(this->motorJoint->GetIndex()); + // ASSERT_NE(nullptr, dartJoint); + // + // // Joint friction + // { + // this->motorJoint->SetPosition(0, GZ_DTOR(90.0)); + // this->motorJoint->SetVelocity(0, 0); + // const double kFrictionCoef = 0.5; + // dartJoint->setCoulombFriction(0, kFrictionCoef); + // this->Step(10); + // auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); + // EXPECT_NEAR(kFrictionCoef, wrenchAtMotorJointInJoint.torque.z(), 1e-4); + // dartJoint->setCoulombFriction(0, 0.0); + // } + // + // // Joint damping + // { + // this->motorJoint->SetPosition(0, GZ_DTOR(90.0)); + // this->motorJoint->SetVelocity(0, 0); + // const double kDampingCoef = 0.2; + // dartJoint->setDampingCoefficient(0, kDampingCoef); + // this->Step(100); + // const double omega = this->motorJoint->GetVelocity(0); + // this->Step(1); + // auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); + // EXPECT_NEAR(-omega * kDampingCoef, wrenchAtMotorJointInJoint.torque.z(), + // 1e-3); + // dartJoint->setDampingCoefficient(0, 0.0); + // } + // + // // Joint stiffness + // { + // // Note: By default, the spring reference position is 0. + // this->motorJoint->SetPosition(0, GZ_DTOR(30.0)); + // this->motorJoint->SetVelocity(0, 0); + // const double kSpringStiffness = 0.7; + // dartJoint->setSpringStiffness(0, kSpringStiffness); + // this->Step(1); + // const double theta = this->motorJoint->GetPosition(0); + // this->Step(1); + // auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); + // EXPECT_NEAR(-theta * kSpringStiffness, wrenchAtMotorJointInJoint.torque.z(), + // 1e-3); + // dartJoint->setSpringStiffness(0, 0.0); + // } +} + +// Check that the transmitted wrench is affected by contact forces +TYPED_TEST(JointTransmittedWrenchFixture, ContactForces) +{ + auto box = this->world->GetModel("box"); + ASSERT_NE(nullptr, box); + auto boxFreeGroup = box->FindFreeGroup(); + ASSERT_NE(nullptr, boxFreeGroup); + gz::physics::Pose3d X_WB(Eigen::Translation3d(0, 1, 1)); + boxFreeGroup->SetWorldPose(X_WB); + + this->motorJoint->SetPosition(0, GZ_DTOR(90.0)); + // After this many steps, the pendulum is in contact with the box + this->Step(1000); + const double theta = this->motorJoint->GetPosition(0); + // Sanity check that the pendulum is at rest + EXPECT_NEAR(0.0, this->motorJoint->GetVelocity(0), 1e-3); + + auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); + + // To compute the reaction forces, we consider the pivot on the contact point + // between the pendulum and the box and the fact that the sum of moments about + // the pivot is zero. We also note that all forces, including the reaction + // forces, are in the vertical (world's z-axis) direction. + // + // Notation: + // Fp_z: Reaction force at pendulum joint (pin) in the world's z-axis + // M_b: Moment about the contact point between box and pendulum + // + // Fp_z = √(Fn² + Ft²) // Since all of the reaction force is in the world's + // z-axis + // + // ∑M_b = 0 = -Fp_z * sin(θ) * (2*r) + m₁*g*sin(θ)*r + m₂*g*sin(θ)*(2*r) + // + // Fp_z = 0.5 * g * (m₁ + 2*m₂) + // + // We can then compute the tangential (Ft) and normal (Fn) components as + // + // Ft = Fp_z * sin(θ) + // Fn = -Fp_z * cos(θ) + + const double reactionForceAtP = + 0.5 * this->kGravity * (this->kArmLinkMass + 2 * this->kSensorLinkMass); + + gz::physics::Wrench3d expectedWrenchAtMotorJointInJoint{ + gz::physics::Vector3d::Zero(), + {-reactionForceAtP * cos(theta), reactionForceAtP * sin(theta), 0}}; + + EXPECT_TRUE(gz::physics::test::Equal(expectedWrenchAtMotorJointInJoint, + wrenchAtMotorJointInJoint, 1e-4)); +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + if (!JointTransmittedWrenchFeaturesTest< + JointTransmittedWrenchFeatureList>::init(argc, argv)) + return -1; + return RUN_ALL_TESTS(); +} diff --git a/test/common_test/link_features.cc b/test/common_test/link_features.cc index 2da18d74b..8b36b6424 100644 --- a/test/common_test/link_features.cc +++ b/test/common_test/link_features.cc @@ -75,28 +75,7 @@ class LinkFeaturesTest: public: gz::plugin::Loader loader; }; -// A predicate-formatter for asserting that two vectors are approximately equal. -class AssertVectorApprox -{ - public: explicit AssertVectorApprox(double _tol = 1e-6) : tol(_tol) - { - } - - public: ::testing::AssertionResult operator()( - const char *_mExpr, const char *_nExpr, Eigen::Vector3d _m, - Eigen::Vector3d _n) - { - if (gz::physics::test::Equal(_m, _n, this->tol)) - return ::testing::AssertionSuccess(); - - return ::testing::AssertionFailure() - << _mExpr << " and " << _nExpr << " ([" << _m.transpose() - << "] and [" << _n.transpose() << "]" - << ") are not equal"; - } - - private: double tol; -}; +using AssertVectorApprox = gz::physics::test::AssertVectorApprox; struct LinkFeaturesList : gz::physics::FeatureList< gz::physics::AddLinkExternalForceTorque, diff --git a/test/common_test/shape_features.cc b/test/common_test/shape_features.cc index 4f1f9ff60..c2de4cf96 100644 --- a/test/common_test/shape_features.cc +++ b/test/common_test/shape_features.cc @@ -66,28 +66,7 @@ class ShapeFeaturesTest: public: gz::plugin::Loader loader; }; -// A predicate-formatter for asserting that two vectors are approximately equal. -class AssertVectorApprox -{ - public: explicit AssertVectorApprox(double _tol = 1e-6) : tol(_tol) - { - } - - public: ::testing::AssertionResult operator()( - const char *_mExpr, const char *_nExpr, Eigen::Vector3d _m, - Eigen::Vector3d _n) - { - if (gz::physics::test::Equal(_m, _n, this->tol)) - return ::testing::AssertionSuccess(); - - return ::testing::AssertionFailure() - << _mExpr << " and " << _nExpr << " ([" << _m.transpose() - << "] and [" << _n.transpose() << "]" - << ") are not equal"; - } - - private: double tol; -}; +using AssertVectorApprox = gz::physics::test::AssertVectorApprox; struct ShapeFeaturesList : gz::physics::FeatureList< gz::physics::AttachFixedJointFeature, diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 8e291efd3..1087e2d5c 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -117,32 +117,26 @@ class SimulationFeaturesTest: }; template -std::unordered_set> LoadWorlds( +gz::physics::World3dPtr LoadPluginAndWorld( const gz::plugin::Loader &_loader, - const std::set pluginNames, + const std::string &_pluginName, const std::string &_world) { - std::unordered_set> worlds; - for (const std::string &name : pluginNames) - { - gz::plugin::PluginPtr plugin = _loader.Instantiate(name); - - gzdbg << " -- Plugin name: " << name << std::endl; + gz::plugin::PluginPtr plugin = _loader.Instantiate(_pluginName); - auto engine = - gz::physics::RequestEngine3d::From(plugin); - EXPECT_NE(nullptr, engine); + gzdbg << " -- Plugin name: " << _pluginName << std::endl; - sdf::Root root; - const sdf::Errors &errors = root.Load(_world); - EXPECT_EQ(0u, errors.size()); - const sdf::World *sdfWorld = root.WorldByIndex(0); - auto world = engine->ConstructWorld(*sdfWorld); - EXPECT_NE(nullptr, world); + auto engine = + gz::physics::RequestEngine3d::From(plugin); + EXPECT_NE(nullptr, engine); - worlds.insert(world); - } - return worlds; + sdf::Root root; + const sdf::Errors &errors = root.Load(_world); + EXPECT_EQ(0u, errors.size()); + const sdf::World *sdfWorld = root.WorldByIndex(0); + auto world = engine->ConstructWorld(*sdfWorld); + EXPECT_NE(nullptr, world); + return world; } /// \brief Step forward in a world @@ -207,12 +201,12 @@ TYPED_TEST_SUITE(SimulationFeaturesContactsTest, ///////////////////////////////////////////////// TYPED_TEST(SimulationFeaturesContactsTest, Contacts) { - auto worlds = LoadWorlds( - this->loader, - this->pluginNames, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); - for (const auto &world : worlds) + for (const std::string &name : this->pluginNames) { + auto world = LoadPluginAndWorld( + this->loader, + name, + gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); auto checkedOutput = StepWorld(world, true, 1).first; EXPECT_TRUE(checkedOutput); @@ -240,12 +234,16 @@ TYPED_TEST_SUITE(SimulationFeaturesStepTest, ///////////////////////////////////////////////// TYPED_TEST(SimulationFeaturesStepTest, StepWorld) { - auto worlds = LoadWorlds( - this->loader, - this->pluginNames, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); - for (const auto &world : worlds) + for (const std::string &name : this->pluginNames) { +#ifdef _WIN32 + // See https://github.com/gazebosim/gz-physics/issues/483 + CHECK_UNSUPPORTED_ENGINE(name, "bullet") +#endif + auto world = LoadPluginAndWorld( + this->loader, + name, + gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); auto checkedOutput = StepWorld(world, true, 1000).first; EXPECT_TRUE(checkedOutput); } @@ -272,35 +270,34 @@ TYPED_TEST(SimulationFeaturesFallingTest, Falling) { for (const std::string &name : this->pluginNames) { - if(this->PhysicsEngineName(name) == "tpe") - { - GTEST_SKIP(); - } + CHECK_UNSUPPORTED_ENGINE(name, "tpe") - auto worlds = LoadWorlds( - this->loader, - this->pluginNames, - gz::common::joinPaths(TEST_WORLD_DIR, "falling.world")); - for (const auto &world : worlds) - { - auto [checkedOutput, output] = - StepWorld(world, true, 1000); - EXPECT_TRUE(checkedOutput); - - const std::vector worldPoses = - output.template Get().entries; - - ASSERT_GE(worldPoses.size(), 1); - const auto link = world->GetModel(0)->GetLink("sphere_link"); - - // get the pose of the link from the list of worldPoses - auto poseIt = std::find_if(worldPoses.begin(), worldPoses.end(), - [&](const auto &_wPose) - { return _wPose.body == link->EntityID(); }); - ASSERT_NE(poseIt, worldPoses.end()); - auto pos = poseIt->pose.Pos(); - EXPECT_NEAR(pos.Z(), 1.0, 5e-2); - } +#ifdef _WIN32 + // See https://github.com/gazebosim/gz-physics/issues/483 + CHECK_UNSUPPORTED_ENGINE(name, "bullet") +#endif + + auto world = LoadPluginAndWorld( + this->loader, name, + gz::common::joinPaths(TEST_WORLD_DIR, "falling.world")); + + auto [checkedOutput, output] = + StepWorld(world, true, 1000); + EXPECT_TRUE(checkedOutput); + + const std::vector worldPoses = + output.template Get().entries; + + ASSERT_GE(worldPoses.size(), 1); + const auto link = world->GetModel(0)->GetLink("sphere_link"); + + // get the pose of the link from the list of worldPoses + auto poseIt = std::find_if(worldPoses.begin(), worldPoses.end(), + [&](const auto &_wPose) + { return _wPose.body == link->EntityID(); }); + ASSERT_NE(poseIt, worldPoses.end()); + auto pos = poseIt->pose.Pos(); + EXPECT_NEAR(pos.Z(), 1.0, 5e-2); } } @@ -337,12 +334,15 @@ TYPED_TEST_SUITE(SimulationFeaturesShapeFeaturesTest, ///////////////////////////////////////////////// TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) { - auto worlds = LoadWorlds( - this->loader, - this->pluginNames, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); - for (const auto &world : worlds) + for (const std::string &name : this->pluginNames) { +#ifdef _WIN32 + // See https://github.com/gazebosim/gz-physics/issues/483 + CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone") +#endif + auto world = LoadPluginAndWorld( + this->loader, name, + gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); std::cerr << "world model count " << world->GetModelCount() << '\n'; // test ShapeFeatures auto sphere = world->GetModel("sphere"); @@ -357,7 +357,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) EXPECT_EQ(1u, sphereLink->GetShapeCount()); auto sphere2 = sphereLink->AttachSphereShape( - "sphere2", 1.0, Eigen::Isometry3d::Identity()); + "sphere2", 1.0, Eigen::Isometry3d::Identity()); EXPECT_EQ(2u, sphereLink->GetShapeCount()); EXPECT_EQ(sphere2, sphereLink->GetShape(1)); @@ -370,13 +370,13 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) auto boxShape = boxCollision->CastToBoxShape(); EXPECT_NE(nullptr, boxShape); EXPECT_EQ(gz::math::Vector3d(100, 100, 1), - gz::math::eigen3::convert(boxShape->GetSize())); + gz::math::eigen3::convert(boxShape->GetSize())); auto box2 = boxLink->AttachBoxShape( "box2", gz::math::eigen3::convert( gz::math::Vector3d(1.2, 1.2, 1.2)), - Eigen::Isometry3d::Identity()); + Eigen::Isometry3d::Identity()); EXPECT_EQ(2u, boxLink->GetShapeCount()); EXPECT_EQ(box2, boxLink->GetShape(1)); EXPECT_EQ(gz::math::Vector3d(1.2, 1.2, 1.2), @@ -390,15 +390,15 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) EXPECT_NEAR(1.1, cylinderShape->GetHeight(), 1e-6); auto cylinder2 = cylinderLink->AttachCylinderShape( - "cylinder2", 3.0, 4.0, Eigen::Isometry3d::Identity()); + "cylinder2", 3.0, 4.0, Eigen::Isometry3d::Identity()); EXPECT_EQ(2u, cylinderLink->GetShapeCount()); EXPECT_EQ(cylinder2, cylinderLink->GetShape(1)); EXPECT_NEAR(3.0, - cylinderLink->GetShape(1)->CastToCylinderShape()->GetRadius(), - 1e-6); + cylinderLink->GetShape(1)->CastToCylinderShape()->GetRadius(), + 1e-6); EXPECT_NEAR(4.0, - cylinderLink->GetShape(1)->CastToCylinderShape()->GetHeight(), - 1e-6); + cylinderLink->GetShape(1)->CastToCylinderShape()->GetHeight(), + 1e-6); auto ellipsoid = world->GetModel("ellipsoid"); auto ellipsoidLink = ellipsoid->GetLink(0); @@ -409,9 +409,9 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) gz::math::eigen3::convert(ellipsoidShape->GetRadii()), 0.1)); auto ellipsoid2 = ellipsoidLink->AttachEllipsoidShape( - "ellipsoid2", - gz::math::eigen3::convert(gz::math::Vector3d(0.2, 0.3, 0.5)), - Eigen::Isometry3d::Identity()); + "ellipsoid2", + gz::math::eigen3::convert(gz::math::Vector3d(0.2, 0.3, 0.5)), + Eigen::Isometry3d::Identity()); EXPECT_EQ(2u, ellipsoidLink->GetShapeCount()); EXPECT_EQ(ellipsoid2, ellipsoidLink->GetShape(1)); @@ -423,7 +423,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) EXPECT_NEAR(0.6, capsuleShape->GetLength(), 1e-6); auto capsule2 = capsuleLink->AttachCapsuleShape( - "capsule2", 0.2, 0.6, Eigen::Isometry3d::Identity()); + "capsule2", 0.2, 0.6, Eigen::Isometry3d::Identity()); EXPECT_EQ(2u, capsuleLink->GetShapeCount()); EXPECT_EQ(capsule2, capsuleLink->GetShape(1)); @@ -442,15 +442,15 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) EXPECT_TRUE(gz::math::Vector3d(-1, -1, -1).Equal( gz::math::eigen3::convert(sphereAABB).Min(), 0.1)); EXPECT_EQ(gz::math::Vector3d(1, 1, 1), - gz::math::eigen3::convert(sphereAABB).Max()); + gz::math::eigen3::convert(sphereAABB).Max()); EXPECT_EQ(gz::math::Vector3d(-50, -50, -0.5), - gz::math::eigen3::convert(boxAABB).Min()); + gz::math::eigen3::convert(boxAABB).Min()); EXPECT_EQ(gz::math::Vector3d(50, 50, 0.5), - gz::math::eigen3::convert(boxAABB).Max()); + gz::math::eigen3::convert(boxAABB).Max()); EXPECT_EQ(gz::math::Vector3d(-0.5, -0.5, -0.55), - gz::math::eigen3::convert(cylinderAABB).Min()); + gz::math::eigen3::convert(cylinderAABB).Min()); EXPECT_EQ(gz::math::Vector3d(0.5, 0.5, 0.55), - gz::math::eigen3::convert(cylinderAABB).Max()); + gz::math::eigen3::convert(cylinderAABB).Max()); EXPECT_TRUE(gz::math::Vector3d(-0.2, -0.3, -0.5).Equal( gz::math::eigen3::convert(ellipsoidAABB).Min(), 0.1)); EXPECT_TRUE(gz::math::Vector3d(0.2, 0.3, 0.5).Equal( @@ -468,34 +468,25 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) auto capsuleModelAABB = capsule->GetAxisAlignedBoundingBox(); EXPECT_EQ(gz::math::Vector3d(-1, 0.5, -0.5), - gz::math::eigen3::convert(sphereModelAABB).Min()); + gz::math::eigen3::convert(sphereModelAABB).Min()); EXPECT_EQ(gz::math::Vector3d(1, 2.5, 1.5), - gz::math::eigen3::convert(sphereModelAABB).Max()); + gz::math::eigen3::convert(sphereModelAABB).Max()); EXPECT_EQ(gz::math::Vector3d(-50, -50, -0.1), - gz::math::eigen3::convert(boxModelAABB).Min()); + gz::math::eigen3::convert(boxModelAABB).Min()); EXPECT_EQ(gz::math::Vector3d(50, 50, 1.1), - gz::math::eigen3::convert(boxModelAABB).Max()); + gz::math::eigen3::convert(boxModelAABB).Max()); EXPECT_EQ(gz::math::Vector3d(-3, -4.5, -1.5), - gz::math::eigen3::convert(cylinderModelAABB).Min()); + gz::math::eigen3::convert(cylinderModelAABB).Min()); EXPECT_EQ(gz::math::Vector3d(3, 1.5, 2.5), - gz::math::eigen3::convert(cylinderModelAABB).Max()); - /* - /// \TODO(mjcarroll) The ellipsoid bounding box happens to return a - /// FLT_MAX rather than the correct value of 1.2 on the bullet-featherstone - /// implementation on Ubuntu Focal. - /// Since Focal is a lower-priority platform for featherstone and there - /// should be few downstream impacts of this calculation, I am commenting - /// it for now. - /// Tracked in: https://github.com/gazebosim/gz-physics/issues/440 + gz::math::eigen3::convert(cylinderModelAABB).Max()); EXPECT_TRUE(gz::math::Vector3d(-0.2, -5.3, 0.2).Equal( - gz::math::eigen3::convert(ellipsoidModelAABB).Min(), 0.1)); + gz::math::eigen3::convert(ellipsoidModelAABB).Min(), 0.1)); EXPECT_TRUE(gz::math::Vector3d(0.2, -4.7, 1.2).Equal( - gz::math::eigen3::convert(ellipsoidModelAABB).Max(), 0.1)); - */ + gz::math::eigen3::convert(ellipsoidModelAABB).Max(), 0.1)); EXPECT_EQ(gz::math::Vector3d(-0.2, -3.2, 0), - gz::math::eigen3::convert(capsuleModelAABB).Min()); + gz::math::eigen3::convert(capsuleModelAABB).Min()); EXPECT_EQ(gz::math::Vector3d(0.2, -2.8, 1), - gz::math::eigen3::convert(capsuleModelAABB).Max()); + gz::math::eigen3::convert(capsuleModelAABB).Max()); } } @@ -509,13 +500,13 @@ TYPED_TEST_SUITE(SimulationFeaturesTestBasic, TYPED_TEST(SimulationFeaturesTestBasic, FreeGroup) { - auto worlds = LoadWorlds( - this->loader, - this->pluginNames, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); - - for (const auto &world : worlds) + for (const std::string &name : this->pluginNames) { + auto world = LoadPluginAndWorld( + this->loader, + name, + gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); + // model free group test auto model = world->GetModel("sphere"); auto freeGroup = model->FindFreeGroup(); @@ -556,13 +547,12 @@ TYPED_TEST(SimulationFeaturesTestBasic, FreeGroup) TYPED_TEST(SimulationFeaturesTestBasic, ShapeBoundingBox) { - auto worlds = LoadWorlds( - this->loader, - this->pluginNames, - gz::common::joinPaths(TEST_WORLD_DIR, "falling.world")); - - for (const auto &world : worlds) + for (const std::string &name : this->pluginNames) { + auto world = LoadPluginAndWorld( + this->loader, + name, + gz::common::joinPaths(TEST_WORLD_DIR, "falling.world")); auto sphere = world->GetModel("sphere"); auto sphereCollision = sphere->GetLink(0)->GetShape(0); auto ground = world->GetModel("box"); @@ -606,13 +596,13 @@ TYPED_TEST(SimulationFeaturesTestBasic, ShapeBoundingBox) TYPED_TEST(SimulationFeaturesTestBasic, CollideBitmasks) { - auto worlds = LoadWorlds( - this->loader, - this->pluginNames, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes_bitmask.sdf")); - - for (const auto &world : worlds) + for (const std::string &name : this->pluginNames) { + auto world = LoadPluginAndWorld( + this->loader, + name, + gz::common::joinPaths(TEST_WORLD_DIR, "shapes_bitmask.sdf")); + auto baseBox = world->GetModel("box_base"); auto filteredBox = world->GetModel("box_filtered"); auto collidingBox = world->GetModel("box_colliding"); @@ -650,14 +640,14 @@ TYPED_TEST(SimulationFeaturesTestBasic, CollideBitmasks) TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) { - std::unordered_set> worlds = - LoadWorlds( + for (const std::string &name : this->pluginNames) + { + auto world = + LoadPluginAndWorld( this->loader, - this->pluginNames, + name, gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); - for (const auto &world : worlds) - { auto sphere = world->GetModel("sphere"); auto sphereFreeGroup = sphere->FindFreeGroup(); EXPECT_NE(nullptr, sphereFreeGroup); @@ -691,7 +681,8 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) for (auto &contact : contacts) { - const auto &contactPoint = contact.Get::ContactPoint>(); + const auto &contactPoint = + contact.template Get::ContactPoint>(); ASSERT_TRUE(contactPoint.collision1); ASSERT_TRUE(contactPoint.collision2); EXPECT_NE(contactPoint.collision1, contactPoint.collision2); @@ -749,7 +740,8 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) contactBoxEllipsoid = 0u; for (auto contact : contacts) { - const auto &contactPoint = contact.Get::ContactPoint>(); + const auto &contactPoint = + contact.template Get::ContactPoint>(); ASSERT_TRUE(contactPoint.collision1); ASSERT_TRUE(contactPoint.collision2); EXPECT_NE(contactPoint.collision1, contactPoint.collision2); @@ -862,231 +854,266 @@ TYPED_TEST(SimulationFeaturesTestFeaturesContactPropertiesCallback, ContactPrope { for (const std::string &name : this->pluginNames) { - std::unordered_set> worlds = - LoadWorlds( - this->loader, - this->pluginNames, - gz::common::joinPaths(TEST_WORLD_DIR, "contact.sdf")); + auto world = + LoadPluginAndWorld( + this->loader, + name, + gz::common::joinPaths(TEST_WORLD_DIR, "contact.sdf")); - for (const auto &world : worlds) + auto sphere = world->GetModel("sphere"); + auto groundPlane = world->GetModel("ground_plane"); + auto groundPlaneCollision = groundPlane->GetLink(0)->GetShape(0); + + // Use a set because the order of collisions is not determined. + std::set> possibleCollisions = { + groundPlaneCollision, + sphere->GetLink(0)->GetShape(0), + sphere->GetLink(1)->GetShape(0), + sphere->GetLink(2)->GetShape(0), + sphere->GetLink(3)->GetShape(0), + }; + std::map, Eigen::Vector3d> expectations { - auto sphere = world->GetModel("sphere"); - auto groundPlane = world->GetModel("ground_plane"); - auto groundPlaneCollision = groundPlane->GetLink(0)->GetShape(0); - - // Use a set because the order of collisions is not determined. - std::set> possibleCollisions = { - groundPlaneCollision, - sphere->GetLink(0)->GetShape(0), - sphere->GetLink(1)->GetShape(0), - sphere->GetLink(2)->GetShape(0), - sphere->GetLink(3)->GetShape(0), - }; - std::map, Eigen::Vector3d> expectations - { - {sphere->GetLink(0)->GetShape(0), {0.0, 0.0, 0.0}}, + {sphere->GetLink(0)->GetShape(0), {0.0, 0.0, 0.0}}, {sphere->GetLink(1)->GetShape(0), {0.0, 1.0, 0.0}}, {sphere->GetLink(2)->GetShape(0), {1.0, 0.0, 0.0}}, {sphere->GetLink(3)->GetShape(0), {1.0, 1.0, 0.0}}, - }; + }; - const double gravity = 9.8; - std::map, double> forceExpectations - { - // Contact force expectations are: link mass * gravity. - {sphere->GetLink(0)->GetShape(0), 0.1 * gravity}, + const double gravity = 9.8; + std::map, double> forceExpectations + { + // Contact force expectations are: link mass * gravity. + {sphere->GetLink(0)->GetShape(0), 0.1 * gravity}, {sphere->GetLink(1)->GetShape(0), 1.0 * gravity}, {sphere->GetLink(2)->GetShape(0), 2.0 * gravity}, {sphere->GetLink(3)->GetShape(0), 3.0 * gravity}, - }; - - // This procedure checks the validity of a generated contact point. It is - // used both when checking the contacts after the step is finished and for - // checking them inside the contact joint properties callback. The callback - // is called after the contacts are generated but before they affect the - // physics. That is why contact force is zero during the callback. - auto checkContact = [&]( + }; + + // This procedure checks the validity of a generated contact point. It is + // used both when checking the contacts after the step is finished and for + // checking them inside the contact joint properties callback. The callback + // is called after the contacts are generated but before they affect the + // physics. That is why contact force is zero during the callback. + auto checkContact = [&]( const gz::physics::World3d::Contact& _contact, const bool zeroForce) + { + const auto &contactPoint = + _contact.Get::ContactPoint>(); + ASSERT_TRUE(contactPoint.collision1); + ASSERT_TRUE(contactPoint.collision2); + + EXPECT_TRUE(possibleCollisions.find(contactPoint.collision1) != + possibleCollisions.end()); + EXPECT_TRUE(possibleCollisions.find(contactPoint.collision2) != + possibleCollisions.end()); + EXPECT_NE(contactPoint.collision1, contactPoint.collision2); + + Eigen::Vector3d expectedContactPos = Eigen::Vector3d::Zero(); + + // The test expectations are all on the collision that is not the ground + // plane. + auto testCollision = contactPoint.collision1; + if (testCollision == groundPlaneCollision) { - const auto &contactPoint = - _contact.Get::ContactPoint>(); - ASSERT_TRUE(contactPoint.collision1); - ASSERT_TRUE(contactPoint.collision2); - - EXPECT_TRUE(possibleCollisions.find(contactPoint.collision1) != - possibleCollisions.end()); - EXPECT_TRUE(possibleCollisions.find(contactPoint.collision2) != - possibleCollisions.end()); - EXPECT_NE(contactPoint.collision1, contactPoint.collision2); - - Eigen::Vector3d expectedContactPos = Eigen::Vector3d::Zero(); - - // The test expectations are all on the collision that is not the ground - // plane. - auto testCollision = contactPoint.collision1; - if (testCollision == groundPlaneCollision) - { - testCollision = contactPoint.collision2; - } - - expectedContactPos = expectations.at(testCollision); - - EXPECT_TRUE(gz::physics::test::Equal(expectedContactPos, - contactPoint.point, 1e-6)); - - // Check if the engine populated the extra contact data struct - const auto* extraContactData = - _contact.Query::ExtraContactData>(); - ASSERT_NE(nullptr, extraContactData); - - // The normal of the contact force is a vector pointing up (z positive) - EXPECT_NEAR(extraContactData->normal[0], 0.0, 1e-3); - EXPECT_NEAR(extraContactData->normal[1], 0.0, 1e-3); - EXPECT_NEAR(extraContactData->normal[2], 1.0, 1e-3); - - // The contact force has only a z component and its value is - // the the weight of the sphere times the gravitational acceleration - EXPECT_NEAR(extraContactData->force[0], 0.0, 1e-3); - EXPECT_NEAR(extraContactData->force[1], 0.0, 1e-3); - EXPECT_NEAR(extraContactData->force[2], - zeroForce ? 0 : forceExpectations.at(testCollision), 1e-3); - }; + testCollision = contactPoint.collision2; + } - #ifdef DART_HAS_CONTACT_SURFACE - size_t numContactCallbackCalls = 0u; - auto contactCallback = [&]( + expectedContactPos = expectations.at(testCollision); + + EXPECT_TRUE(gz::physics::test::Equal(expectedContactPos, + contactPoint.point, 1e-6)); + + // Check if the engine populated the extra contact data struct + const auto* extraContactData = + _contact.Query::ExtraContactData>(); + ASSERT_NE(nullptr, extraContactData); + + // The normal of the contact force is a vector pointing up (z positive) + EXPECT_NEAR(extraContactData->normal[0], 0.0, 1e-3); + EXPECT_NEAR(extraContactData->normal[1], 0.0, 1e-3); + EXPECT_NEAR(extraContactData->normal[2], 1.0, 1e-3); + + // The contact force has only a z component and its value is + // the the weight of the sphere times the gravitational acceleration + EXPECT_NEAR(extraContactData->force[0], 0.0, 1e-3); + EXPECT_NEAR(extraContactData->force[1], 0.0, 1e-3); + EXPECT_NEAR(extraContactData->force[2], + zeroForce ? 0 : forceExpectations.at(testCollision), 1e-3); + }; + +#ifdef DART_HAS_CONTACT_SURFACE + size_t numContactCallbackCalls = 0u; + auto contactCallback = [&]( const gz::physics::World3d::Contact& _contact, size_t _numContactsOnCollision, ContactSurfaceParams& _surfaceParams) - { - numContactCallbackCalls++; - checkContact(_contact, true); - EXPECT_EQ(1u, _numContactsOnCollision); - // the values in _surfaceParams are implemented as std::optional to allow - // physics engines fill only those parameters that are actually - // implemented - ASSERT_TRUE(_surfaceParams.frictionCoeff.has_value()); - ASSERT_TRUE(_surfaceParams.secondaryFrictionCoeff.has_value()); - // not implemented in DART yet - EXPECT_FALSE(_surfaceParams.rollingFrictionCoeff.has_value()); - // not implemented in DART yet - EXPECT_FALSE(_surfaceParams.secondaryRollingFrictionCoeff.has_value()); - // not implemented in DART yet - EXPECT_FALSE(_surfaceParams.torsionalFrictionCoeff.has_value()); - ASSERT_TRUE(_surfaceParams.slipCompliance.has_value()); - ASSERT_TRUE(_surfaceParams.secondarySlipCompliance.has_value()); - ASSERT_TRUE(_surfaceParams.restitutionCoeff.has_value()); - ASSERT_TRUE(_surfaceParams.firstFrictionalDirection.has_value()); - ASSERT_TRUE(_surfaceParams.contactSurfaceMotionVelocity.has_value()); - // these constraint parameters are implemented in DART but are not filled - // when the callback is called; they are only read after the callback ends - EXPECT_FALSE(_surfaceParams.errorReductionParameter.has_value()); - EXPECT_FALSE(_surfaceParams.maxErrorReductionVelocity.has_value()); - EXPECT_FALSE(_surfaceParams.maxErrorAllowance.has_value()); - EXPECT_FALSE(_surfaceParams.constraintForceMixing.has_value()); - - EXPECT_NEAR(_surfaceParams.frictionCoeff.value(), 1.0, 1e-6); - EXPECT_NEAR(_surfaceParams.secondaryFrictionCoeff.value(), 1.0, 1e-6); - EXPECT_NEAR(_surfaceParams.slipCompliance.value(), 0.0, 1e-6); - EXPECT_NEAR(_surfaceParams.secondarySlipCompliance.value(), 0.0, 1e-6); - EXPECT_NEAR(_surfaceParams.restitutionCoeff.value(), 0.0, 1e-6); - - EXPECT_TRUE(gz::physics::test::Equal(Eigen::Vector3d(0, 0, 1), - _surfaceParams.firstFrictionalDirection.value(), 1e-6)); - - EXPECT_TRUE(gz::physics::test::Equal(Eigen::Vector3d(0, 0, 0), - _surfaceParams.contactSurfaceMotionVelocity.value(), 1e-6)); - }; - world->AddContactPropertiesCallback("test", contactCallback); - #endif + { + numContactCallbackCalls++; + checkContact(_contact, true); + EXPECT_EQ(1u, _numContactsOnCollision); + // the values in _surfaceParams are implemented as std::optional to allow + // physics engines fill only those parameters that are actually + // implemented + ASSERT_TRUE(_surfaceParams.frictionCoeff.has_value()); + ASSERT_TRUE(_surfaceParams.secondaryFrictionCoeff.has_value()); + // not implemented in DART yet + EXPECT_FALSE(_surfaceParams.rollingFrictionCoeff.has_value()); + // not implemented in DART yet + EXPECT_FALSE(_surfaceParams.secondaryRollingFrictionCoeff.has_value()); + // not implemented in DART yet + EXPECT_FALSE(_surfaceParams.torsionalFrictionCoeff.has_value()); + ASSERT_TRUE(_surfaceParams.slipCompliance.has_value()); + ASSERT_TRUE(_surfaceParams.secondarySlipCompliance.has_value()); + ASSERT_TRUE(_surfaceParams.restitutionCoeff.has_value()); + ASSERT_TRUE(_surfaceParams.firstFrictionalDirection.has_value()); + ASSERT_TRUE(_surfaceParams.contactSurfaceMotionVelocity.has_value()); + // these constraint parameters are implemented in DART but are not filled + // when the callback is called; they are only read after the callback ends + EXPECT_FALSE(_surfaceParams.errorReductionParameter.has_value()); + EXPECT_FALSE(_surfaceParams.maxErrorReductionVelocity.has_value()); + EXPECT_FALSE(_surfaceParams.maxErrorAllowance.has_value()); + EXPECT_FALSE(_surfaceParams.constraintForceMixing.has_value()); + + EXPECT_NEAR(_surfaceParams.frictionCoeff.value(), 1.0, 1e-6); + EXPECT_NEAR(_surfaceParams.secondaryFrictionCoeff.value(), 1.0, 1e-6); + EXPECT_NEAR(_surfaceParams.slipCompliance.value(), 0.0, 1e-6); + EXPECT_NEAR(_surfaceParams.secondarySlipCompliance.value(), 0.0, 1e-6); + EXPECT_NEAR(_surfaceParams.restitutionCoeff.value(), 0.0, 1e-6); + + EXPECT_TRUE(gz::physics::test::Equal(Eigen::Vector3d(0, 0, 1), + _surfaceParams.firstFrictionalDirection.value(), 1e-6)); + + EXPECT_TRUE(gz::physics::test::Equal(Eigen::Vector3d(0, 0, 0), + _surfaceParams.contactSurfaceMotionVelocity.value(), 1e-6)); + }; + world->AddContactPropertiesCallback("test", contactCallback); +#endif - // The first step already has contacts, but the contact force due to the - // impact does not match the steady-state force generated by the - // body's weight. - StepWorld(world, true); + // The first step already has contacts, but the contact force due to the + // impact does not match the steady-state force generated by the + // body's weight. + StepWorld(world, true); - #ifdef DART_HAS_CONTACT_SURFACE - // There are 4 collision bodies in the world all colliding at the same time - EXPECT_EQ(4u, numContactCallbackCalls); - #endif +#ifdef DART_HAS_CONTACT_SURFACE + // There are 4 collision bodies in the world all colliding at the same time + EXPECT_EQ(4u, numContactCallbackCalls); +#endif - // After a second step, the contact force reaches steady-state - StepWorld(world, false); + // After a second step, the contact force reaches steady-state + StepWorld(world, false); - #ifdef DART_HAS_CONTACT_SURFACE - // There are 4 collision bodies in the world all colliding at the same time - EXPECT_EQ(8u, numContactCallbackCalls); - #endif +#ifdef DART_HAS_CONTACT_SURFACE + // There are 4 collision bodies in the world all colliding at the same time + EXPECT_EQ(8u, numContactCallbackCalls); +#endif - auto contacts = world->GetContactsFromLastStep(); - if(this->PhysicsEngineName(name) != "tpe") - { - EXPECT_EQ(4u, contacts.size()); - } + auto contacts = world->GetContactsFromLastStep(); + if(this->PhysicsEngineName(name) != "tpe") + { + EXPECT_EQ(4u, contacts.size()); + } - for (auto &contact : contacts) - { - checkContact(contact, false); - } + for (auto &contact : contacts) + { + checkContact(contact, false); + } - #ifdef DART_HAS_CONTACT_SURFACE - // removing a non-existing callback yields no error but returns false - EXPECT_FALSE(world->RemoveContactPropertiesCallback("foo")); +#ifdef DART_HAS_CONTACT_SURFACE + // removing a non-existing callback yields no error but returns false + EXPECT_FALSE(world->RemoveContactPropertiesCallback("foo")); - // removing an existing callback works and the callback is no longer called - EXPECT_TRUE(world->RemoveContactPropertiesCallback("test")); + // removing an existing callback works and the callback is no longer called + EXPECT_TRUE(world->RemoveContactPropertiesCallback("test")); - // Third step - StepWorld(world, false); + // Third step + StepWorld(world, false); - // Number of callback calls is the same as after the 2nd call - EXPECT_EQ(8u, numContactCallbackCalls); + // Number of callback calls is the same as after the 2nd call + EXPECT_EQ(8u, numContactCallbackCalls); - // Now we check that changing _surfaceParams inside the contact properties - // callback affects the result of the simulation; we set - // contactSurfaceMotionVelocity to [1,0,0] which accelerates the contact - // points from 0 m/s to 1 m/s in a single simulation step. + // Now we check that changing _surfaceParams inside the contact properties + // callback affects the result of the simulation; we set + // contactSurfaceMotionVelocity to [1,0,0] which accelerates the contact + // points from 0 m/s to 1 m/s in a single simulation step. - auto contactCallback2 = [&]( + auto contactCallback2 = [&]( const gz::physics::World3d::Contact& /*_contact*/, size_t /*_numContactsOnCollision*/, ContactSurfaceParams& _surfaceParams) - { - numContactCallbackCalls++; - // friction direction is [0,0,1] and contact surface motion velocity uses - // the X value to denote the desired velocity along the friction direction - _surfaceParams.contactSurfaceMotionVelocity->x() = 1.0; - }; - world->AddContactPropertiesCallback("test2", contactCallback2); - - numContactCallbackCalls = 0u; - // Fourth step - StepWorld(world, false); - EXPECT_EQ(4u, numContactCallbackCalls); - - // Adjust the expected forces to account for the added acceleration along Z - forceExpectations = - { - // Contact force expectations are: - // link mass * (gravity + acceleration to 1 m.s^-1 in 1 ms) - {sphere->GetLink(0)->GetShape(0), 0.1 * gravity + 100}, - {sphere->GetLink(1)->GetShape(0), 1.0 * gravity + 999.99}, - {sphere->GetLink(2)->GetShape(0), 2.0 * gravity + 1999.98}, - {sphere->GetLink(3)->GetShape(0), 3.0 * gravity + 2999.97}, - }; - - // Verify that the detected contacts correspond to the adjusted expectations - contacts = world->GetContactsFromLastStep(); - EXPECT_EQ(4u, contacts.size()); - for (auto &contact : contacts) - { - checkContact(contact, false); - } - - EXPECT_TRUE(world->RemoveContactPropertiesCallback("test2")); - #endif + { + numContactCallbackCalls++; + // friction direction is [0,0,1] and contact surface motion velocity uses + // the X value to denote the desired velocity along the friction direction + _surfaceParams.contactSurfaceMotionVelocity->x() = 1.0; + }; + world->AddContactPropertiesCallback("test2", contactCallback2); + + numContactCallbackCalls = 0u; + // Fourth step + StepWorld(world, false); + EXPECT_EQ(4u, numContactCallbackCalls); + + // Adjust the expected forces to account for the added acceleration along Z + forceExpectations = + { + // Contact force expectations are: + // link mass * (gravity + acceleration to 1 m.s^-1 in 1 ms) + {sphere->GetLink(0)->GetShape(0), 0.1 * gravity + 100}, + {sphere->GetLink(1)->GetShape(0), 1.0 * gravity + 999.99}, + {sphere->GetLink(2)->GetShape(0), 2.0 * gravity + 1999.98}, + {sphere->GetLink(3)->GetShape(0), 3.0 * gravity + 2999.97}, + }; + + // Verify that the detected contacts correspond to the adjusted expectations + contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(4u, contacts.size()); + for (auto &contact : contacts) + { + checkContact(contact, false); } + + EXPECT_TRUE(world->RemoveContactPropertiesCallback("test2")); +#endif + } +} + +TYPED_TEST(SimulationFeaturesTestBasic, MultipleCollisions) +{ + for (const std::string &name : this->pluginNames) + { + CHECK_UNSUPPORTED_ENGINE(name, "tpe") + + auto world = LoadPluginAndWorld( + this->loader, + name, + gz::common::joinPaths(TEST_WORLD_DIR, "multiple_collisions.sdf")); + + // model free group test + auto model = world->GetModel("box"); + auto freeGroup = model->FindFreeGroup(); + ASSERT_NE(nullptr, freeGroup); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + ASSERT_NE(nullptr, freeGroup->CanonicalLink()); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + ASSERT_NE(nullptr, freeGroup->RootLink()); + + auto link = model->GetLink("box_link"); + auto freeGroupLink = link->FindFreeGroup(); + ASSERT_NE(nullptr, freeGroupLink); + + StepWorld(world, true); + + auto frameData = model->GetLink(0)->FrameDataRelativeToWorld(); + EXPECT_EQ(gz::math::Pose3d(0, 0, 4, 0, 0, 0), + gz::math::eigen3::convert(frameData.pose)); + + StepWorld(world, false, 1000); + frameData = model->GetLink(0)->FrameDataRelativeToWorld(); + gz::math::Pose3d framePose = gz::math::eigen3::convert(frameData.pose); + + EXPECT_NEAR(0.5, framePose.Z(), 0.1); } } diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index a359cded7..c2165f563 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -33,30 +33,7 @@ #include - -// A predicate-formatter for asserting that two vectors are approximately equal. -class AssertVectorApprox -{ -public: explicit AssertVectorApprox(double _tol = 1e-6) : tol(_tol) -{ -} - -public: ::testing::AssertionResult operator()( - const char *_mExpr, const char *_nExpr, Eigen::Vector3d _m, - Eigen::Vector3d _n) -{ - if (gz::physics::test::Equal(_m, _n, this->tol)) - return ::testing::AssertionSuccess(); - - return ::testing::AssertionFailure() - << _mExpr << " and " << _nExpr << " ([" << _m.transpose() - << "] and [" << _n.transpose() << "]" - << ") are not equal"; -} - -private: double tol; -}; - +using AssertVectorApprox = gz::physics::test::AssertVectorApprox; template class WorldFeaturesTest: @@ -97,7 +74,7 @@ using GravityFeatures = gz::physics::FeatureList< using GravityFeaturesTestTypes = ::testing::Types; TYPED_TEST_SUITE(WorldFeaturesTest, - GravityFeatures); + GravityFeatures,); ///////////////////////////////////////////////// TYPED_TEST(WorldFeaturesTest, GravityFeatures) diff --git a/test/common_test/worlds/falling_added_mass.world b/test/common_test/worlds/falling_added_mass.world new file mode 100644 index 000000000..677d21719 --- /dev/null +++ b/test/common_test/worlds/falling_added_mass.world @@ -0,0 +1,143 @@ + + + + + + + + 0 0 2 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 0.4 + 0 + 0 + 0.4 + 0 + 0.4 + + 1.0 + + + 0.0 0.0 0.0 0 0 0 + + + 1 + + + + + 0.0 0.0 0.0 0 0 0 + + + 1 + + + + + + + + 0 0 2 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 0.4 + 0 + 0 + 0.4 + 0 + 0.4 + + + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + 1.0 + + + + + + 0 0 2 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 0.4 + 0 + 0 + 0.4 + 0 + 0.4 + + + 1 + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + 1.0 + + + + + + 0 0 2 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 0.4 + 0 + 0 + 0.4 + 0 + 0.4 + + 2.0 + + + + + diff --git a/test/common_test/worlds/multiple_collisions.sdf b/test/common_test/worlds/multiple_collisions.sdf new file mode 100644 index 000000000..b58baf34b --- /dev/null +++ b/test/common_test/worlds/multiple_collisions.sdf @@ -0,0 +1,94 @@ + + + + + + gz-physics-bullet-featherstone-plugin + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + false + 0 0 4.0 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + 0 0 0.25 0 0 0 + + + 1 1 0.5 + + + + + 0 0 0.25 0 0 0 + + + 1 1 0.5 + + + + + + + 0 0 -0.25 0 0 0 + + + 1 1 0.5 + + + + + 0 0 -0.25 0 0 0 + + + 1 1 0.5 + + + + + + + diff --git a/test/integration/DoublePendulum.cc b/test/integration/DoublePendulum.cc index c19bf1047..329d1e5e2 100644 --- a/test/integration/DoublePendulum.cc +++ b/test/integration/DoublePendulum.cc @@ -59,17 +59,17 @@ void DoublePendulum_TEST(gz::plugin::PluginPtr _plugin) auto world = engine->GetWorld(0); - gz::physics::ForwardStep::State state; - gz::physics::ForwardStep::Output output; - gz::physics::ForwardStep::Input input; + ForwardStep::State state; + ForwardStep::Output output; + ForwardStep::Input input; const std::chrono::duration dt(std::chrono::milliseconds(1)); - gz::physics::TimeStep &timeStep = - input.Get(); + TimeStep &timeStep = + input.Get(); timeStep.dt = dt.count(); - gz::physics::GeneralizedParameters &efforts = - input.Get(); + GeneralizedParameters &efforts = + input.Get(); efforts.dofs.push_back(0); efforts.dofs.push_back(1); efforts.forces.push_back(0.0); @@ -78,11 +78,11 @@ void DoublePendulum_TEST(gz::plugin::PluginPtr _plugin) // No input on the first step, let's just see the output world->Step(output, state, input); - ASSERT_TRUE(output.Has()); - const auto positions0 = output.Get(); + ASSERT_TRUE(output.Has()); + const auto positions0 = output.Get(); - ASSERT_TRUE(output.Has()); - const auto poses0 = output.Get(); + ASSERT_TRUE(output.Has()); + const auto poses0 = output.Get(); // the double pendulum is initially fully inverted // and angles are defined as zero in this state @@ -116,7 +116,7 @@ void DoublePendulum_TEST(gz::plugin::PluginPtr _plugin) unsigned int settleSteps = settleTime / dt; for (unsigned int i = 0; i < settleSteps; ++i) { - auto positions = output.Get(); + auto positions = output.Get(); double error0 = positions.positions[positions.dofs[0]] - target10; double error1 = positions.positions[positions.dofs[1]] - target11; @@ -127,15 +127,15 @@ void DoublePendulum_TEST(gz::plugin::PluginPtr _plugin) } // expect joints are near target positions - ASSERT_TRUE(output.Has()); - auto positions1 = output.Get(); + ASSERT_TRUE(output.Has()); + auto positions1 = output.Get(); double angle10 = positions1.positions[positions1.dofs[0]]; double angle11 = positions1.positions[positions1.dofs[1]]; EXPECT_NEAR(target10, angle10, 1e-5); EXPECT_NEAR(target11, angle11, 1e-3); - ASSERT_TRUE(output.Has()); - const auto poses1 = output.Get(); + ASSERT_TRUE(output.Has()); + const auto poses1 = output.Get(); ASSERT_EQ(2u, poses1.entries.size()); for (const auto & worldPose : poses1.entries) { @@ -151,7 +151,7 @@ void DoublePendulum_TEST(gz::plugin::PluginPtr _plugin) } // test recording the state and repeatability - gz::physics::ForwardStep::State bookmark = state; + ForwardStep::State bookmark = state; std::vector errorHistory0; std::vector errorHistory1; @@ -168,7 +168,7 @@ void DoublePendulum_TEST(gz::plugin::PluginPtr _plugin) for (unsigned int i = 0; i < settleSteps; ++i) { - auto positions = output.Get(); + auto positions = output.Get(); double error0 = positions.positions[positions.dofs[0]] - target20; double error1 = positions.positions[positions.dofs[1]] - target21; errorHistory0.push_back(error0); @@ -181,16 +181,16 @@ void DoublePendulum_TEST(gz::plugin::PluginPtr _plugin) } // expect joints are near target positions again - ASSERT_TRUE(output.Has()); - auto positions2 = output.Get(); + ASSERT_TRUE(output.Has()); + auto positions2 = output.Get(); double angle20 = positions2.positions[positions2.dofs[0]]; double angle21 = positions2.positions[positions2.dofs[1]]; EXPECT_NEAR(target20, angle20, 1e-4); EXPECT_NEAR(target21, angle21, 1e-3); // // Go back to the bookmarked state and run through the steps again. - // gz::physics::SetState *setState = - // _plugin->QueryInterface(); + // SetState *setState = + // _plugin->QueryInterface(); // ASSERT_TRUE(setState); // setState->SetStateTo(bookmark); @@ -205,7 +205,7 @@ void DoublePendulum_TEST(gz::plugin::PluginPtr _plugin) // for (unsigned int i = 0; i < settleSteps; ++i) // { - // auto positions = output.Get(); + // auto positions = output.Get(); // double error0 = positions.positions[positions.dofs[0]] - target20; // double error1 = positions.positions[positions.dofs[1]] - target21; // EXPECT_DOUBLE_EQ(error0, errorHistory0[i]); @@ -218,8 +218,8 @@ void DoublePendulum_TEST(gz::plugin::PluginPtr _plugin) // } // // expect joints are near target positions again - // ASSERT_TRUE(output.Has()); - // auto positions3 = output.Get(); + // ASSERT_TRUE(output.Has()); + // auto positions3 = output.Get(); // double angle30 = positions3.positions[positions3.dofs[0]]; // double angle31 = positions3.positions[positions3.dofs[1]]; // EXPECT_DOUBLE_EQ(angle20, angle30); diff --git a/test/integration/FeatureSystem.cc b/test/integration/FeatureSystem.cc index fc0ee3241..48fd3b453 100644 --- a/test/integration/FeatureSystem.cc +++ b/test/integration/FeatureSystem.cc @@ -43,7 +43,7 @@ TEST(FeatureSystem, MockPluginInvalidFeatures) { // mismatch between 2d and 3d mock::MockEngine3dPtr engine = - gz::physics::RequestEngine3d::From( + RequestEngine3d::From( LoadMockPlugin("mock::EntitiesPlugin2d")); EXPECT_EQ(nullptr, engine); } @@ -52,7 +52,7 @@ TEST(FeatureSystem, MockPluginInvalidFeatures) TEST(FeatureSystem, MockPlugin) { mock::MockEngine3dPtr engine = - gz::physics::RequestEngine3d::From( + RequestEngine3d::From( LoadMockPlugin("mock::EntitiesPlugin3d")); EXPECT_EQ("Only one engine", engine->Name()); @@ -116,7 +116,7 @@ TEST(FeatureSystem, MockPlugin) TEST(FeatureSystem, MockCenterOfMass3d) { mock::MockEngine3dPtr engine = - gz::physics::RequestEngine3d::From( + RequestEngine3d::From( LoadMockPlugin("mock::EntitiesPlugin3d")); mock::MockWorld3dPtr world = engine->GetWorld("Some world"); @@ -166,7 +166,7 @@ TEST(FeatureSystem, MockCenterOfMass3d) TEST(FeatureSystem, MockCenterOfMass2d) { mock::MockEngine2dPtr engine = - gz::physics::RequestEngine2d::From( + RequestEngine2d::From( LoadMockPlugin("mock::EntitiesPlugin2d")); mock::MockWorld2dPtr world = engine->GetWorld("Some world"); diff --git a/test/plugins/CMakeLists.txt b/test/plugins/CMakeLists.txt index 61dc62edd..6d76ac351 100644 --- a/test/plugins/CMakeLists.txt +++ b/test/plugins/CMakeLists.txt @@ -21,7 +21,12 @@ foreach(plugin IN LISTS plugins) endforeach() if (DART_FOUND) - target_link_libraries(MockDoublePendulum PUBLIC ${DART_LIBRARIES}) + # Linking against libdart-collision-bullet causes a segfault on macOS for + # some reason. We'll remove it since MockDoublePendulum doesn't use bullet + # for collision checking. + set(DART_LIBRARIES_NO_BULLET ${DART_LIBRARIES}) + list(REMOVE_ITEM DART_LIBRARIES_NO_BULLET dart-collision-bullet) + target_link_libraries(MockDoublePendulum PUBLIC ${DART_LIBRARIES_NO_BULLET}) target_compile_definitions(MockDoublePendulum PRIVATE "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\"") if (MSVC) diff --git a/tpe/include/ignition/physics/tpe-plugin/include/ignition/physics/tpe-plugin/Export.hh b/tpe/include/ignition/physics/tpe-plugin/include/ignition/physics/tpe-plugin/Export.hh new file mode 100644 index 000000000..2db192189 --- /dev/null +++ b/tpe/include/ignition/physics/tpe-plugin/include/ignition/physics/tpe-plugin/Export.hh @@ -0,0 +1,19 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include diff --git a/tpe/include/ignition/physics/tpelib/include/ignition/physics/tpelib/Export.hh b/tpe/include/ignition/physics/tpelib/include/ignition/physics/tpelib/Export.hh new file mode 100644 index 000000000..aae259c4a --- /dev/null +++ b/tpe/include/ignition/physics/tpelib/include/ignition/physics/tpelib/Export.hh @@ -0,0 +1,19 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include diff --git a/tpe/lib/src/CollisionDetector_TEST.cc b/tpe/lib/src/CollisionDetector_TEST.cc index 25a26003c..1e580c678 100644 --- a/tpe/lib/src/CollisionDetector_TEST.cc +++ b/tpe/lib/src/CollisionDetector_TEST.cc @@ -89,7 +89,7 @@ TEST(CollisionDetector, CheckCollisions) Entity &collisionAEnt = linkA->AddCollision(); Collision *collisionA = static_cast(&collisionAEnt); BoxShape boxShapeA; - boxShapeA.SetSize(gz::math::Vector3d(4, 4, 4)); + boxShapeA.SetSize(math::Vector3d(4, 4, 4)); collisionA->SetShape(boxShapeA); // model B @@ -377,7 +377,7 @@ TEST(CollisionDetector, CheckStaticCollisionFiltering) Entity &collisionAEnt = linkA->AddCollision(); Collision *collisionA = static_cast(&collisionAEnt); BoxShape boxShapeA; - boxShapeA.SetSize(gz::math::Vector3d(4, 4, 4)); + boxShapeA.SetSize(math::Vector3d(4, 4, 4)); collisionA->SetShape(boxShapeA); // model B @@ -388,7 +388,7 @@ TEST(CollisionDetector, CheckStaticCollisionFiltering) Entity &collisionBEnt = linkB->AddCollision(); Collision *collisionB = static_cast(&collisionBEnt); BoxShape boxShapeB; - boxShapeB.SetSize(gz::math::Vector3d(4, 4, 4)); + boxShapeB.SetSize(math::Vector3d(4, 4, 4)); collisionB->SetShape(boxShapeB); // check collisions diff --git a/tpe/lib/src/Collision_TEST.cc b/tpe/lib/src/Collision_TEST.cc index 9b481be82..6222ba219 100644 --- a/tpe/lib/src/Collision_TEST.cc +++ b/tpe/lib/src/Collision_TEST.cc @@ -60,11 +60,11 @@ TEST(Collision, BoxShape) { Collision collision; BoxShape boxShape; - boxShape.SetSize(gz::math::Vector3d(0.5, 0.5, 0.5)); + boxShape.SetSize(math::Vector3d(0.5, 0.5, 0.5)); collision.SetShape(boxShape); auto result = collision.GetShape(); ASSERT_NE(nullptr, result); - EXPECT_EQ(gz::math::Vector3d(0.25, 0.25, 0.25), + EXPECT_EQ(math::Vector3d(0.25, 0.25, 0.25), result->GetBoundingBox().Max()); } @@ -78,7 +78,7 @@ TEST(Collision, CapsuleShape) collision.SetShape(CapsuleShape); auto result = collision.GetShape(); ASSERT_NE(nullptr, result); - EXPECT_EQ(gz::math::Vector3d(2.0, 2.0, 3.5), + EXPECT_EQ(math::Vector3d(2.0, 2.0, 3.5), result->GetBoundingBox().Max()); } @@ -92,7 +92,7 @@ TEST(Collision, CylinderShape) collision.SetShape(cylinderShape); auto result = collision.GetShape(); ASSERT_NE(nullptr, result); - EXPECT_EQ(gz::math::Vector3d(2.0, 2.0, 1.5), + EXPECT_EQ(math::Vector3d(2.0, 2.0, 1.5), result->GetBoundingBox().Max()); } @@ -105,7 +105,7 @@ TEST(Collision, EllipsoidShape) collision.SetShape(EllipsoidShape); auto result = collision.GetShape(); ASSERT_NE(nullptr, result); - EXPECT_EQ(gz::math::Vector3d(1.0, 2.0, 1.3), + EXPECT_EQ(math::Vector3d(1.0, 2.0, 1.3), result->GetBoundingBox().Max());} ///////////////////////////////////////////////// @@ -117,7 +117,7 @@ TEST(Collision, SphereShape) collision.SetShape(sphereShape); auto result = collision.GetShape(); ASSERT_NE(nullptr, result); - EXPECT_EQ(gz::math::Vector3d(2.0, 2.0, 2.0), + EXPECT_EQ(math::Vector3d(2.0, 2.0, 2.0), result->GetBoundingBox().Max()); } diff --git a/tpe/lib/src/Shape.hh b/tpe/lib/src/Shape.hh index d7a5f7cae..1cdb15415 100644 --- a/tpe/lib/src/Shape.hh +++ b/tpe/lib/src/Shape.hh @@ -249,7 +249,7 @@ class GZ_PHYSICS_TPELIB_VISIBLE MeshShape : public Shape /// \brief Set mesh /// \param[in] _mesh Mesh object - public: void SetMesh(const gz::common::Mesh &_mesh); + public: void SetMesh(const common::Mesh &_mesh); /// \brief Get mesh scale /// \return Mesh scale diff --git a/tpe/plugin/src/EntityManagement_TEST.cc b/tpe/plugin/src/EntityManagement_TEST.cc index 4b9ca3cd8..e248f2e62 100644 --- a/tpe/plugin/src/EntityManagement_TEST.cc +++ b/tpe/plugin/src/EntityManagement_TEST.cc @@ -23,21 +23,23 @@ #include "EntityManagementFeatures.hh" -struct TestFeatureList : gz::physics::FeatureList< - gz::physics::tpeplugin::EntityManagementFeatureList +using namespace gz; + +struct TestFeatureList : physics::FeatureList< + physics::tpeplugin::EntityManagementFeatureList > { }; TEST(EntityManagement_TEST, ConstructEmptyWorld) { - gz::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(tpe_plugin_LIB); - gz::plugin::PluginPtr tpe_plugin = + plugin::PluginPtr tpe_plugin = loader.Instantiate("gz::physics::tpeplugin::Plugin"); // GetEntities_TEST auto engine = - gz::physics::RequestEngine3d::From(tpe_plugin); + physics::RequestEngine3d::From(tpe_plugin); ASSERT_NE(nullptr, engine); EXPECT_EQ("tpe", engine->GetName()); EXPECT_EQ(0u, engine->GetIndex()); @@ -98,14 +100,14 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) TEST(EntityManagement_TEST, RemoveEntities) { - gz::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(tpe_plugin_LIB); - gz::plugin::PluginPtr tpe_plugin = + plugin::PluginPtr tpe_plugin = loader.Instantiate("gz::physics::tpeplugin::Plugin"); auto engine = - gz::physics::RequestEngine3d::From(tpe_plugin); + physics::RequestEngine3d::From(tpe_plugin); ASSERT_NE(nullptr, engine); auto world = engine->ConstructEmptyWorld("empty world"); diff --git a/tpe/plugin/src/SDFFeatures_TEST.cc b/tpe/plugin/src/SDFFeatures_TEST.cc index 062b59714..b9b592d86 100644 --- a/tpe/plugin/src/SDFFeatures_TEST.cc +++ b/tpe/plugin/src/SDFFeatures_TEST.cc @@ -41,28 +41,30 @@ #include "lib/src/World.hh" #include "World.hh" -struct TestFeatureList : gz::physics::FeatureList< - gz::physics::tpeplugin::RetrieveWorld, - gz::physics::GetModelFromWorld, - gz::physics::sdf::ConstructSdfLink, - gz::physics::sdf::ConstructSdfModel, - gz::physics::sdf::ConstructSdfNestedModel, - gz::physics::sdf::ConstructSdfWorld +using namespace gz; + +struct TestFeatureList : physics::FeatureList< + physics::tpeplugin::RetrieveWorld, + physics::GetModelFromWorld, + physics::sdf::ConstructSdfLink, + physics::sdf::ConstructSdfModel, + physics::sdf::ConstructSdfNestedModel, + physics::sdf::ConstructSdfWorld > { }; -using World = gz::physics::World3d; -using WorldPtr = gz::physics::World3dPtr; +using World = physics::World3d; +using WorldPtr = physics::World3dPtr; auto LoadEngine() { - gz::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(tpe_plugin_LIB); - gz::plugin::PluginPtr tpe_plugin = + plugin::PluginPtr tpe_plugin = loader.Instantiate("gz::physics::tpeplugin::Plugin"); auto engine = - gz::physics::RequestEngine3d::From(tpe_plugin); + physics::RequestEngine3d::From(tpe_plugin); return engine; } @@ -96,264 +98,264 @@ TEST(SDFFeatures_TEST, CheckTpeData) // check model 01 { - gz::physics::tpelib::Entity &model = + physics::tpelib::Entity &model = tpeWorld->GetChildByName("ground_plane"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); EXPECT_EQ("ground_plane", model.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, model.GetPose()); - EXPECT_EQ(gz::math::Pose3d::Zero, model.GetWorldPose()); + EXPECT_EQ(math::Pose3d::Zero, model.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, model.GetWorldPose()); EXPECT_EQ(1u, model.GetChildCount()); - gz::physics::tpelib::Entity &link = model.GetChildByName("link"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link = model.GetChildByName("link"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); EXPECT_EQ("link", link.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, link.GetPose()); - EXPECT_EQ(gz::math::Pose3d::Zero, link.GetWorldPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetWorldPose()); EXPECT_EQ(1u, link.GetChildCount()); - gz::physics::tpelib::Entity &collision = + physics::tpelib::Entity &collision = link.GetChildByName("collision"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision.GetId()); EXPECT_EQ("collision", collision.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, collision.GetPose()); - EXPECT_EQ(gz::math::Pose3d::Zero, collision.GetWorldPose()); + EXPECT_EQ(math::Pose3d::Zero, collision.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, collision.GetWorldPose()); } // check model 02 { - gz::physics::tpelib::Entity &model = + physics::tpelib::Entity &model = tpeWorld->GetChildByName("double_pendulum_with_base"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); EXPECT_EQ("double_pendulum_with_base", model.GetName()); - EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), model.GetPose()); - EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), model.GetWorldPose()); + EXPECT_EQ(math::Pose3d(1, 0, 0, 0, 0, 0), model.GetPose()); + EXPECT_EQ(math::Pose3d(1, 0, 0, 0, 0, 0), model.GetWorldPose()); EXPECT_EQ(3u, model.GetChildCount()); - gz::physics::tpelib::Entity &link = model.GetChildByName("base"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link = model.GetChildByName("base"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); EXPECT_EQ("base", link.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, link.GetPose()); - EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), model.GetWorldPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(math::Pose3d(1, 0, 0, 0, 0, 0), model.GetWorldPose()); EXPECT_EQ(2u, link.GetChildCount()); - gz::physics::tpelib::Entity &collision = + physics::tpelib::Entity &collision = link.GetChildByName("col_plate_on_ground"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision.GetId()); EXPECT_EQ("col_plate_on_ground", collision.GetName()); - EXPECT_EQ(gz::math::Pose3d(0, 0, 0.01, 0, 0, 0), collision.GetPose()); - EXPECT_EQ(gz::math::Pose3d(1, 0, 0.01, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 0.01, 0, 0, 0), collision.GetPose()); + EXPECT_EQ(math::Pose3d(1, 0, 0.01, 0, 0, 0), collision.GetWorldPose()); - gz::physics::tpelib::Entity &collision02 = + physics::tpelib::Entity &collision02 = link.GetChildByName("col_pole"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision.GetId()); EXPECT_EQ("col_pole", collision02.GetName()); - EXPECT_EQ(gz::math::Pose3d(-0.275, 0, 1.1, 0, 0, 0), + EXPECT_EQ(math::Pose3d(-0.275, 0, 1.1, 0, 0, 0), collision02.GetPose()); - EXPECT_EQ(gz::math::Pose3d(0.725, 0, 1.1, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.725, 0, 1.1, 0, 0, 0), collision02.GetWorldPose()); - gz::physics::tpelib::Entity &link02 = + physics::tpelib::Entity &link02 = model.GetChildByName("upper_link"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link02.GetId()); EXPECT_EQ("upper_link", link02.GetName()); - EXPECT_EQ(gz::math::Pose3d(0, 0, 2.1, -1.5708, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 2.1, -1.5708, 0, 0), link02.GetPose()); - EXPECT_EQ(gz::math::Pose3d(1, 0, 2.1, -1.5708, 0, 0), + EXPECT_EQ(math::Pose3d(1, 0, 2.1, -1.5708, 0, 0), link02.GetWorldPose()); EXPECT_EQ(3u, link02.GetChildCount()); - gz::physics::tpelib::Entity &collision03 = + physics::tpelib::Entity &collision03 = link02.GetChildByName("col_upper_joint"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision03.GetId()); EXPECT_EQ("col_upper_joint", collision03.GetName()); - EXPECT_EQ(gz::math::Pose3d(-0.05, 0, 0, 0, 1.5708, 0), + EXPECT_EQ(math::Pose3d(-0.05, 0, 0, 0, 1.5708, 0), collision03.GetPose()); EXPECT_EQ( - gz::math::Pose3d(0.95, 0, 2.1, -1.5708, -4e-06, -1.5708), + math::Pose3d(0.95, 0, 2.1, -1.5708, -4e-06, -1.5708), collision03.GetWorldPose()); - gz::physics::tpelib::Entity &collision04 = + physics::tpelib::Entity &collision04 = link02.GetChildByName("col_lower_joint"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision04.GetId()); EXPECT_EQ("col_lower_joint", collision04.GetName()); - EXPECT_EQ(gz::math::Pose3d(0, 0, 1.0, 0, 1.5708, 0), + EXPECT_EQ(math::Pose3d(0, 0, 1.0, 0, 1.5708, 0), collision04.GetPose()); EXPECT_EQ( - gz::math::Pose3d(1, 1, 2.1, -1.5708, -4e-06, -1.5708), + math::Pose3d(1, 1, 2.1, -1.5708, -4e-06, -1.5708), collision04.GetWorldPose()); - gz::physics::tpelib::Entity &collision05 = + physics::tpelib::Entity &collision05 = link02.GetChildByName("col_cylinder"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision05.GetId()); EXPECT_EQ("col_cylinder", collision05.GetName()); - EXPECT_EQ(gz::math::Pose3d(0, 0, 0.5, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 0.5, 0, 0, 0), collision05.GetPose()); - EXPECT_EQ(gz::math::Pose3d(1, 0.5, 2.1, -1.5708, 0, 0), + EXPECT_EQ(math::Pose3d(1, 0.5, 2.1, -1.5708, 0, 0), collision05.GetWorldPose()); - gz::physics::tpelib::Entity &link03 = + physics::tpelib::Entity &link03 = model.GetChildByName("lower_link"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link03.GetId()); EXPECT_EQ("lower_link", link03.GetName()); - EXPECT_EQ(gz::math::Pose3d(0.25, 1.0, 2.1, -2, 0, 0), + EXPECT_EQ(math::Pose3d(0.25, 1.0, 2.1, -2, 0, 0), link03.GetPose()); - EXPECT_EQ(gz::math::Pose3d(1.25, 1.0, 2.1, -2, 0, 0), + EXPECT_EQ(math::Pose3d(1.25, 1.0, 2.1, -2, 0, 0), link03.GetWorldPose()); EXPECT_EQ(2u, link03.GetChildCount()); - gz::physics::tpelib::Entity &collision06 = + physics::tpelib::Entity &collision06 = link03.GetChildByName("col_lower_joint"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision06.GetId()); EXPECT_EQ("col_lower_joint", collision06.GetName()); - EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 1.5708, 0), + EXPECT_EQ(math::Pose3d(0, 0, 0, 0, 1.5708, 0), collision06.GetPose()); EXPECT_EQ( - gz::math::Pose3d(1.25, 1, 2.1, -1.57079, -0.429204, -1.5708), + math::Pose3d(1.25, 1, 2.1, -1.57079, -0.429204, -1.5708), collision06.GetWorldPose()); - gz::physics::tpelib::Entity &collision07 = + physics::tpelib::Entity &collision07 = link03.GetChildByName("col_cylinder"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision07.GetId()); EXPECT_EQ("col_cylinder", collision07.GetName()); - EXPECT_EQ(gz::math::Pose3d(0, 0, 0.5, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 0.5, 0, 0, 0), collision07.GetPose()); - EXPECT_EQ(gz::math::Pose3d(1.25, 1.45465, 1.89193, -2, 0, 0), + EXPECT_EQ(math::Pose3d(1.25, 1.45465, 1.89193, -2, 0, 0), collision07.GetWorldPose()); } // check model 03 { - gz::physics::tpelib::Entity &model = + physics::tpelib::Entity &model = tpeWorld->GetChildByName("free_body"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); EXPECT_EQ("free_body", model.GetName()); - EXPECT_EQ(gz::math::Pose3d(0, 10, 10, 0, 0, 0), model.GetPose()); + EXPECT_EQ(math::Pose3d(0, 10, 10, 0, 0, 0), model.GetPose()); EXPECT_EQ(1u, model.GetChildCount()); - gz::physics::tpelib::Entity &link = model.GetChildByName("link"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link = model.GetChildByName("link"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); EXPECT_EQ("link", link.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); EXPECT_EQ(0u, link.GetChildCount()); } // check model 04 { - gz::physics::tpelib::Entity &model = + physics::tpelib::Entity &model = tpeWorld->GetChildByName("joint_limit_test"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); EXPECT_EQ("joint_limit_test", model.GetName()); - EXPECT_EQ(gz::math::Pose3d(10, 0, 2, 0, 0, 0), model.GetPose()); + EXPECT_EQ(math::Pose3d(10, 0, 2, 0, 0, 0), model.GetPose()); EXPECT_EQ(2u, model.GetChildCount()); - gz::physics::tpelib::Entity &link = model.GetChildByName("base"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link = model.GetChildByName("base"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); EXPECT_EQ("base", link.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); EXPECT_EQ(0u, link.GetChildCount()); - gz::physics::tpelib::Entity &link02 = model.GetChildByName("bar"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link02 = model.GetChildByName("bar"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link02.GetId()); EXPECT_EQ("bar", link02.GetName()); - EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), link02.GetPose()); + EXPECT_EQ(math::Pose3d(1, 0, 0, 0, 0, 0), link02.GetPose()); EXPECT_EQ(0u, link02.GetChildCount()); } // check model 05 { - gz::physics::tpelib::Entity &model = + physics::tpelib::Entity &model = tpeWorld->GetChildByName("screw_joint_test"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); EXPECT_EQ("screw_joint_test", model.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, model.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, model.GetPose()); EXPECT_EQ(2u, model.GetChildCount()); - gz::physics::tpelib::Entity &link = model.GetChildByName("link0"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link = model.GetChildByName("link0"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); EXPECT_EQ("link0", link.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); EXPECT_EQ(0u, link.GetChildCount()); - gz::physics::tpelib::Entity &link02 = model.GetChildByName("link1"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link02 = model.GetChildByName("link1"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link02.GetId()); EXPECT_EQ("link1", link02.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, link02.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link02.GetPose()); EXPECT_EQ(0u, link02.GetChildCount()); } // check model 06 { - gz::physics::tpelib::Entity &model = + physics::tpelib::Entity &model = tpeWorld->GetChildByName("unsupported_joint_test"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); EXPECT_EQ("unsupported_joint_test", model.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, model.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, model.GetPose()); EXPECT_EQ(6u, model.GetChildCount()); - gz::physics::tpelib::Entity &link = model.GetChildByName("link0"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link = model.GetChildByName("link0"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); EXPECT_EQ("link0", link.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); EXPECT_EQ(0u, link.GetChildCount()); - gz::physics::tpelib::Entity &link02 = model.GetChildByName("link1"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link02 = model.GetChildByName("link1"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link02.GetId()); EXPECT_EQ("link1", link02.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, link02.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link02.GetPose()); EXPECT_EQ(0u, link02.GetChildCount()); - gz::physics::tpelib::Entity &link03 = model.GetChildByName("link2"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link03 = model.GetChildByName("link2"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link03.GetId()); EXPECT_EQ("link2", link03.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, link03.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link03.GetPose()); EXPECT_EQ(0u, link03.GetChildCount()); - gz::physics::tpelib::Entity &link04 = model.GetChildByName("link3"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link04 = model.GetChildByName("link3"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link04.GetId()); EXPECT_EQ("link3", link04.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, link04.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link04.GetPose()); EXPECT_EQ(0u, link04.GetChildCount()); - gz::physics::tpelib::Entity &link05 = model.GetChildByName("link4"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link05 = model.GetChildByName("link4"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link05.GetId()); EXPECT_EQ("link4", link05.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, link05.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link05.GetPose()); EXPECT_EQ(0u, link05.GetChildCount()); - gz::physics::tpelib::Entity &link06 = model.GetChildByName("link5"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link06 = model.GetChildByName("link5"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link06.GetId()); EXPECT_EQ("link5", link06.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, link06.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link06.GetPose()); EXPECT_EQ(0u, link06.GetChildCount()); } } @@ -369,57 +371,57 @@ TEST(SDFFeatures_TEST, NestedModel) EXPECT_EQ(1u, world.GetModelCount()); // check top level model - gz::physics::tpelib::Entity &model = + physics::tpelib::Entity &model = tpeWorld->GetChildByName("model"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); EXPECT_EQ("model", model.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, model.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, model.GetPose()); EXPECT_EQ(2u, model.GetChildCount()); - gz::physics::tpelib::Entity &link = model.GetChildByName("link"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + physics::tpelib::Entity &link = model.GetChildByName("link"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); EXPECT_EQ("link", link.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, link.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); EXPECT_EQ(1u, link.GetChildCount()); - gz::physics::tpelib::Entity &collision = + physics::tpelib::Entity &collision = link.GetChildByName("collision"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), collision.GetId()); EXPECT_EQ("collision", collision.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, collision.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, collision.GetPose()); // check nested model - gz::physics::tpelib::Entity &nestedModel = + physics::tpelib::Entity &nestedModel = model.GetChildByName("nested_model"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), nestedModel.GetId()); EXPECT_EQ("nested_model", nestedModel.GetName()); - EXPECT_EQ(gz::math::Pose3d(1, 2, 2, 0, 0, 0), nestedModel.GetPose()); + EXPECT_EQ(math::Pose3d(1, 2, 2, 0, 0, 0), nestedModel.GetPose()); EXPECT_EQ(1u, nestedModel.GetChildCount()); - gz::physics::tpelib::Entity &nestedLink = + physics::tpelib::Entity &nestedLink = nestedModel.GetChildByName("nested_link"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), nestedLink.GetId()); EXPECT_EQ("nested_link", nestedLink.GetName()); - EXPECT_EQ(gz::math::Pose3d(3, 1, 1, 0, 0, 1.5707), + EXPECT_EQ(math::Pose3d(3, 1, 1, 0, 0, 1.5707), nestedLink.GetPose()); EXPECT_EQ(1u, nestedLink.GetChildCount()); - gz::physics::tpelib::Entity &nestedCollision = + physics::tpelib::Entity &nestedCollision = nestedLink.GetChildByName("nested_collision"); - ASSERT_NE(gz::physics::tpelib::Entity::kNullEntity.GetId(), + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), nestedCollision.GetId()); EXPECT_EQ("nested_collision", nestedCollision.GetName()); - EXPECT_EQ(gz::math::Pose3d::Zero, nestedCollision.GetPose()); + EXPECT_EQ(math::Pose3d::Zero, nestedCollision.GetPose()); // canonical link - gz::physics::tpelib::Model *m = - static_cast(&model); - gz::physics::tpelib::Entity canLink = m->GetCanonicalLink(); + physics::tpelib::Model *m = + static_cast(&model); + physics::tpelib::Entity canLink = m->GetCanonicalLink(); EXPECT_EQ(link.GetId(), canLink.GetId()); } diff --git a/tpe/plugin/src/ShapeFeatures.cc b/tpe/plugin/src/ShapeFeatures.cc index 8d3db5cd7..1caf408c2 100644 --- a/tpe/plugin/src/ShapeFeatures.cc +++ b/tpe/plugin/src/ShapeFeatures.cc @@ -401,7 +401,7 @@ LinearVector3d ShapeFeatures::GetMeshShapeScale( Identity ShapeFeatures::AttachMeshShape( const Identity &_linkID, const std::string &_name, - const gz::common::Mesh &_mesh, + const common::Mesh &_mesh, const Pose3d &_pose, const LinearVector3d &_scale) { diff --git a/tpe/plugin/src/ShapeFeatures.hh b/tpe/plugin/src/ShapeFeatures.hh index 1b077a9f3..bdc89130b 100644 --- a/tpe/plugin/src/ShapeFeatures.hh +++ b/tpe/plugin/src/ShapeFeatures.hh @@ -146,7 +146,7 @@ class ShapeFeatures : public: Identity AttachMeshShape( const Identity &_linkID, const std::string &_name, - const gz::common::Mesh &_mesh, + const common::Mesh &_mesh, const Pose3d &_pose, const LinearVector3d &_scale) override; diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index a22cc2d60..7fa3de841 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -49,50 +49,52 @@ #include "SimulationFeatures.hh" #include "World.hh" -struct TestFeatureList : gz::physics::FeatureList< - gz::physics::tpeplugin::SimulationFeatureList, - gz::physics::tpeplugin::ShapeFeatureList, - gz::physics::tpeplugin::EntityManagementFeatureList, - gz::physics::tpeplugin::FreeGroupFeatureList, - gz::physics::tpeplugin::RetrieveWorld, - gz::physics::GetContactsFromLastStepFeature, - gz::physics::LinkFrameSemantics, - gz::physics::GetModelBoundingBox, - gz::physics::sdf::ConstructSdfWorld, - gz::physics::sdf::ConstructSdfModel, - gz::physics::sdf::ConstructSdfNestedModel, - gz::physics::sdf::ConstructSdfLink, - gz::physics::sdf::ConstructSdfCollision +using namespace gz; + +struct TestFeatureList : physics::FeatureList< + physics::tpeplugin::SimulationFeatureList, + physics::tpeplugin::ShapeFeatureList, + physics::tpeplugin::EntityManagementFeatureList, + physics::tpeplugin::FreeGroupFeatureList, + physics::tpeplugin::RetrieveWorld, + physics::GetContactsFromLastStepFeature, + physics::LinkFrameSemantics, + physics::GetModelBoundingBox, + physics::sdf::ConstructSdfWorld, + physics::sdf::ConstructSdfModel, + physics::sdf::ConstructSdfNestedModel, + physics::sdf::ConstructSdfLink, + physics::sdf::ConstructSdfCollision > { }; -using TestEnginePtr = gz::physics::Engine3dPtr; -using TestWorldPtr = gz::physics::World3dPtr; -using TestModelPtr = gz::physics::Model3dPtr; -using TestLinkPtr = gz::physics::Link3dPtr; -using TestShapePtr = gz::physics::Shape3dPtr; -using TestContactPoint = gz::physics::World3d::ContactPoint; +using TestEnginePtr = physics::Engine3dPtr; +using TestWorldPtr = physics::World3dPtr; +using TestModelPtr = physics::Model3dPtr; +using TestLinkPtr = physics::Link3dPtr; +using TestShapePtr = physics::Shape3dPtr; +using TestContactPoint = physics::World3d::ContactPoint; std::unordered_set LoadWorlds( const std::string &_library, const std::string &_world) { - gz::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(_library); const std::set pluginNames = - gz::physics::FindFeatures3d::From(loader); + physics::FindFeatures3d::From(loader); EXPECT_EQ(1u, pluginNames.size()); std::unordered_set worlds; for (const std::string &name : pluginNames) { - gz::plugin::PluginPtr plugin = loader.Instantiate(name); + plugin::PluginPtr plugin = loader.Instantiate(name); gzdbg << " -- Plugin name: " << name << std::endl; auto engine = - gz::physics::RequestEngine3d::From(plugin); + physics::RequestEngine3d::From(plugin); EXPECT_NE(nullptr, engine); sdf::Root root; @@ -107,10 +109,10 @@ std::unordered_set LoadWorlds( } ///////////////////////////////////////////////// -static gz::math::Pose3d ResolveSdfPose( +static math::Pose3d ResolveSdfPose( const ::sdf::SemanticPose &_semPose) { - gz::math::Pose3d pose; + math::Pose3d pose; ::sdf::Errors errors = _semPose.Resolve(pose); EXPECT_TRUE(errors.empty()) << errors; return pose; @@ -119,23 +121,23 @@ static gz::math::Pose3d ResolveSdfPose( std::unordered_set LoadWorldsPiecemeal( const std::string &_library, const std::string &_world) { - gz::plugin::Loader loader; + plugin::Loader loader; loader.LoadLib(_library); const std::set pluginNames = - gz::physics::FindFeatures3d::From(loader); + physics::FindFeatures3d::From(loader); EXPECT_EQ(1u, pluginNames.size()); std::unordered_set worlds; for (const std::string &name : pluginNames) { - gz::plugin::PluginPtr plugin = loader.Instantiate(name); + plugin::PluginPtr plugin = loader.Instantiate(name); gzdbg << " -- Plugin name: " << name << std::endl; auto engine = - gz::physics::RequestEngine3d::From(plugin); + physics::RequestEngine3d::From(plugin); EXPECT_NE(nullptr, engine); sdf::Root root; @@ -234,9 +236,9 @@ std::unordered_set LoadWorldsPiecemeal( bool StepWorld(const TestWorldPtr &_world, bool _firstTime, const std::size_t _numSteps = 1) { - gz::physics::ForwardStep::Input input; - gz::physics::ForwardStep::State state; - gz::physics::ForwardStep::Output output; + physics::ForwardStep::Input input; + physics::ForwardStep::State state; + physics::ForwardStep::Output output; bool checkedOutput = false; for (size_t i = 0; i < _numSteps; ++i) @@ -250,7 +252,7 @@ bool StepWorld(const TestWorldPtr &_world, bool _firstTime, if (_firstTime && (i == 0)) { EXPECT_FALSE( - output.Get().entries.empty()); + output.Get().entries.empty()); checkedOutput = true; } } @@ -283,8 +285,8 @@ TEST_P(SimulationFeatures_TEST, StepWorld) auto link = model->GetLink(0); ASSERT_NE(nullptr, link); auto frameData = link->FrameDataRelativeToWorld(); - EXPECT_EQ(gz::math::Pose3d(0, 1.5, 0.5, 0, 0, 0), - gz::math::eigen3::convert(frameData.pose)); + EXPECT_EQ(math::Pose3d(0, 1.5, 0.5, 0, 0, 0), + math::eigen3::convert(frameData.pose)); } } @@ -315,13 +317,13 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) auto boxLink = box->GetLink(0); auto boxCollision = boxLink->GetShape(0); auto boxShape = boxCollision->CastToBoxShape(); - EXPECT_EQ(gz::math::Vector3d(100, 100, 1), - gz::math::eigen3::convert(boxShape->GetSize())); + EXPECT_EQ(math::Vector3d(100, 100, 1), + math::eigen3::convert(boxShape->GetSize())); auto box2 = boxLink->AttachBoxShape( "box2", - gz::math::eigen3::convert( - gz::math::Vector3d(1.2, 1.2, 1.2)), + math::eigen3::convert( + math::Vector3d(1.2, 1.2, 1.2)), Eigen::Isometry3d::Identity()); EXPECT_EQ(2u, boxLink->GetShapeCount()); EXPECT_EQ(box2, boxLink->GetShape(1)); @@ -343,12 +345,12 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) auto ellipsoidCollision = ellipsoidLink->GetShape(0); auto ellipsoidShape = ellipsoidCollision->CastToEllipsoidShape(); EXPECT_EQ( - gz::math::Vector3d(0.2, 0.3, 0.5), - gz::math::eigen3::convert(ellipsoidShape->GetRadii())); + math::Vector3d(0.2, 0.3, 0.5), + math::eigen3::convert(ellipsoidShape->GetRadii())); auto ellipsoid2 = ellipsoidLink->AttachEllipsoidShape( "ellipsoid2", - gz::math::eigen3::convert(gz::math::Vector3d(0.2, 0.3, 0.5)), + math::eigen3::convert(math::Vector3d(0.2, 0.3, 0.5)), Eigen::Isometry3d::Identity()); EXPECT_EQ(2u, ellipsoidLink->GetShapeCount()); EXPECT_EQ(ellipsoid2, ellipsoidLink->GetShape(1)); @@ -377,26 +379,26 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) auto capsuleAABB = capsuleCollision->GetAxisAlignedBoundingBox(*capsuleCollision); - EXPECT_EQ(gz::math::Vector3d(-1, -1, -1), - gz::math::eigen3::convert(sphereAABB).Min()); - EXPECT_EQ(gz::math::Vector3d(1, 1, 1), - gz::math::eigen3::convert(sphereAABB).Max()); - EXPECT_EQ(gz::math::Vector3d(-50, -50, -0.5), - gz::math::eigen3::convert(boxAABB).Min()); - EXPECT_EQ(gz::math::Vector3d(50, 50, 0.5), - gz::math::eigen3::convert(boxAABB).Max()); - EXPECT_EQ(gz::math::Vector3d(-0.5, -0.5, -0.55), - gz::math::eigen3::convert(cylinderAABB).Min()); - EXPECT_EQ(gz::math::Vector3d(0.5, 0.5, 0.55), - gz::math::eigen3::convert(cylinderAABB).Max()); - EXPECT_EQ(gz::math::Vector3d(-0.2, -0.3, -0.5), - gz::math::eigen3::convert(ellipsoidAABB).Min()); - EXPECT_EQ(gz::math::Vector3d(0.2, 0.3, 0.5), - gz::math::eigen3::convert(ellipsoidAABB).Max()); - EXPECT_EQ(gz::math::Vector3d(-0.2, -0.2, -0.5), - gz::math::eigen3::convert(capsuleAABB).Min()); - EXPECT_EQ(gz::math::Vector3d(0.2, 0.2, 0.5), - gz::math::eigen3::convert(capsuleAABB).Max()); + EXPECT_EQ(math::Vector3d(-1, -1, -1), + math::eigen3::convert(sphereAABB).Min()); + EXPECT_EQ(math::Vector3d(1, 1, 1), + math::eigen3::convert(sphereAABB).Max()); + EXPECT_EQ(math::Vector3d(-50, -50, -0.5), + math::eigen3::convert(boxAABB).Min()); + EXPECT_EQ(math::Vector3d(50, 50, 0.5), + math::eigen3::convert(boxAABB).Max()); + EXPECT_EQ(math::Vector3d(-0.5, -0.5, -0.55), + math::eigen3::convert(cylinderAABB).Min()); + EXPECT_EQ(math::Vector3d(0.5, 0.5, 0.55), + math::eigen3::convert(cylinderAABB).Max()); + EXPECT_EQ(math::Vector3d(-0.2, -0.3, -0.5), + math::eigen3::convert(ellipsoidAABB).Min()); + EXPECT_EQ(math::Vector3d(0.2, 0.3, 0.5), + math::eigen3::convert(ellipsoidAABB).Max()); + EXPECT_EQ(math::Vector3d(-0.2, -0.2, -0.5), + math::eigen3::convert(capsuleAABB).Min()); + EXPECT_EQ(math::Vector3d(0.2, 0.2, 0.5), + math::eigen3::convert(capsuleAABB).Max()); // check model AABB. By default, the AABBs are in world frame auto sphereModelAABB = sphere->GetAxisAlignedBoundingBox(); @@ -404,26 +406,26 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) auto cylinderModelAABB = cylinder->GetAxisAlignedBoundingBox(); auto ellipsoidModelAABB = ellipsoid->GetAxisAlignedBoundingBox(); auto capsuleModelAABB = capsule->GetAxisAlignedBoundingBox(); - EXPECT_EQ(gz::math::Vector3d(-1, 0.5, -0.5), - gz::math::eigen3::convert(sphereModelAABB).Min()); - EXPECT_EQ(gz::math::Vector3d(1, 2.5, 1.5), - gz::math::eigen3::convert(sphereModelAABB).Max()); - EXPECT_EQ(gz::math::Vector3d(-50, -50, -0.1), - gz::math::eigen3::convert(boxModelAABB).Min()); - EXPECT_EQ(gz::math::Vector3d(50, 50, 1.1), - gz::math::eigen3::convert(boxModelAABB).Max()); - EXPECT_EQ(gz::math::Vector3d(-3, -4.5, -1.5), - gz::math::eigen3::convert(cylinderModelAABB).Min()); - EXPECT_EQ(gz::math::Vector3d(3, 1.5, 2.5), - gz::math::eigen3::convert(cylinderModelAABB).Max()); - EXPECT_EQ(gz::math::Vector3d(-0.2, -5.3, 0.2), - gz::math::eigen3::convert(ellipsoidModelAABB).Min()); - EXPECT_EQ(gz::math::Vector3d(0.2, -4.7, 1.2), - gz::math::eigen3::convert(ellipsoidModelAABB).Max()); - EXPECT_EQ(gz::math::Vector3d(-0.2, -3.2, 0), - gz::math::eigen3::convert(capsuleModelAABB).Min()); - EXPECT_EQ(gz::math::Vector3d(0.2, -2.8, 1), - gz::math::eigen3::convert(capsuleModelAABB).Max()); + EXPECT_EQ(math::Vector3d(-1, 0.5, -0.5), + math::eigen3::convert(sphereModelAABB).Min()); + EXPECT_EQ(math::Vector3d(1, 2.5, 1.5), + math::eigen3::convert(sphereModelAABB).Max()); + EXPECT_EQ(math::Vector3d(-50, -50, -0.1), + math::eigen3::convert(boxModelAABB).Min()); + EXPECT_EQ(math::Vector3d(50, 50, 1.1), + math::eigen3::convert(boxModelAABB).Max()); + EXPECT_EQ(math::Vector3d(-3, -4.5, -1.5), + math::eigen3::convert(cylinderModelAABB).Min()); + EXPECT_EQ(math::Vector3d(3, 1.5, 2.5), + math::eigen3::convert(cylinderModelAABB).Max()); + EXPECT_EQ(math::Vector3d(-0.2, -5.3, 0.2), + math::eigen3::convert(ellipsoidModelAABB).Min()); + EXPECT_EQ(math::Vector3d(0.2, -4.7, 1.2), + math::eigen3::convert(ellipsoidModelAABB).Max()); + EXPECT_EQ(math::Vector3d(-0.2, -3.2, 0), + math::eigen3::convert(capsuleModelAABB).Min()); + EXPECT_EQ(math::Vector3d(0.2, -2.8, 1), + math::eigen3::convert(capsuleModelAABB).Max()); } } @@ -453,25 +455,25 @@ TEST_P(SimulationFeatures_TEST, FreeGroup) StepWorld(world, true); freeGroup->SetWorldPose( - gz::math::eigen3::convert( - gz::math::Pose3d(0, 0, 2, 0, 0, 0))); + math::eigen3::convert( + math::Pose3d(0, 0, 2, 0, 0, 0))); freeGroup->SetWorldLinearVelocity( - gz::math::eigen3::convert(gz::math::Vector3d(0.1, 0.2, 0.3))); + math::eigen3::convert(math::Vector3d(0.1, 0.2, 0.3))); freeGroup->SetWorldAngularVelocity( - gz::math::eigen3::convert(gz::math::Vector3d(0.4, 0.5, 0.6))); + math::eigen3::convert(math::Vector3d(0.4, 0.5, 0.6))); auto frameData = model->GetLink(0)->FrameDataRelativeToWorld(); - EXPECT_EQ(gz::math::Pose3d(0, 0, 2, 0, 0, 0), - gz::math::eigen3::convert(frameData.pose)); + EXPECT_EQ(math::Pose3d(0, 0, 2, 0, 0, 0), + math::eigen3::convert(frameData.pose)); // Step the world StepWorld(world, false); // Check that the first link's velocities are updated frameData = model->GetLink(0)->FrameDataRelativeToWorld(); - EXPECT_EQ(gz::math::Vector3d(0.1, 0.2, 0.3), - gz::math::eigen3::convert(frameData.linearVelocity)); - EXPECT_EQ(gz::math::Vector3d(0.4, 0.5, 0.6), - gz::math::eigen3::convert(frameData.angularVelocity)); + EXPECT_EQ(math::Vector3d(0.1, 0.2, 0.3), + math::eigen3::convert(frameData.linearVelocity)); + EXPECT_EQ(math::Vector3d(0.4, 0.5, 0.6), + math::eigen3::convert(frameData.angularVelocity)); } } @@ -496,15 +498,15 @@ TEST_P(SimulationFeatures_TEST, NestedFreeGroup) ASSERT_NE(nullptr, freeGroup); ASSERT_NE(nullptr, freeGroup->RootLink()); - gz::math::Pose3d newPose(1, 1, 0, 0, 0, 0); - freeGroup->SetWorldPose(gz::math::eigen3::convert(newPose)); + math::Pose3d newPose(1, 1, 0, 0, 0, 0); + freeGroup->SetWorldPose(math::eigen3::convert(newPose)); { auto link = model->GetLink("link"); ASSERT_NE(nullptr, link); auto frameData = link->FrameDataRelativeToWorld(); EXPECT_EQ(newPose, - gz::math::eigen3::convert(frameData.pose)); + math::eigen3::convert(frameData.pose)); } { auto nestedModel = model->GetNestedModel("nested_model"); @@ -513,14 +515,14 @@ TEST_P(SimulationFeatures_TEST, NestedFreeGroup) ASSERT_NE(nullptr, nestedLink); // Poses from SDF - gz::math::Pose3d nestedModelPose(1, 2, 2, 0, 0, 0); - gz::math::Pose3d nestedLinkPose(3, 1, 1, 0, 0, GZ_PI_2); + math::Pose3d nestedModelPose(1, 2, 2, 0, 0, 0); + math::Pose3d nestedLinkPose(3, 1, 1, 0, 0, GZ_PI_2); - gz::math::Pose3d nestedLinkExpectedPose = + math::Pose3d nestedLinkExpectedPose = newPose * nestedModelPose * nestedLinkPose; EXPECT_EQ(nestedLinkExpectedPose, - gz::math::eigen3::convert( + math::eigen3::convert( nestedLink->FrameDataRelativeToWorld().pose)); } } @@ -627,7 +629,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) { contactBoxSphere++; Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0, 1.5, 0.5); - EXPECT_TRUE(gz::physics::test::Equal(expectedContactPos, + EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } else if ((m1->GetName() == "box" && m2->GetName() == "cylinder") || @@ -635,7 +637,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) { contactBoxCylinder++; Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0, -1.5, 0.5); - EXPECT_TRUE(gz::physics::test::Equal(expectedContactPos, + EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } else if ((m1->GetName() == "box" && m2->GetName() == "capsule") || @@ -643,7 +645,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) { contactBoxCapsule++; Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -3, 0.5); - EXPECT_TRUE(gz::physics::test::Equal(expectedContactPos, + EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } else if ((m1->GetName() == "box" && m2->GetName() == "ellipsoid") || @@ -651,7 +653,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) { contactBoxEllipsoid++; Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -5, 0.6); - EXPECT_TRUE(gz::physics::test::Equal(expectedContactPos, + EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } else @@ -666,8 +668,8 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_EQ(1u, contactBoxEllipsoid); // move sphere away - sphereFreeGroup->SetWorldPose(gz::math::eigen3::convert( - gz::math::Pose3d(0, 100, 0.5, 0, 0, 0))); + sphereFreeGroup->SetWorldPose(math::eigen3::convert( + math::Pose3d(0, 100, 0.5, 0, 0, 0))); // step and get contacts checkedOutput = StepWorld(world, false); @@ -697,7 +699,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) { contactBoxCylinder++; Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0, -1.5, 0.5); - EXPECT_TRUE(gz::physics::test::Equal(expectedContactPos, + EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } else if ((m1->GetName() == "box" && m2->GetName() == "capsule") || @@ -705,7 +707,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) { contactBoxCapsule++; Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -3, 0.5); - EXPECT_TRUE(gz::physics::test::Equal(expectedContactPos, + EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } else if ((m1->GetName() == "box" && m2->GetName() == "ellipsoid") || @@ -713,7 +715,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) { contactBoxEllipsoid++; Eigen::Vector3d expectedContactPos = Eigen::Vector3d(0.0, -5, 0.6); - EXPECT_TRUE(gz::physics::test::Equal(expectedContactPos, + EXPECT_TRUE(physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); } else @@ -726,16 +728,16 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_EQ(1u, contactBoxEllipsoid); // move cylinder away - cylinderFreeGroup->SetWorldPose(gz::math::eigen3::convert( - gz::math::Pose3d(0, -100, 0.5, 0, 0, 0))); + cylinderFreeGroup->SetWorldPose(math::eigen3::convert( + math::Pose3d(0, -100, 0.5, 0, 0, 0))); // move capsule away - capsuleFreeGroup->SetWorldPose(gz::math::eigen3::convert( - gz::math::Pose3d(0, -100, 100, 0, 0, 0))); + capsuleFreeGroup->SetWorldPose(math::eigen3::convert( + math::Pose3d(0, -100, 100, 0, 0, 0))); // move ellipsoid away - ellipsoidFreeGroup->SetWorldPose(gz::math::eigen3::convert( - gz::math::Pose3d(0, -100, -100, 0, 0, 0))); + ellipsoidFreeGroup->SetWorldPose(math::eigen3::convert( + math::Pose3d(0, -100, -100, 0, 0, 0))); // step and get contacts checkedOutput = StepWorld(world, false); @@ -748,4 +750,4 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) } INSTANTIATE_TEST_SUITE_P(PhysicsPlugins, SimulationFeatures_TEST, - ::testing::ValuesIn(gz::physics::test::g_PhysicsPluginLibraries)); + ::testing::ValuesIn(physics::test::g_PhysicsPluginLibraries));