diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 8b42f9d59..28e7d7ff4 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -79,7 +79,7 @@ Identity SDFFeatures::ConstructSdfWorld( auto gravity = _sdfWorld.Gravity(); worldInfo->world->setGravity(btVector3(gravity[0], gravity[1], gravity[2])); - for (std::size_t i=0; i < _sdfWorld.ModelCount(); ++i) + for (std::size_t i = 0; i < _sdfWorld.ModelCount(); ++i) { const ::sdf::Model *model = _sdfWorld.ModelByIndex(i); @@ -636,26 +636,31 @@ bool SDFFeatures::AddSdfCollision( mu = ode->Mu(); mu2 = ode->Mu2(); } - - if (const auto bullet = friction->Element()->GetElement("bullet")) + if (friction->Element()) { - if (const auto f1 = bullet->GetElement("friction")) - mu = f1->Get(); + if (const auto bullet = friction->Element()->GetElement("bullet")) + { + if (const auto f1 = bullet->GetElement("friction")) + mu = f1->Get(); - if (const auto f2 = bullet->GetElement("friction2")) - mu2 = f2->Get(); + if (const auto f2 = bullet->GetElement("friction2")) + mu2 = f2->Get(); - // What is fdir1 for in the SDF's spec? + // What is fdir1 for in the SDF's spec? - if (const auto rolling = bullet->GetElement("rolling_friction")) - rollingFriction = rolling->Get(); + if (const auto rolling = bullet->GetElement("rolling_friction")) + rollingFriction = rolling->Get(); + } } } - if (const auto bounce = surface->Element()->GetElement("bounce")) + if (surface->Element()) { - if (const auto r = bounce->GetElement("restitution_coefficient")) - restitution = r->Get(); + if (const auto bounce = surface->Element()->GetElement("bounce")) + { + if (const auto r = bounce->GetElement("restitution_coefficient")) + restitution = r->Get(); + } } } diff --git a/bullet-featherstone/src/WorldFeatures.cc b/bullet-featherstone/src/WorldFeatures.cc new file mode 100644 index 000000000..30250004f --- /dev/null +++ b/bullet-featherstone/src/WorldFeatures.cc @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include + +#include "WorldFeatures.hh" + +namespace gz { +namespace physics { +namespace bullet_featherstone { + +///////////////////////////////////////////////// +void WorldFeatures::SetWorldGravity( + const Identity &_id, const LinearVectorType &_gravity) +{ + auto worldInfo = this->ReferenceInterface(_id); + if (worldInfo) + worldInfo->world->setGravity( + btVector3(_gravity(0), _gravity(1), _gravity(2))); +} + +///////////////////////////////////////////////// +WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity( + const Identity &_id) const +{ + const auto worldInfo = this->ReferenceInterface(_id); + if (worldInfo) + { + // auto world = this->ReferenceInterface(_id); + return WorldFeatures::LinearVectorType( + worldInfo->world->getGravity().x(), + worldInfo->world->getGravity().y(), + worldInfo->world->getGravity().z()); + } + else + { + return WorldFeatures::LinearVectorType(0, 0, 0); + } +} +} +} +} diff --git a/bullet-featherstone/src/WorldFeatures.hh b/bullet-featherstone/src/WorldFeatures.hh new file mode 100644 index 000000000..aefa84fd1 --- /dev/null +++ b/bullet-featherstone/src/WorldFeatures.hh @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef GZ_PHYSICS_BULLET_SRC_WORLDFEATURES_HH_ +#define GZ_PHYSICS_BULLET_SRC_WORLDFEATURES_HH_ + +#include + +#include + +#include "Base.hh" + +namespace gz { +namespace physics { +namespace bullet_featherstone { + +struct WorldFeatureList : FeatureList< + Gravity +> { }; + +class WorldFeatures : + public virtual Base, + public virtual Implements3d +{ + // Documentation inherited + public: void SetWorldGravity( + const Identity &_id, const LinearVectorType &_gravity) override; + + // Documentation inherited + public: LinearVectorType GetWorldGravity(const Identity &_id) const override; +}; + +} +} +} + +#endif diff --git a/bullet-featherstone/src/plugin.cc b/bullet-featherstone/src/plugin.cc index 69d683ebb..94fa4aad1 100644 --- a/bullet-featherstone/src/plugin.cc +++ b/bullet-featherstone/src/plugin.cc @@ -27,6 +27,7 @@ #include "KinematicsFeatures.hh" #include "FreeGroupFeatures.hh" #include "JointFeatures.hh" +#include "WorldFeatures.hh" namespace gz { namespace physics { @@ -38,7 +39,8 @@ struct BulletFeatures : FeatureList < FreeGroupFeatureList, KinematicsFeatureList, SDFFeatureList, - JointFeatureList + JointFeatureList, + WorldFeatureList > { }; class Plugin : @@ -49,7 +51,8 @@ class Plugin : public virtual FreeGroupFeatures, public virtual KinematicsFeatures, public virtual SDFFeatures, - public virtual JointFeatures + public virtual JointFeatures, + public virtual WorldFeatures {}; GZ_PHYSICS_ADD_PLUGIN(Plugin, FeaturePolicy3d, BulletFeatures) diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 383fd21f5..53fdcb7af 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -4,6 +4,7 @@ set(tests basic_test construct_empty_world simulation_features + world_features ) function(configure_common_test PHYSICS_ENGINE_NAME test_name) diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc new file mode 100644 index 000000000..ee5acf58b --- /dev/null +++ b/test/common_test/world_features.cc @@ -0,0 +1,140 @@ +/* + * 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 "../helpers/TestLibLoader.hh" +#include "../Utils.hh" + +#include +#include +#include + +#include +#include + +#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; +}; + + +template +class WorldFeaturesTest: + public testing::Test, public gz::physics::TestLibLoader +{ + // Documentation inherited + public: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + + loader.LoadLib(WorldFeaturesTest::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(); + } + } + + public: std::set pluginNames; + public: gz::plugin::Loader loader; +}; + +using GravityFeatures = gz::physics::FeatureList< + gz::physics::GetEngineInfo, + gz::physics::Gravity, + gz::physics::sdf::ConstructSdfWorld +>; + +template +class GravityFeaturesTestClass : public WorldFeaturesTest{}; +TYPED_TEST_CASE(GravityFeaturesTestClass, GravityFeatures); + +///////////////////////////////////////////////// +TYPED_TEST(GravityFeaturesTestClass, GravityFeatures) +{ + 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, "test.world")); + EXPECT_TRUE(errors.empty()) << errors; + const sdf::World *sdfWorld = root.WorldByIndex(0); + EXPECT_NE(nullptr, sdfWorld); + + auto graphErrors = sdfWorld->ValidateGraphs(); + EXPECT_EQ(0u, graphErrors.size()) << graphErrors; + + Eigen::Vector3d gravity = {0, 0, -9.8};//= Eigen::Vector3d::Zero(); + + gz::physics::World3dPtr world = + engine->ConstructWorld(*sdfWorld); + EXPECT_NE(nullptr, world); + + AssertVectorApprox vectorPredicate(1e-6); + EXPECT_PRED_FORMAT2(vectorPredicate, gravity, + world->GetGravity()); + + world->SetGravity({8, 4, 3}); + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d(8, 4, 3), + world->GetGravity()); + } +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + if(!WorldFeaturesTest::init(argc, argv)) + return -1; + return RUN_ALL_TESTS(); +} diff --git a/test/common_test/worlds/test.world b/test/common_test/worlds/test.world new file mode 100644 index 000000000..b75496857 --- /dev/null +++ b/test/common_test/worlds/test.world @@ -0,0 +1,383 @@ + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + false + + + 0 0 1 + 100 100 + + + + 0.9 0.9 0.9 1 + + + + + + 1 0 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 1 1 0 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 1 1 0 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + 1.1 + + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 1 1 0 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 1 1 0 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 1 1 0 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + + 0.1 + + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 1 1 0 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 1 1 0 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + 3.0 + + + + + + upper_link + lower_link + + 1.0 0 0 + + 3.0 + + + + + + + 0.0 10.0 10.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + + 1.0 + + + + + 0.8 + + + + + + + + 10 0 2 0 0 0 + + + 100 + + + 0 0 0 -1.57 0 0 + + + 0.1 + 0.2 + + + + + + 1 0 0 0 0 0 + + 1 + + + + + 0.1 0.1 0.1 + + + + + + world + base + + + -1 0 0 0 0 0 + base + bar + + 0 1 0 + + -0.5 + 0.5 + 100 + + + + + + + 10 10 2 0 0 0 + + + 100 + + + 0 0 0 -1.57 0 0 + + + 0.1 + 0.2 + + + + + + 0 0 -1 0 0 0 + + 1 + + + + + 0.1 0.1 0.1 + + + + + + world + base + + + 0 0 1 0 0 0 + base + bar + + 0 1 0 + + + + + + + + link0 + link1 + 2 + + + + + + + + link0 + link1 + + + + + link2 + link3 + + + + + link4 + link5 + + + +