From 128f74066fbad7158927e45454e6741785fbafa5 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 23 Oct 2023 16:33:39 -0700 Subject: [PATCH 1/6] Use correct link indicies when constructing fixed constraints (#530) Signed-off-by: Michael Carroll --- bullet-featherstone/src/JointFeatures.cc | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 7a27f997c..2081025e9 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -303,11 +303,11 @@ Identity JointFeatures::AttachFixedJoint( const BaseLink3dPtr &_parent, const std::string &_name) { - auto linkInfo = this->ReferenceInterface(_childID); - auto modelInfo = this->ReferenceInterface(linkInfo->model); - auto parentLinkInfo = this->ReferenceInterface( + auto *linkInfo = this->ReferenceInterface(_childID); + auto *modelInfo = this->ReferenceInterface(linkInfo->model); + auto *parentLinkInfo = this->ReferenceInterface( _parent->FullIdentity()); - auto parentModelInfo = this->ReferenceInterface( + auto *parentModelInfo = this->ReferenceInterface( parentLinkInfo->model); auto *world = this->ReferenceInterface(modelInfo->world); @@ -322,16 +322,18 @@ Identity JointFeatures::AttachFixedJoint( linkInfo->model }); - auto jointInfo = this->ReferenceInterface(jointID); + auto parentLinkIdx = parentLinkInfo->indexInModel.value_or(-1); + auto childLinkIdx = linkInfo->indexInModel.value_or(-1); + auto *jointInfo = this->ReferenceInterface(jointID); jointInfo->fixedConstraint = std::make_shared( - parentModelInfo->body.get(), -1, - modelInfo->body.get(), -1, + parentModelInfo->body.get(), parentLinkIdx, + modelInfo->body.get(), childLinkIdx, btVector3(0, 0, 0), btVector3(0, 0, 0), btMatrix3x3::getIdentity(), btMatrix3x3::getIdentity()); - if (world && world->world) + if (world != nullptr && world->world) { world->world->addMultiBodyConstraint(jointInfo->fixedConstraint.get()); return this->GenerateIdentity(jointID, this->joints.at(jointID)); From b83354dc18c7d72f0c37c9056c83fb2f32bc0ba6 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 30 Oct 2023 11:49:48 -0700 Subject: [PATCH 2/6] Add a test to verify behavior of detachable joints (#563) The reported bug was that the detachable joint was not correctly linked between objects' canonical links with bullet-featherstone. While this was addressed and confirmed working in #530, this adds a test to prevent future regression (or for future physics implementations) Signed-off-by: Michael Carroll --- test/common_test/CMakeLists.txt | 1 + test/common_test/detachable_joint.cc | 202 ++++++++++++++++++ .../common_test/worlds/detachable_joint.world | 138 ++++++++++++ 3 files changed, 341 insertions(+) create mode 100644 test/common_test/detachable_joint.cc create mode 100644 test/common_test/worlds/detachable_joint.world diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 2dbfe6ab2..2e38a0c25 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -7,6 +7,7 @@ set(tests basic_test collisions construct_empty_world + detachable_joint free_joint_features joint_features joint_transmitted_wrench_features diff --git a/test/common_test/detachable_joint.cc b/test/common_test/detachable_joint.cc new file mode 100644 index 000000000..ab06e6bb9 --- /dev/null +++ b/test/common_test/detachable_joint.cc @@ -0,0 +1,202 @@ +/* + * 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 "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 DetachableJointTest: + public testing::Test, public gz::physics::TestLibLoader +{ + // Documentation inherited + public: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + + std::cerr << "DetachableJointTest::GetLibToTest() " << DetachableJointTest::GetLibToTest() << '\n'; + + loader.LoadLib(DetachableJointTest::GetLibToTest()); + + 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 DetachableJointFeatureList: 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::AttachFixedJointFeature, + gz::physics::DetachJointFeature, + gz::physics::SetJointTransformFromParentFeature, + gz::physics::sdf::ConstructSdfWorld +> { }; + +using DetachableJointTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(DetachableJointTest, + DetachableJointTestTypes); + +TYPED_TEST(DetachableJointTest, CorrectAttachmentPoints) +{ + 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, "detachable_joint.world")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + ASSERT_NE(nullptr, world); + + auto cylinder1 = world->GetModel("cylinder1"); + ASSERT_NE(nullptr, cylinder1); + auto cylinder1_base_link = cylinder1->GetLink("base_link"); + ASSERT_NE(nullptr, cylinder1_base_link); + auto cylinder1_link1 = cylinder1->GetLink("link1"); + ASSERT_NE(nullptr, cylinder1_link1); + + auto cylinder2 = world->GetModel("cylinder2"); + ASSERT_NE(nullptr, cylinder2); + auto cylinder2_link2 = cylinder2->GetLink("link2"); + ASSERT_NE(nullptr, cylinder2_link2); + + // Create a detachable joint + const auto poseParent = + cylinder1_link1->FrameDataRelativeToWorld().pose; + const auto poseChild = + cylinder2_link2->FrameDataRelativeToWorld().pose; + const auto poseParentChild = poseParent.inverse() * poseChild; + auto jointPtrPhys = + cylinder2_link2->AttachFixedJoint(cylinder1_link1); + ASSERT_NE(nullptr, jointPtrPhys); + jointPtrPhys->SetTransformFromParent(poseParentChild); + + { + // Check initial conditions + // Cylinder 1 is fixed 0.5m above cylinder 2, which is 1.5m above the + // ground. + auto frameDataC1L1 = cylinder1_link1->FrameDataRelativeToWorld(); + auto frameDataC2L2 = cylinder2_link2->FrameDataRelativeToWorld(); + EXPECT_EQ(gz::math::Pose3d(0, 0, 2, 0, 0, 0), + gz::math::eigen3::convert(frameDataC1L1.pose)); + EXPECT_EQ(gz::math::Pose3d(0, 0, 1.5, 0, 0, 0), + gz::math::eigen3::convert(frameDataC2L2.pose)); + } + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + for (std::size_t i = 0; i < 1000 ; ++i) + { + // step forward and expect lower link to fall + world->Step(output, state, input); + } + + { + // Check final conditions with joint attached + // If the joint was attached correctly, the top cylinder should be + // fixed 0.5m above the bottom cylinder, which is resting on the ground. + auto frameDataC1L1 = cylinder1_link1->FrameDataRelativeToWorld(); + auto frameDataC2L2 = cylinder2_link2->FrameDataRelativeToWorld(); + EXPECT_NEAR(0.55, frameDataC1L1.pose.translation().z(), 1e-2); + EXPECT_NEAR(0.05, frameDataC2L2.pose.translation().z(), 1e-2); + } + + // Detach joint and step physics + jointPtrPhys->Detach(); + for (std::size_t i = 0; i < 1000; ++i) + { + world->Step(output, state, input); + } + + { + // Check final conditions after joint detached + // If the joint was detached correctly, the top cylinder should be + // resting directly on the bottom cylinder, which is resting on the + // ground. + auto frameDataC1L1 = cylinder1_link1->FrameDataRelativeToWorld(); + auto frameDataC2L2 = cylinder2_link2->FrameDataRelativeToWorld(); + EXPECT_NEAR(0.15, frameDataC1L1.pose.translation().z(), 1e-2); + EXPECT_NEAR(0.05, frameDataC2L2.pose.translation().z(), 1e-2); + } + } +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + if (!DetachableJointTest::init( + argc, argv)) + return -1; + return RUN_ALL_TESTS(); +} diff --git a/test/common_test/worlds/detachable_joint.world b/test/common_test/worlds/detachable_joint.world new file mode 100644 index 000000000..b7e4c5310 --- /dev/null +++ b/test/common_test/worlds/detachable_joint.world @@ -0,0 +1,138 @@ + + + + + gz-physics-bullet_featherstone-plugin + + + + + true + + + + + 0 0 1 + 2 2 + + + + + + + + + world + base_link + + 0 0.0 2.0 0 0 0 + + + 1.0 + + 0.0068 + 0 + 0 + 0.0068 + 0 + 0.0032 + + + + + base_link + link1 + + 0 0 1 + + + + + 1.0 + + 0.0068 + 0 + 0 + 0.0068 + 0 + 0.0032 + + + + + + 0.2 + 0.1 + + + + + + + 0.2 + 0.1 + + + + 0.6 0.6 0.9 1 + 0.6 0.6 0.9 1 + 1.0 1.0 1.0 1 + + + + + + + link1 + cylinder2 + link2 + + + + + 0 0.0 1.5 0 0 0 + + + 1.0 + + 0.0068 + 0 + 0 + 0.0068 + 0 + 0.0032 + + + + + + 0.2 + 0.1 + + + + + + + 0.2 + 0.1 + + + + 0.9 0.6 0.2 1 + 0.9 0.6 0.2 1 + 1.0 1.0 1.0 1 + + + + + + + From 3b7a7c8ce4213378aef160808e58432449631458 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 3 Nov 2023 16:47:12 -0700 Subject: [PATCH 3/6] Add sample ctest cmds to tutorial (#566) Signed-off-by: Ian Chen --- tutorials/02_installation.md | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/tutorials/02_installation.md b/tutorials/02_installation.md index 6c724d0f8..10622bf0f 100644 --- a/tutorials/02_installation.md +++ b/tutorials/02_installation.md @@ -227,6 +227,22 @@ Follow these steps to run tests and static code analysis in your clone of this r make test ``` + Each test is registered with `ctest`. These can then be filtered with the `ctest` command line. + + ``` + # See a list of all available tests + ctest -N + + # Run all tests in dartsim (verbose) + ctest -R dartsim -V + + # Run all INTEGRATION tests (verbose) + ctest -R INTEGRATION -V + + # Run all COMMON tests (verbose) + ctest -R COMMON -V + ``` + 3. You will need Cppcheck in order to run static code checks. On Ubuntu Cppcheck can be installed using ``` sudo apt-get install cppcheck From 1260ef275d96ae4f9f360c80a8bf810fc7ca121f Mon Sep 17 00:00:00 2001 From: shameekganguly Date: Tue, 7 Nov 2023 19:34:34 -0800 Subject: [PATCH 4/6] bullet-featherstoneFix how links are flattened in ConstructSdfModel (#562) * Add test to show seg-fault with unsorted links Signed-off-by: Shameek Ganguly Signed-off-by: Steve Peters Signed-off-by: Ian Chen Co-authored-by: Steve Peters Co-authored-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 87 +++++++++---------- test/common_test/world_features.cc | 75 ++++++++++++---- .../worlds/world_unsorted_links.sdf | 36 ++++++++ 3 files changed, 134 insertions(+), 64 deletions(-) create mode 100644 test/common_test/worlds/world_unsorted_links.sdf diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 5f63e5694..1ddce408d 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -135,9 +135,12 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) const ::sdf::Joint *rootJoint = nullptr; bool fixed = false; const std::string &rootModelName = _sdfModel.Name(); + // a map of parent link to a vector of its child links + std::unordered_map> + linkTree; const auto checkModel = - [&rootLink, &rootJoint, &fixed, &parentOf, &rootModelName]( + [&rootLink, &rootJoint, &fixed, &parentOf, &rootModelName, &linkTree]( const ::sdf::Model &model) -> bool { for (std::size_t i = 0; i < model.JointCount(); ++i) @@ -217,6 +220,10 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) << rootModelName << "] has multiple parent joints. That is not " << "supported by the gz-physics-bullet-featherstone plugin.\n"; } + if (parent != nullptr) + { + linkTree[parent].push_back(child); + } } return true; @@ -231,11 +238,10 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) return std::nullopt; } - std::vector flatLinks; - std::unordered_map linkIndex; - const auto flattenLinks = - [&rootLink, &parentOf, &rootModelName, &flatLinks, &linkIndex]( - const ::sdf::Model &model) -> bool + // Find root link in model and verify that there is only one root link in + // the model. Returns false if more than one root link is found + const auto findRootLink = + [&rootLink, &parentOf, &rootModelName](const ::sdf::Model &model) -> bool { for (std::size_t i = 0; i < model.LinkCount(); ++i) { @@ -254,23 +260,35 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) } rootLink = link; - continue; } - - linkIndex[link] = linkIndex.size(); - flatLinks.push_back(link); } return true; }; - if (!flattenLinks(_sdfModel)) + if (rootLink == nullptr && !findRootLink(_sdfModel)) + { + // No root link was found in this model return std::nullopt; + } + // find root link in nested models if one was not already found for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i) { - if (!flattenLinks(*_sdfModel.ModelByIndex(i))) + if (rootLink != nullptr) + { + break; + } + if (!findRootLink(*_sdfModel.ModelByIndex(i))) + { return std::nullopt; + } + } + + if (!rootLink) + { + gzerr << "No root link was found for model [" << _sdfModel.Name() << "\n"; + return std::nullopt; } // The documentation for bullet does not mention whether parent links must @@ -280,44 +298,23 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) // come before their children in the array, and do not work correctly if that // ordering is not held. Out of an abundance of caution we will assume that // ordering is necessary. - for (std::size_t i = 0; i < flatLinks.size(); ++i) + std::vector flatLinks; + std::function flattenLinkTree = + [&](const ::sdf::Link *link) { - // Every element in flatLinks should have a parent if the earlier validation - // was done correctly. - if (parentOf.size() == 0) + if (link != rootLink) { - break; + flatLinks.push_back(link); } - const auto *parentJoint = parentOf.at(flatLinks[i]).joint; - - const auto *parentLink = - _sdfModel.LinkByName(parentJoint->ParentName()); - const auto p_index_it = linkIndex.find(parentLink); - if (p_index_it == linkIndex.end()) + if (auto it = linkTree.find(link); it != linkTree.end()) { - // If the parent index cannot be found, that must mean the parent is the - // root link, so this link can go anywhere in the list as long as it is - // before its own children. - assert(parentLink == rootLink); - continue; - } - - auto &p_index = p_index_it->second; - if (i < p_index) - { - // The current link is in front of its parent link in the array. We must - // swap their places. - std::swap(flatLinks[i], flatLinks[p_index]); - p_index = i; - linkIndex[flatLinks[p_index]] = p_index; + for (const auto &child_link : it->second) + { + flattenLinkTree(child_link); + } } - } - - if (!rootLink) - { - gzerr << "No root link was found for model [" << _sdfModel.Name() << "\n"; - return std::nullopt; - } + }; + flattenLinkTree(rootLink); const auto &M = rootLink->Inertial().MassMatrix(); const auto mass = static_cast(M.Mass()); diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index b9d7d0011..6c08ddb98 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -67,7 +67,7 @@ class WorldFeaturesTest: public: gz::plugin::Loader loader; }; -using GravityFeatures = gz::physics::FeatureList< +struct GravityFeatures : gz::physics::FeatureList< gz::physics::GetEngineInfo, gz::physics::Gravity, gz::physics::sdf::ConstructSdfWorld, @@ -75,15 +75,12 @@ using GravityFeatures = gz::physics::FeatureList< gz::physics::GetModelFromWorld, gz::physics::GetLinkFromModel, gz::physics::ForwardStep ->; +> { }; -using GravityFeaturesTestTypes = - ::testing::Types; -TYPED_TEST_SUITE(WorldFeaturesTest, - GravityFeatures,); +using WorldFeaturesTestGravity = WorldFeaturesTest; ///////////////////////////////////////////////// -TYPED_TEST(WorldFeaturesTest, GravityFeatures) +TEST_F(WorldFeaturesTestGravity, GravityFeatures) { for (const std::string &name : this->pluginNames) { @@ -182,19 +179,59 @@ TYPED_TEST(WorldFeaturesTest, GravityFeatures) } } -struct WorldModelFeatureList - : gz::physics::FeatureList +struct ConstructModelFeatures : gz::physics::FeatureList< + gz::physics::GetEngineInfo, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::sdf::ConstructSdfModel, + gz::physics::GetModelFromWorld, + gz::physics::ForwardStep +> { }; + +using WorldFeaturesTestConstructModel = + WorldFeaturesTest; + +///////////////////////////////////////////////// +TEST_F(WorldFeaturesTestConstructModel, ConstructModelUnsortedLinks) { -}; + 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, "world_unsorted_links.sdf")); + EXPECT_TRUE(errors.empty()) << errors; + const sdf::World *sdfWorld = root.WorldByIndex(0); + ASSERT_NE(nullptr, sdfWorld); + + // Per https://github.com/gazebosim/gz-physics/pull/562, there is a + // seg-fault in the bullet-featherstone implementation of ConstructSdfModel + // (called by ConstructSdfWorld). So loading the world successfully + // is enough for this test. + auto world = engine->ConstructWorld(*sdfWorld); + EXPECT_NE(nullptr, world); + } +} + +struct WorldModelFeatureList : gz::physics::FeatureList< + GravityFeatures, + gz::physics::WorldModelFeature, + gz::physics::RemoveEntities, + gz::physics::GetNestedModelFromModel, + gz::physics::sdf::ConstructSdfLink, + gz::physics::sdf::ConstructSdfJoint, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfNestedModel, + gz::physics::ConstructEmptyLinkFeature, + gz::physics::ConstructEmptyNestedModelFeature +> { }; class WorldModelTest : public WorldFeaturesTest { diff --git a/test/common_test/worlds/world_unsorted_links.sdf b/test/common_test/worlds/world_unsorted_links.sdf new file mode 100644 index 000000000..fb2cd9c49 --- /dev/null +++ b/test/common_test/worlds/world_unsorted_links.sdf @@ -0,0 +1,36 @@ + + + + + + + + + + world + parent_link + + + parent_link + child_linkA + + 0 0 1 + + + + child_linkA + child_linkB + + 0 0 1 + + + + child_linkB + child_linkC + + 0 0 1 + + + + + From cf74036edb75605498e4f505ffbdfebf6b6f2999 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 9 Nov 2023 10:19:23 -0800 Subject: [PATCH 5/6] bullet-featherstone: support off-diagonal inertia (#544) The bullet APIs expect the moment of inertia matrix to already be diagonalized. This changes the bullet-featherstone plugin to compute the principal moments of inertia in the same manner as the bullet plugin. It will now load models with off-diagonal inertia values without giving errors. Signed-off-by: Steve Peters --- bullet-featherstone/src/SDFFeatures.cc | 85 ++++++++++---------------- 1 file changed, 33 insertions(+), 52 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 1ddce408d..01854690e 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -116,6 +116,7 @@ struct Structure const ::sdf::Joint *rootJoint; btScalar mass; btVector3 inertia; + math::Pose3d linkToPrincipalAxesPose; /// Is the root link fixed bool fixedBase; @@ -127,6 +128,20 @@ struct Structure std::vector flatLinks; }; +///////////////////////////////////////////////// +void extractInertial( + const math::Inertiald &_inertial, + btScalar &_mass, + btVector3 &_principalInertiaMoments, + math::Pose3d &_linkToPrincipalAxesPose) +{ + const auto &M = _inertial.MassMatrix(); + _mass = static_cast(M.Mass()); + _principalInertiaMoments = convertVec(M.PrincipalMoments()); + _linkToPrincipalAxesPose = _inertial.Pose(); + _linkToPrincipalAxesPose.Rot() *= M.PrincipalAxesOffset(); +} + ///////////////////////////////////////////////// std::optional ValidateModel(const ::sdf::Model &_sdfModel) { @@ -316,23 +331,14 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) }; flattenLinkTree(rootLink); - const auto &M = rootLink->Inertial().MassMatrix(); - 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) - { - gzerr << "The base link of the model is required to have a diagonal " - << "inertia matrix by gz-physics-bullet-featherstone, but the " - << "Model [" << _sdfModel.Name() << "] has a non-zero diagonal " - << "value: " << I << "\n"; - return std::nullopt; - } - } + btScalar mass; + btVector3 inertia; + math::Pose3d linkToPrincipalAxesPose; + extractInertial(rootLink->Inertial(), mass, inertia, linkToPrincipalAxesPose); return Structure{ - rootLink, rootJoint, mass, inertia, fixed, parentOf, flatLinks}; + rootLink, rootJoint, mass, inertia, linkToPrincipalAxesPose, fixed, + parentOf, flatLinks}; } ///////////////////////////////////////////////// @@ -350,7 +356,7 @@ Identity SDFFeatures::ConstructSdfModel( const auto *world = this->ReferenceInterface(_worldID); const auto rootInertialToLink = - gz::math::eigen3::convert(structure.rootLink->Inertial().Pose()).inverse(); + gz::math::eigen3::convert(structure.linkToPrincipalAxesPose).inverse(); const auto modelID = this->AddModel( _sdfModel.Name(), _worldID, rootInertialToLink, std::make_unique( @@ -389,8 +395,12 @@ Identity SDFFeatures::ConstructSdfModel( for (int i = 0; i < static_cast(structure.flatLinks.size()); ++i) { const auto *link = structure.flatLinks[static_cast(i)]; + btScalar mass; + btVector3 inertia; + math::Pose3d linkToPrincipalAxesPose; + extractInertial(link->Inertial(), mass, inertia, linkToPrincipalAxesPose); const Eigen::Isometry3d linkToComTf = gz::math::eigen3::convert( - link->Inertial().Pose()); + linkToPrincipalAxesPose); if (linkIDs.find(link) == linkIDs.end()) { @@ -399,22 +409,6 @@ Identity SDFFeatures::ConstructSdfModel( linkIDs.insert(std::make_pair(link, linkID)); } - const auto &M = link->Inertial().MassMatrix(); - const btScalar mass = static_cast(M.Mass()); - const auto inertia = btVector3(convertVec(M.DiagonalMoments())); - - for (const double I : {M.Ixy(), M.Ixz(), M.Iyz()}) - { - if (std::abs(I) > 1e-3) - { - gzerr << "Links are required to have a diagonal inertia matrix in " - << "gz-physics-bullet-featherstone, but Link [" << link->Name() - << "] in Model [" << model->name << "] has a non-zero off " - << "diagonal value in its inertia matrix\n"; - return this->GenerateInvalidId(); - } - } - if (structure.parentOf.size()) { const auto &parentInfo = structure.parentOf.at(link); @@ -614,25 +608,12 @@ Identity SDFFeatures::ConstructSdfModel( { const auto *link = structure.rootLink; - const auto &M = link->Inertial().MassMatrix(); - const btScalar mass = static_cast(M.Mass()); - const auto inertia = convertVec(M.DiagonalMoments()); - - for (const double I : {M.Ixy(), M.Ixz(), M.Iyz()}) - { - if (std::abs(I) > 1e-3) - { - gzerr << "Links are required to have a diagonal inertia matrix in " - << "gz-physics-bullet-featherstone, but Link [" << link->Name() - << "] in Model [" << model->name << "] has a non-zero off " - << "diagonal value in its inertia matrix\n"; - } - else - { - model->body->setBaseMass(mass); - model->body->setBaseInertia(inertia); - } - } + btScalar mass; + btVector3 inertia; + math::Pose3d linkToPrincipalAxesPose; + extractInertial(link->Inertial(), mass, inertia, linkToPrincipalAxesPose); + model->body->setBaseMass(mass); + model->body->setBaseInertia(inertia); } world->world->addMultiBody(model->body.get()); From 5e58f888ad50c31400b6a421a65133cf8ee568b0 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 9 Nov 2023 12:04:49 -0800 Subject: [PATCH 6/6] bullet-featherstone: fix setting angular velocity (#567) This fixes the SetFreeGroupWorldAngularVelocity implementation in bullet-featherstone and updates the FreeGroup test in the simulation_features common test with the following changes: * Use FeatureList struct for faster build * Use sphere.sdf instead of shapes.world to avoid initial contact and gravity. * Verify free group pose and velocities in addition to link values. * Use tighter tolerances for velocity expectations. Signed-off-by: Steve Peters --- bullet-featherstone/src/FreeGroupFeatures.cc | 24 +--- bullet-featherstone/src/KinematicsFeatures.cc | 2 +- test/common_test/simulation_features.cc | 131 +++++++++++++----- 3 files changed, 97 insertions(+), 60 deletions(-) diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index 5fd18e9f3..c557d6465 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -68,29 +68,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( if (model != nullptr) { - // Set angular velocity the each one of the joints of the model - for (const auto& jointID : model->jointEntityIds) - { - auto jointInfo = this->joints[jointID]; - if (!jointInfo->motor) - { - auto *modelInfo = this->ReferenceInterface(jointInfo->model); - jointInfo->motor = std::make_shared( - modelInfo->body.get(), - std::get(jointInfo->identifier).indexInBtModel, - 0, - static_cast(0), - static_cast(jointInfo->effort)); - auto *world = this->ReferenceInterface(modelInfo->world); - world->world->addMultiBodyConstraint(jointInfo->motor.get()); - } - - if (jointInfo->motor) - { - jointInfo->motor->setVelocityTarget( - static_cast(_angularVelocity[2])); - } - } + model->body->setBaseOmega(convertVec(_angularVelocity)); } } diff --git a/bullet-featherstone/src/KinematicsFeatures.cc b/bullet-featherstone/src/KinematicsFeatures.cc index 74f16776d..85341ce6a 100644 --- a/bullet-featherstone/src/KinematicsFeatures.cc +++ b/bullet-featherstone/src/KinematicsFeatures.cc @@ -104,7 +104,7 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( } } - if (model->body == nullptr) + if (!model || model->body == nullptr) model = this->FrameInterface(_id); } diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 1087e2d5c..44eb69456 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -52,7 +52,7 @@ #include // The features that an engine must have to be loaded by this loader. -using Features = gz::physics::FeatureList< +struct Features : gz::physics::FeatureList< gz::physics::ConstructEmptyWorldFeature, gz::physics::FindFreeGroupFeature, @@ -84,7 +84,7 @@ using Features = gz::physics::FeatureList< gz::physics::GetCylinderShapeProperties, gz::physics::GetCapsuleShapeProperties, gz::physics::GetEllipsoidShapeProperties ->; +> {}; template class SimulationFeaturesTest: @@ -139,6 +139,8 @@ gz::physics::World3dPtr LoadPluginAndWorld( return world; } +using AssertVectorApprox = gz::physics::test::AssertVectorApprox; + /// \brief Step forward in a world /// \param[in] _world The world to step in /// \param[in] _firstTime Whether this is the very first time this world is @@ -184,11 +186,11 @@ std::pair StepWorld( } // The features that an engine must have to be loaded by this loader. -using FeaturesContacts = gz::physics::FeatureList< +struct FeaturesContacts : gz::physics::FeatureList< gz::physics::sdf::ConstructSdfWorld, gz::physics::GetContactsFromLastStepFeature, gz::physics::ForwardStep ->; +> {}; template class SimulationFeaturesContactsTest : @@ -218,10 +220,10 @@ TYPED_TEST(SimulationFeaturesContactsTest, Contacts) // The features that an engine must have to be loaded by this loader. -using FeaturesStep = gz::physics::FeatureList< +struct FeaturesStep : gz::physics::FeatureList< gz::physics::sdf::ConstructSdfWorld, gz::physics::ForwardStep ->; +> {}; template class SimulationFeaturesStepTest : @@ -303,7 +305,7 @@ TYPED_TEST(SimulationFeaturesFallingTest, Falling) // The features that an engine must have to be loaded by this loader. -using FeaturesShapeFeatures = gz::physics::FeatureList< +struct FeaturesShapeFeatures : gz::physics::FeatureList< gz::physics::sdf::ConstructSdfWorld, gz::physics::GetModelFromWorld, gz::physics::GetLinkFromModel, @@ -321,7 +323,7 @@ using FeaturesShapeFeatures = gz::physics::FeatureList< gz::physics::GetCylinderShapeProperties, gz::physics::GetCapsuleShapeProperties, gz::physics::GetEllipsoidShapeProperties ->; +> {}; template class SimulationFeaturesShapeFeaturesTest : @@ -490,22 +492,38 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) } } +struct FreeGroupFeatures : gz::physics::FeatureList< + gz::physics::FindFreeGroupFeature, + gz::physics::SetFreeGroupWorldPose, + gz::physics::SetFreeGroupWorldVelocity, + + gz::physics::GetModelFromWorld, + gz::physics::GetLinkFromModel, + + gz::physics::FreeGroupFrameSemantics, + gz::physics::LinkFrameSemantics, + + gz::physics::sdf::ConstructSdfWorld, + + gz::physics::ForwardStep +> {}; + template -class SimulationFeaturesTestBasic : +class SimulationFeaturesTestFreeGroup : public SimulationFeaturesTest{}; -using SimulationFeaturesTestBasicTypes = - ::testing::Types; -TYPED_TEST_SUITE(SimulationFeaturesTestBasic, - SimulationFeaturesTestBasicTypes); +using SimulationFeaturesTestFreeGroupTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesTestFreeGroup, + SimulationFeaturesTestFreeGroupTypes); -TYPED_TEST(SimulationFeaturesTestBasic, FreeGroup) +TYPED_TEST(SimulationFeaturesTestFreeGroup, FreeGroup) { for (const std::string &name : this->pluginNames) { - auto world = LoadPluginAndWorld( + auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); + gz::common::joinPaths(TEST_WORLD_DIR, "sphere.sdf")); // model free group test auto model = world->GetModel("sphere"); @@ -520,31 +538,72 @@ TYPED_TEST(SimulationFeaturesTestBasic, FreeGroup) auto freeGroupLink = link->FindFreeGroup(); ASSERT_NE(nullptr, freeGroupLink); - StepWorld(world, true); + StepWorld(world, true); + // Set initial pose. + const gz::math::Pose3d initialPose{0, 0, 2, 0, 0, 0}; freeGroup->SetWorldPose( - gz::math::eigen3::convert( - gz::math::Pose3d(0, 0, 2, 0, 0, 0))); - freeGroup->SetWorldLinearVelocity( - gz::math::eigen3::convert(gz::math::Vector3d(0.1, 0.2, 0.3))); - freeGroup->SetWorldAngularVelocity( - gz::math::eigen3::convert(gz::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)); + gz::math::eigen3::convert(initialPose)); + + // Set initial velocities. + const Eigen::Vector3d initialLinearVelocity{0.1, 0.2, 0.3}; + const Eigen::Vector3d initialAngularVelocity{0.4, 0.5, 0.6}; + freeGroup->SetWorldLinearVelocity(initialLinearVelocity); + freeGroup->SetWorldAngularVelocity(initialAngularVelocity); + + auto freeGroupFrameData = freeGroup->FrameDataRelativeToWorld(); + auto linkFrameData = model->GetLink(0)->FrameDataRelativeToWorld(); + + // Before stepping, check that pose matches what was set. + EXPECT_EQ(initialPose, + gz::math::eigen3::convert(freeGroupFrameData.pose)); + EXPECT_EQ(initialPose, + gz::math::eigen3::convert(linkFrameData.pose)); + + // Before stepping, check that velocities match what was set. + AssertVectorApprox vectorPredicateVelocity(1e-7); + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + initialLinearVelocity, + freeGroupFrameData.linearVelocity); + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + initialLinearVelocity, + linkFrameData.linearVelocity); + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + initialAngularVelocity, + freeGroupFrameData.angularVelocity); + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + initialAngularVelocity, + linkFrameData.angularVelocity); // Step the world - StepWorld(world, false); - // Check that the first link's velocities are updated - frameData = model->GetLink(0)->FrameDataRelativeToWorld(); - EXPECT_TRUE(gz::math::Vector3d(0.1, 0.2, 0.3).Equal( - gz::math::eigen3::convert(frameData.linearVelocity), 0.1)); - EXPECT_EQ(gz::math::Vector3d(0.4, 0.5, 0.6), - gz::math::eigen3::convert(frameData.angularVelocity)); + StepWorld(world, false); + + // Check the velocities again. + freeGroupFrameData = freeGroup->FrameDataRelativeToWorld(); + linkFrameData = model->GetLink(0)->FrameDataRelativeToWorld(); + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + initialLinearVelocity, + freeGroupFrameData.linearVelocity); + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + initialLinearVelocity, + linkFrameData.linearVelocity); + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + initialAngularVelocity, + freeGroupFrameData.angularVelocity); + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + initialAngularVelocity, + linkFrameData.angularVelocity); } } +template +class SimulationFeaturesTestBasic : + public SimulationFeaturesTest{}; +using SimulationFeaturesTestBasicTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesTestBasic, + SimulationFeaturesTestBasicTypes); + TYPED_TEST(SimulationFeaturesTestBasic, ShapeBoundingBox) { for (const std::string &name : this->pluginNames) @@ -796,7 +855,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) } } -using FeaturesContactPropertiesCallback = gz::physics::FeatureList< +struct FeaturesContactPropertiesCallback : gz::physics::FeatureList< gz::physics::ConstructEmptyWorldFeature, gz::physics::FindFreeGroupFeature, @@ -833,7 +892,7 @@ using FeaturesContactPropertiesCallback = gz::physics::FeatureList< gz::physics::GetCylinderShapeProperties, gz::physics::GetCapsuleShapeProperties, gz::physics::GetEllipsoidShapeProperties ->; +> {}; #ifdef DART_HAS_CONTACT_SURFACE using ContactSurfaceParams =