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/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index c418f12e6..1dcd1783f 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 { @@ -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) @@ -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"; @@ -684,6 +661,23 @@ Identity SDFFeatures::ConstructSdfLink( 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)) 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..138a2be4b 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -935,36 +935,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/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 87b8a481f..a622c8f7c 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -74,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); 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/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/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 30bc561c3..d043c78bb 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -1,6 +1,7 @@ set(TEST_TYPE "COMMON_TEST") set(tests + added_mass addexternalforcetorque basic_test collisions 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/world_features.cc b/test/common_test/world_features.cc index 9e0280311..c2165f563 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -74,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 + + + + +