From 4706fae2b6b2b548244db18349fd4f351ba7cc3d Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 19 Aug 2022 14:50:26 -0700 Subject: [PATCH 01/13] README: update CI badges, fix expected joint limits (#401) Signed-off-by: Steve Peters --- README.md | 8 ++++---- dartsim/src/SDFFeatures_TEST.cc | 8 ++++++-- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index a4c96b4a3..a46afb765 100644 --- a/README.md +++ b/README.md @@ -9,10 +9,10 @@ Build | Status -- | -- -Test coverage | [![codecov](https://codecov.io/gh/gazebosim/gz-physics/branch/main/graph/badge.svg)](https://codecov.io/gh/gazebosim/gz-physics/branch/main) -Ubuntu Focal | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-main-focal-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-main-focal-amd64) -Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-main-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-main-homebrew-amd64) -Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ign_physics-ci-win)](https://build.osrfoundation.org/job/ign_physics-ci-win) +Test coverage | [![codecov](https://codecov.io/gh/gazebosim/gz-physics/branch/gz-physics6/graph/badge.svg)](https://codecov.io/gh/gazebosim/gz-physics/branch/gz-physics6) +Ubuntu Focal | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-gz-physics6-focal-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-gz-physics6-focal-amd64) +Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-gz-physics6-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-gz-physics6-homebrew-amd64) +Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ign_physics-gz-6-win)](https://build.osrfoundation.org/job/ign_physics-gz-6-win) Gazebo Physics, a component of [Gazebo](https://gazebosim.org), provides an abstract physics interface designed to support simulation and rapid development of robot applications. diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 2c8abc72b..138a2be4b 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -344,12 +344,16 @@ TEST_P(SDFFeatures_TEST, CheckDartsimData) // Test that things were parsed correctly. These values are either stated or // implied in the test.world SDF file. verify(skeleton->getJoint(1)->getDof(0), - 3.0, 0.0, 0.0, 0.0, -1e16, 1e16, + 3.0, 0.0, 0.0, 0.0, + -std::numeric_limits::infinity(), + std::numeric_limits::infinity(), std::numeric_limits::infinity(), std::numeric_limits::infinity()); verify(skeleton->getJoint(2)->getDof(0), - 3.0, 0.0, 0.0, 0.0, -1e16, 1e16, + 3.0, 0.0, 0.0, 0.0, + -std::numeric_limits::infinity(), + std::numeric_limits::infinity(), std::numeric_limits::infinity(), std::numeric_limits::infinity()); From e36166d969ceb04fe7e677cc6805cbbb9eb4728e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 30 Aug 2022 16:33:21 +0200 Subject: [PATCH 02/13] UPdate Codecov ignored files (#406) Signed-off-by: ahcorde --- codecov.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/codecov.yml b/codecov.yml index df92ff2d0..7604339af 100644 --- a/codecov.yml +++ b/codecov.yml @@ -1,2 +1,5 @@ ignore: - "tpe/lib/src/aabb_tree" + - test/* + - examples/* + - "**/*_TEST.cc" From 99a8c8e5d37b1e381a227a2e57c667bb0161ee3e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 1 Sep 2022 23:58:22 +0200 Subject: [PATCH 03/13] Improved simulation features common tests (#407) Signed-off-by: ahcorde --- bullet/CMakeLists.txt | 28 -- bullet/src/EntityManagement_TEST.cc | 51 --- bullet/src/SDFFeatures_TEST.cc | 128 ------- dartsim/src/SimulationFeatures_TEST.cc | 484 ------------------------ test/common_test/simulation_features.cc | 460 ++++++++++++++++++++-- test/common_test/worlds/contact.sdf | 93 +++++ test/common_test/worlds/falling.world | 75 ++++ 7 files changed, 586 insertions(+), 733 deletions(-) delete mode 100644 bullet/src/EntityManagement_TEST.cc delete mode 100644 bullet/src/SDFFeatures_TEST.cc delete mode 100644 dartsim/src/SimulationFeatures_TEST.cc create mode 100644 test/common_test/worlds/contact.sdf create mode 100644 test/common_test/worlds/falling.world diff --git a/bullet/CMakeLists.txt b/bullet/CMakeLists.txt index ccd775167..7b8e3d8a3 100644 --- a/bullet/CMakeLists.txt +++ b/bullet/CMakeLists.txt @@ -49,31 +49,3 @@ else() endif() EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned}) INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR}) - -# Testing -gz_build_tests( - TYPE UNIT_bullet - SOURCES ${test_sources} - LIB_DEPS - ${features} - gz-plugin${GZ_PLUGIN_VER}::loader - gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} - ${PROJECT_LIBRARY_TARGET_NAME}-sdf - ${PROJECT_LIBRARY_TARGET_NAME}-mesh - TEST_LIST tests) - -foreach(test ${tests}) - - target_compile_definitions(${test} PRIVATE - "bullet_plugin_LIB=\"$\"" - "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\"" - "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\"") - -endforeach() - -if(TARGET UNIT_FindFeatures_TEST) - - target_compile_definitions(UNIT_FindFeatures_TEST PRIVATE - "bullet_plugin_LIB=\"$\"") - -endif() diff --git a/bullet/src/EntityManagement_TEST.cc b/bullet/src/EntityManagement_TEST.cc deleted file mode 100644 index a07c095a8..000000000 --- a/bullet/src/EntityManagement_TEST.cc +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Copyright (C) 2021 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 "EntityManagementFeatures.hh" -#include "JointFeatures.hh" - -// ToDo(Lobotuerk): Once more tests are added into this plugin, delete line 35 -// and uncomment 31~33 adding another feature list to clear the warn. -// struct TestFeatureList : gz::physics::FeatureList< -// gz::physics::bullet::EntityManagementFeatureList -// > { }; - -using TestFeatureList = gz::physics::bullet::EntityManagementFeatureList; - -TEST(EntityManagement_TEST, ConstructEmptyWorld) -{ - gz::plugin::Loader loader; - loader.LoadLib(bullet_plugin_LIB); - - gz::plugin::PluginPtr bullet = - loader.Instantiate("gz::physics::bullet::Plugin"); - - auto engine = - gz::physics::RequestEngine3d::From(bullet); - ASSERT_NE(nullptr, engine); - - auto world = engine->ConstructEmptyWorld("empty world"); - ASSERT_NE(nullptr, world); -} diff --git a/bullet/src/SDFFeatures_TEST.cc b/bullet/src/SDFFeatures_TEST.cc deleted file mode 100644 index 59475288f..000000000 --- a/bullet/src/SDFFeatures_TEST.cc +++ /dev/null @@ -1,128 +0,0 @@ -/* - * Copyright (C) 2021 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 - -struct TestFeatureList : gz::physics::FeatureList< - gz::physics::GetBasicJointState, - gz::physics::SetBasicJointState, - gz::physics::sdf::ConstructSdfJoint, - gz::physics::sdf::ConstructSdfLink, - gz::physics::sdf::ConstructSdfModel, - gz::physics::sdf::ConstructSdfWorld -> { }; - -using World = gz::physics::World3d; -using WorldPtr = gz::physics::World3dPtr; - -auto LoadEngine() -{ - gz::plugin::Loader loader; - loader.LoadLib(bullet_plugin_LIB); - - gz::plugin::PluginPtr bullet = - loader.Instantiate("gz::physics::bullet::Plugin"); - - auto engine = - gz::physics::RequestEngine3d::From(bullet); - return engine; -} - -// Create Model with parent and child links. If a link is not set, the joint -// will use the world as that link. -auto CreateTestModel(WorldPtr _world, const std::string &_model, - const std::optional &_parentLink, - const std::optional &_childLink) -{ - sdf::Model sdfModel; - sdfModel.SetName(_model); - auto model = _world->ConstructModel(sdfModel); - EXPECT_NE(nullptr, model); - - sdf::Joint sdfJoint; - sdfJoint.SetName("joint0"); - sdfJoint.SetType(sdf::JointType::REVOLUTE); - if (_parentLink) - { - auto parent = model->ConstructLink(*_parentLink); - EXPECT_NE(nullptr, parent); - sdfJoint.SetParentName(_parentLink->Name()); - } - else - { - sdfJoint.SetParentName("world"); - } - - if (_childLink) - { - auto child = model->ConstructLink(*_childLink); - EXPECT_NE(nullptr, child); - sdfJoint.SetChildName(_childLink->Name()); - } - else - { - sdfJoint.SetChildName("world"); - } - - auto joint0 = model->ConstructJoint(sdfJoint); - return std::make_tuple(model, joint0); -} - -// Test joints with world as parent or child -TEST(SDFFeatures_TEST, WorldIsParentOrChild) -{ - auto engine = LoadEngine(); - ASSERT_NE(nullptr, engine); - sdf::World sdfWorld; - sdfWorld.SetName("default"); - auto world = engine->ConstructWorld(sdfWorld); - EXPECT_NE(nullptr, world); - - std::optional parent = sdf::Link(); - parent->SetName("parent"); - std::optional child = sdf::Link(); - child->SetName("child"); - - { - const auto &[model, joint] = - CreateTestModel(world, "test0", std::nullopt, std::nullopt); - EXPECT_EQ(nullptr, joint); - } - { - const auto &[model, joint] = - CreateTestModel(world, "test2", std::nullopt, child); - EXPECT_NE(nullptr, joint); - } -} diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/dartsim/src/SimulationFeatures_TEST.cc deleted file mode 100644 index c5f93b0ee..000000000 --- a/dartsim/src/SimulationFeatures_TEST.cc +++ /dev/null @@ -1,484 +0,0 @@ -/* - * Copyright (C) 2018 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 - -// Features -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -struct TestFeatureList : gz::physics::FeatureList< - gz::physics::LinkFrameSemantics, - gz::physics::ForwardStep, - gz::physics::GetContactsFromLastStepFeature, - gz::physics::GetEntities, - gz::physics::GetShapeBoundingBox, - gz::physics::CollisionFilterMaskFeature, -#ifdef DART_HAS_CONTACT_SURFACE - gz::physics::SetContactPropertiesCallbackFeature, -#endif - gz::physics::sdf::ConstructSdfWorld -> { }; - -using TestWorldPtr = gz::physics::World3dPtr; -using TestShapePtr = gz::physics::Shape3dPtr; -using TestWorld = gz::physics::World3d; -using TestContactPoint = TestWorld::ContactPoint; -using TestExtraContactData = TestWorld::ExtraContactData; -using TestContact = TestWorld::Contact; -#ifdef DART_HAS_CONTACT_SURFACE -using ContactSurfaceParams = - gz::physics::SetContactPropertiesCallbackFeature:: - ContactSurfaceParams; -#endif - -std::unordered_set LoadWorlds( - const std::string &_library, - const std::string &_world) -{ - gz::plugin::Loader loader; - loader.LoadLib(_library); - - const std::set pluginNames = - gz::physics::FindFeatures3d::From(loader); - - EXPECT_LT(0u, pluginNames.size()); - - std::unordered_set worlds; - for (const std::string &name : pluginNames) - { - gz::plugin::PluginPtr plugin = loader.Instantiate(name); - - std::cout << " -- Plugin name: " << name << std::endl; - - auto engine = - gz::physics::RequestEngine3d::From(plugin); - EXPECT_NE(nullptr, engine); - - 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); - - worlds.insert(world); - } - - return worlds; -} - -/// \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 -/// being stepped in (true) or not (false) -/// \param[in] _numSteps The number of steps to take in _world -/// \return true if the forward step output was checked, false otherwise -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; - - bool checkedOutput = false; - for (size_t i = 0; i < _numSteps; ++i) - { - _world->Step(output, state, input); - - // If link poses have changed, this should have been written to output. - // Link poses are considered "changed" if they are new, so if this is the - // very first step in a world, all of the link data is new and output - // should not be empty - if (_firstTime && (i == 0)) - { - EXPECT_FALSE( - output.Get().entries.empty()); - checkedOutput = true; - } - } - - return checkedOutput; -} - -class SimulationFeatures_TEST - : public ::testing::Test, - public ::testing::WithParamInterface -{}; - -// Test that the dartsim plugin loaded all the relevant information correctly. -TEST_P(SimulationFeatures_TEST, Falling) -{ - const std::string library = GetParam(); - if (library.empty()) - return; - - std::cout << "Testing library " << library << std::endl; - auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/falling.world"); - - for (const auto &world : worlds) - { - auto checkedOutput = StepWorld(world, true, 1000); - EXPECT_TRUE(checkedOutput); - - auto link = world->GetModel(0)->GetLink(0); - auto pos = link->FrameDataRelativeToWorld().pose.translation(); - EXPECT_NEAR(pos.z(), 1.0, 5e-2); - } -} - -TEST_P(SimulationFeatures_TEST, ShapeBoundingBox) -{ - const std::string library = GetParam(); - if (library.empty()) - return; - - auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/falling.world"); - - for (const auto &world : worlds) - { - auto sphere = world->GetModel("sphere"); - auto sphereCollision = sphere->GetLink(0)->GetShape(0); - auto ground = world->GetModel("box"); - auto groundCollision = ground->GetLink(0)->GetShape(0); - - // Test the bounding boxes in the local frames - auto sphereAABB = - sphereCollision->GetAxisAlignedBoundingBox(*sphereCollision); - - auto groundAABB = - groundCollision->GetAxisAlignedBoundingBox(*groundCollision); - - 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(groundAABB).Min()); - EXPECT_EQ(gz::math::Vector3d(50, 50, 0.5), - gz::math::eigen3::convert(groundAABB).Max()); - - // Test the bounding boxes in the world frames - sphereAABB = sphereCollision->GetAxisAlignedBoundingBox(); - groundAABB = groundCollision->GetAxisAlignedBoundingBox(); - - // The sphere shape has a radius of 1.0, so its bounding box will have - // dimensions of 1.0 x 1.0 x 1.0. When that bounding box is transformed by - // a 45-degree rotation, the dimensions that are orthogonal to the axis of - // rotation will dilate from 1.0 to sqrt(2). - const double d = std::sqrt(2); - EXPECT_EQ(gz::math::Vector3d(-d, -1, 2.0 - d), - gz::math::eigen3::convert(sphereAABB).Min()); - EXPECT_EQ(gz::math::Vector3d(d, 1, 2 + d), - gz::math::eigen3::convert(sphereAABB).Max()); - EXPECT_EQ(gz::math::Vector3d(-50*d, -50*d, -1), - gz::math::eigen3::convert(groundAABB).Min()); - EXPECT_EQ(gz::math::Vector3d(50*d, 50*d, 0), - gz::math::eigen3::convert(groundAABB).Max()); - } -} - -// Tests collision filtering based on bitmasks -TEST_P(SimulationFeatures_TEST, CollideBitmasks) -{ - const std::string library = GetParam(); - if (library.empty()) - return; - - auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/shapes_bitmask.sdf"); - - for (const auto &world : worlds) - { - auto baseBox = world->GetModel("box_base"); - auto filteredBox = world->GetModel("box_filtered"); - auto collidingBox = world->GetModel("box_colliding"); - - auto checkedOutput = StepWorld(world, true); - EXPECT_TRUE(checkedOutput); - auto contacts = world->GetContactsFromLastStep(); - // Only one box (box_colliding) should collide - EXPECT_EQ(4u, contacts.size()); - - // Now disable collisions for the colliding box as well - auto collidingShape = collidingBox->GetLink(0)->GetShape(0); - auto filteredShape = filteredBox->GetLink(0)->GetShape(0); - collidingShape->SetCollisionFilterMask(0xF0); - // Also test the getter - EXPECT_EQ(0xF0, collidingShape->GetCollisionFilterMask()); - // Step and make sure there is no collisions - checkedOutput = StepWorld(world, false); - EXPECT_FALSE(checkedOutput); - contacts = world->GetContactsFromLastStep(); - EXPECT_EQ(0u, contacts.size()); - - // Now remove both filter masks (no collision will be filtered) - // Equivalent to set to 0xFF - collidingShape->RemoveCollisionFilterMask(); - filteredShape->RemoveCollisionFilterMask(); - checkedOutput = StepWorld(world, false); - EXPECT_FALSE(checkedOutput); - // Expect both objects to collide - contacts = world->GetContactsFromLastStep(); - EXPECT_EQ(8u, contacts.size()); - } -} - -TEST_P(SimulationFeatures_TEST, RetrieveContacts) -{ - const std::string library = GetParam(); - if (library.empty()) - return; - - auto worlds = LoadWorlds(library, 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 expectations - { - {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 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 = [&](const TestContact& _contact, const bool zeroForce) - { - const auto &contactPoint = _contact.Get(); - 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(); - 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 TestContact& _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 - - // 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 - - // 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 - - auto contacts = world->GetContactsFromLastStep(); - EXPECT_EQ(4u, contacts.size()); - - 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")); - - // removing an existing callback works and the callback is no longer called - EXPECT_TRUE(world->RemoveContactPropertiesCallback("test")); - - // Third step - StepWorld(world, false); - - // 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. - - auto contactCallback2 = [&]( - const TestContact& /*_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 - } -} - -INSTANTIATE_TEST_SUITE_P(PhysicsPlugins, SimulationFeatures_TEST, - ::testing::ValuesIn(gz::physics::test::g_PhysicsPluginLibraries)); diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index e342dafde..4a8088f60 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -29,6 +29,7 @@ #include #include "../helpers/TestLibLoader.hh" +#include "../Utils.hh" #include #include @@ -89,9 +90,7 @@ using Features = gz::physics::FeatureList< gz::physics::GetEllipsoidShapeProperties >; -using TestWorldPtr = gz::physics::World3dPtr; -using TestContactPoint = gz::physics::World3d::ContactPoint; - +template class SimulationFeaturesTest: public testing::Test, public gz::physics::TestLibLoader { @@ -104,7 +103,7 @@ class SimulationFeaturesTest: // TODO(ahcorde): We should also run the 3f, 2d, and 2f variants of // FindFeatures - pluginNames = gz::physics::FindFeatures3d::From(loader); + pluginNames = gz::physics::FindFeatures3d::From(loader); if (pluginNames.empty()) { std::cerr << "No plugins with required features found in " @@ -116,12 +115,13 @@ class SimulationFeaturesTest: public: gz::plugin::Loader loader; }; -std::unordered_set LoadWorlds( +template +std::unordered_set> LoadWorlds( const gz::plugin::Loader &_loader, const std::set pluginNames, const std::string &_world) { - std::unordered_set worlds; + std::unordered_set> worlds; for (const std::string &name : pluginNames) { gz::plugin::PluginPtr plugin = _loader.Instantiate(name); @@ -129,7 +129,7 @@ std::unordered_set LoadWorlds( gzdbg << " -- Plugin name: " << name << std::endl; auto engine = - gz::physics::RequestEngine3d::From(plugin); + gz::physics::RequestEngine3d::From(plugin); EXPECT_NE(nullptr, engine); sdf::Root root; @@ -149,8 +149,11 @@ std::unordered_set LoadWorlds( /// being stepped in (true) or not (false) /// \param[in] _numSteps The number of steps to take in _world /// \return true if the forward step output was checked, false otherwise -bool StepWorld(const TestWorldPtr &_world, bool _firstTime, - const std::size_t _numSteps = 1) +template +bool StepWorld( + const gz::physics::World3dPtr &_world, + bool _firstTime, + const std::size_t _numSteps = 1) { gz::physics::ForwardStep::Input input; gz::physics::ForwardStep::State state; @@ -176,26 +179,61 @@ bool StepWorld(const TestWorldPtr &_world, bool _firstTime, return checkedOutput; } +template +class SimulationFeaturesTestBasic : + public SimulationFeaturesTest{}; +using SimulationFeaturesTestBasicTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesTestBasic, + SimulationFeaturesTestBasicTypes); + ///////////////////////////////////////////////// -TEST_F(SimulationFeaturesTest, StepWorld) +TYPED_TEST(SimulationFeaturesTestBasic, StepWorld) { - auto worlds = LoadWorlds( - loader, - pluginNames, + auto worlds = LoadWorlds( + this->loader, + this->pluginNames, gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); for (const auto &world : worlds) { - auto checkedOutput = StepWorld(world, true, 1000); + auto checkedOutput = StepWorld(world, true, 1000); EXPECT_TRUE(checkedOutput); } } + ///////////////////////////////////////////////// -TEST_F(SimulationFeaturesTest, ShapeFeatures) +TYPED_TEST(SimulationFeaturesTestBasic, Falling) { - auto worlds = LoadWorlds( - loader, - pluginNames, + for (const std::string &name : this->pluginNames) + { + if(this->PhysicsEngineName(name) == "tpe") + { + GTEST_SKIP(); + } + + auto worlds = LoadWorlds( + this->loader, + this->pluginNames, + gz::common::joinPaths(TEST_WORLD_DIR, "falling.world")); + for (const auto &world : worlds) + { + auto checkedOutput = StepWorld(world, true, 1000); + EXPECT_TRUE(checkedOutput); + + auto link = world->GetModel(0)->GetLink(0); + auto pos = link->FrameDataRelativeToWorld().pose.translation(); + EXPECT_NEAR(pos.z(), 1.0, 5e-2); + } + } +} + +///////////////////////////////////////////////// +TYPED_TEST(SimulationFeaturesTestBasic, ShapeFeatures) +{ + auto worlds = LoadWorlds( + this->loader, + this->pluginNames, gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); for (const auto &world : worlds) { @@ -328,11 +366,11 @@ TEST_F(SimulationFeaturesTest, ShapeFeatures) } } -TEST_F(SimulationFeaturesTest, FreeGroup) +TYPED_TEST(SimulationFeaturesTestBasic, FreeGroup) { - auto worlds = LoadWorlds( - loader, - pluginNames, + auto worlds = LoadWorlds( + this->loader, + this->pluginNames, gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); for (const auto &world : worlds) @@ -350,7 +388,7 @@ TEST_F(SimulationFeaturesTest, FreeGroup) auto freeGroupLink = link->FindFreeGroup(); ASSERT_NE(nullptr, freeGroupLink); - StepWorld(world, true); + StepWorld(world, true); freeGroup->SetWorldPose( gz::math::eigen3::convert( @@ -365,7 +403,7 @@ TEST_F(SimulationFeaturesTest, FreeGroup) gz::math::eigen3::convert(frameData.pose)); // Step the world - StepWorld(world, false); + 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( @@ -375,12 +413,61 @@ TEST_F(SimulationFeaturesTest, 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) + { + auto sphere = world->GetModel("sphere"); + auto sphereCollision = sphere->GetLink(0)->GetShape(0); + auto ground = world->GetModel("box"); + auto groundCollision = ground->GetLink(0)->GetShape(0); + + // Test the bounding boxes in the local frames + auto sphereAABB = + sphereCollision->GetAxisAlignedBoundingBox(*sphereCollision); + + auto groundAABB = + groundCollision->GetAxisAlignedBoundingBox(*groundCollision); -TEST_F(SimulationFeaturesTest, CollideBitmasks) + 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(groundAABB).Min()); + EXPECT_EQ(gz::math::Vector3d(50, 50, 0.5), + gz::math::eigen3::convert(groundAABB).Max()); + + // Test the bounding boxes in the world frames + sphereAABB = sphereCollision->GetAxisAlignedBoundingBox(); + groundAABB = groundCollision->GetAxisAlignedBoundingBox(); + + // The sphere shape has a radius of 1.0, so its bounding box will have + // dimensions of 1.0 x 1.0 x 1.0. When that bounding box is transformed by + // a 45-degree rotation, the dimensions that are orthogonal to the axis of + // rotation will dilate from 1.0 to sqrt(2). + const double d = std::sqrt(2); + EXPECT_EQ(gz::math::Vector3d(-d, -1, 2.0 - d), + gz::math::eigen3::convert(sphereAABB).Min()); + EXPECT_EQ(gz::math::Vector3d(d, 1, 2 + d), + gz::math::eigen3::convert(sphereAABB).Max()); + EXPECT_EQ(gz::math::Vector3d(-50*d, -50*d, -1), + gz::math::eigen3::convert(groundAABB).Min()); + EXPECT_EQ(gz::math::Vector3d(50*d, 50*d, 0), + gz::math::eigen3::convert(groundAABB).Max()); + } +} + +TYPED_TEST(SimulationFeaturesTestBasic, CollideBitmasks) { - auto worlds = LoadWorlds( - loader, - pluginNames, + auto worlds = LoadWorlds( + this->loader, + this->pluginNames, gz::common::joinPaths(TEST_WORLD_DIR, "shapes_bitmask.sdf")); for (const auto &world : worlds) @@ -389,7 +476,7 @@ TEST_F(SimulationFeaturesTest, CollideBitmasks) auto filteredBox = world->GetModel("box_filtered"); auto collidingBox = world->GetModel("box_colliding"); - auto checkedOutput = StepWorld(world, true); + auto checkedOutput = StepWorld(world, true); EXPECT_TRUE(checkedOutput); auto contacts = world->GetContactsFromLastStep(); // Only box_colliding should collide with box_base @@ -402,7 +489,7 @@ TEST_F(SimulationFeaturesTest, CollideBitmasks) // Also test the getter EXPECT_EQ(0xF0, collidingShape->GetCollisionFilterMask()); // Step and make sure there are no collisions - checkedOutput = StepWorld(world, false); + checkedOutput = StepWorld(world, false); EXPECT_FALSE(checkedOutput); contacts = world->GetContactsFromLastStep(); EXPECT_EQ(0u, contacts.size()); @@ -411,7 +498,7 @@ TEST_F(SimulationFeaturesTest, CollideBitmasks) // Equivalent to 0xFF collidingShape->RemoveCollisionFilterMask(); filteredShape->RemoveCollisionFilterMask(); - checkedOutput = StepWorld(world, false); + checkedOutput = StepWorld(world, false); EXPECT_FALSE(checkedOutput); // Expect box_filtered and box_colliding to collide with box_base contacts = world->GetContactsFromLastStep(); @@ -420,12 +507,13 @@ TEST_F(SimulationFeaturesTest, CollideBitmasks) } -TEST_F(SimulationFeaturesTest, RetrieveContacts) +TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) { - auto worlds = LoadWorlds( - loader, - pluginNames, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); + std::unordered_set> worlds = + LoadWorlds( + this->loader, + this->pluginNames, + gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); for (const auto &world : worlds) { @@ -448,7 +536,7 @@ TEST_F(SimulationFeaturesTest, RetrieveContacts) auto box = world->GetModel("box"); // step and get contacts - auto checkedOutput = StepWorld(world, true); + auto checkedOutput = StepWorld(world, true); EXPECT_TRUE(checkedOutput); auto contacts = world->GetContactsFromLastStep(); @@ -462,7 +550,7 @@ TEST_F(SimulationFeaturesTest, RetrieveContacts) for (auto &contact : contacts) { - const auto &contactPoint = contact.Get(); + const auto &contactPoint = contact.Get::ContactPoint>(); ASSERT_TRUE(contactPoint.collision1); ASSERT_TRUE(contactPoint.collision2); EXPECT_NE(contactPoint.collision1, contactPoint.collision2); @@ -507,7 +595,7 @@ TEST_F(SimulationFeaturesTest, RetrieveContacts) gz::math::Pose3d(0, 100, 0.5, 0, 0, 0))); // step and get contacts - checkedOutput = StepWorld(world, false); + checkedOutput = StepWorld(world, false); EXPECT_FALSE(checkedOutput); contacts = world->GetContactsFromLastStep(); @@ -520,7 +608,7 @@ TEST_F(SimulationFeaturesTest, RetrieveContacts) contactBoxEllipsoid = 0u; for (auto contact : contacts) { - const auto &contactPoint = contact.Get<::TestContactPoint>(); + const auto &contactPoint = contact.Get::ContactPoint>(); ASSERT_TRUE(contactPoint.collision1); ASSERT_TRUE(contactPoint.collision2); EXPECT_NE(contactPoint.collision1, contactPoint.collision2); @@ -566,7 +654,7 @@ TEST_F(SimulationFeaturesTest, RetrieveContacts) gz::math::Pose3d(0, -100, -100, 0, 0, 0))); // step and get contacts - checkedOutput = StepWorld(world, false); + checkedOutput = StepWorld(world, false); EXPECT_FALSE(checkedOutput); contacts = world->GetContactsFromLastStep(); @@ -575,9 +663,297 @@ TEST_F(SimulationFeaturesTest, RetrieveContacts) } } +using FeaturesContactPropertiesCallback = gz::physics::FeatureList< + gz::physics::ConstructEmptyWorldFeature, + + gz::physics::FindFreeGroupFeature, + gz::physics::SetFreeGroupWorldPose, + gz::physics::SetFreeGroupWorldVelocity, + + gz::physics::GetContactsFromLastStepFeature, + gz::physics::CollisionFilterMaskFeature, + + gz::physics::GetModelFromWorld, + gz::physics::GetLinkFromModel, + gz::physics::GetShapeFromLink, + gz::physics::GetModelBoundingBox, + + // gz::physics::sdf::ConstructSdfJoint, + gz::physics::sdf::ConstructSdfLink, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfCollision, + gz::physics::sdf::ConstructSdfWorld, + + gz::physics::ForwardStep, + + #ifdef DART_HAS_CONTACT_SURFACE + gz::physics::SetContactPropertiesCallbackFeature, + #endif + + gz::physics::AttachBoxShapeFeature, + gz::physics::AttachSphereShapeFeature, + gz::physics::AttachCylinderShapeFeature, + gz::physics::AttachEllipsoidShapeFeature, + gz::physics::AttachCapsuleShapeFeature, + gz::physics::GetSphereShapeProperties, + gz::physics::GetBoxShapeProperties, + gz::physics::GetCylinderShapeProperties, + gz::physics::GetCapsuleShapeProperties, + gz::physics::GetEllipsoidShapeProperties +>; + +#ifdef DART_HAS_CONTACT_SURFACE +using ContactSurfaceParams = + gz::physics::SetContactPropertiesCallbackFeature:: + ContactSurfaceParams::Policy>; +#endif + +template +class SimulationFeaturesTestFeaturesContactPropertiesCallback : + public SimulationFeaturesTest{}; +using SimulationFeaturesTestFeaturesContactPropertiesCallbackTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesTestFeaturesContactPropertiesCallback, + FeaturesContactPropertiesCallback); + +///////////////////////////////////////////////// +TYPED_TEST(SimulationFeaturesTestFeaturesContactPropertiesCallback, ContactPropertiesCallback) +{ + for (const std::string &name : this->pluginNames) + { + std::unordered_set> worlds = + LoadWorlds( + this->loader, + this->pluginNames, + 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 + { + {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}, + {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 = [&]( + 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) + { + 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); + }; + + #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 + + // 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 + + // 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 + + auto contacts = world->GetContactsFromLastStep(); + if(this->PhysicsEngineName(name) != "tpe") + { + EXPECT_EQ(4u, contacts.size()); + } + + 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")); + + // removing an existing callback works and the callback is no longer called + EXPECT_TRUE(world->RemoveContactPropertiesCallback("test")); + + // Third step + StepWorld(world, false); + + // 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. + + 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 + } + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); - SimulationFeaturesTest::init(argc, argv); + if (!SimulationFeaturesTest::init( + argc, argv)) + return -1; return RUN_ALL_TESTS(); } diff --git a/test/common_test/worlds/contact.sdf b/test/common_test/worlds/contact.sdf new file mode 100644 index 000000000..25bc3be83 --- /dev/null +++ b/test/common_test/worlds/contact.sdf @@ -0,0 +1,93 @@ + + + + + true + + + + + 0 0 1 + 100 100 + + + + + false + + + 0 0 1 + 100 100 + + + + + + + + 0 0 0.5 0 0 0 + + + 1 + + + + + 1 + + + + 0.1 + + + + 0 1 0.5 0 0 0 + + + 1 + + + + + 1 + + + + 1 + + + + 1 0 0.5 0 0 0 + + + 1 + + + + + 1 + + + + 2 + + + + 1 1 0.5 0 0 0 + + + 1 + + + + + 1 + + + + 3 + + + + + diff --git a/test/common_test/worlds/falling.world b/test/common_test/worlds/falling.world new file mode 100644 index 000000000..1d2ea2f43 --- /dev/null +++ b/test/common_test/worlds/falling.world @@ -0,0 +1,75 @@ + + + + + + + 0 0 2 0 0.78539816339 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 + + + + + + + true + 0 0 -0.5 0 0 0.78539816339 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + 0.0 0.0 0.0 0 0 0 + + + + 100 100 1 + + + + + + + + 100 100 1 + + + + + + + From 78cc33bcb30930739d46dc1d5332d4d46a559958 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 2 Sep 2022 17:50:34 +0200 Subject: [PATCH 04/13] Added joint_features common tests (#408) Signed-off-by: ahcorde --- bullet/src/JointFeatures.cc | 9 + bullet/src/SimulationFeatures.cc | 57 +- bullet/src/SimulationFeatures.hh | 10 + dartsim/src/JointFeatures_TEST.cc | 1536 -------------- test/common_test/CMakeLists.txt | 1 + test/common_test/joint_features.cc | 1830 +++++++++++++++++ .../common_test}/worlds/ground.sdf | 0 .../worlds/joint_across_models.sdf | 0 .../common_test}/worlds/joint_constraint.sdf | 0 .../worlds/pendulum_joint_wrench.sdf | 0 test/common_test/worlds/test.world | 383 ++++ 11 files changed, 2285 insertions(+), 1541 deletions(-) delete mode 100644 dartsim/src/JointFeatures_TEST.cc create mode 100644 test/common_test/joint_features.cc rename {dartsim => test/common_test}/worlds/ground.sdf (100%) rename {dartsim => test/common_test}/worlds/joint_across_models.sdf (100%) rename {dartsim => test/common_test}/worlds/joint_constraint.sdf (100%) rename {dartsim => test/common_test}/worlds/pendulum_joint_wrench.sdf (100%) create mode 100644 test/common_test/worlds/test.world diff --git a/bullet/src/JointFeatures.cc b/bullet/src/JointFeatures.cc index cfcdff496..8b01b03f3 100644 --- a/bullet/src/JointFeatures.cc +++ b/bullet/src/JointFeatures.cc @@ -297,6 +297,15 @@ void JointFeatures::SetJointForce( { const JointInfoPtr &jointInfo = this->joints.at(_id.id); const int jointType = jointInfo->constraintType; + + if (!std::isfinite(_value)) + { + gzerr << "Invalid joint force value [" << _value << "] set on joint [" + << jointInfo->name << " DOF " << _dof + << "]. The value will be ignored\n"; + return; + } + switch(jointInfo->constraintType) { case static_cast(::sdf::JointType::REVOLUTE) : diff --git a/bullet/src/SimulationFeatures.cc b/bullet/src/SimulationFeatures.cc index 6bb134243..4ed62aa0f 100644 --- a/bullet/src/SimulationFeatures.cc +++ b/bullet/src/SimulationFeatures.cc @@ -17,22 +17,69 @@ #include "SimulationFeatures.hh" +#include +#include + namespace gz { namespace physics { namespace bullet { +///////////////////////////////////////////////// void SimulationFeatures::WorldForwardStep( const Identity &_worldID, - ForwardStep::Output & /*_h*/, + ForwardStep::Output & _h, ForwardStep::State & /*_x*/, const ForwardStep::Input & _u) { - const WorldInfoPtr &worldInfo = this->worlds.at(_worldID); + const WorldInfoPtr &worldInfo = this->worlds.at(_worldID); - auto *dtDur = - _u.Query(); + auto *dtDur = + _u.Query(); + if (dtDur) + { std::chrono::duration dt = *dtDur; - worldInfo->world->stepSimulation(dt.count(), 1, dt.count()); + stepSize = dt.count(); + } + + worldInfo->world->stepSimulation(this->stepSize, 1, this->stepSize); + this->Write(_h.Get()); +} + +///////////////////////////////////////////////// +void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const +{ + // remove link poses from the previous iteration + _changedPoses.entries.clear(); + _changedPoses.entries.reserve(this->links.size()); + + std::unordered_map newPoses; + + for (const auto &[id, info] : this->links) + { + // make sure the link exists + if (info) + { + WorldPose wp; + wp.pose = info->pose; + wp.body = id; + + auto iter = this->prevLinkPoses.find(id); + if ((iter == this->prevLinkPoses.end()) || + !iter->second.Pos().Equal(wp.pose.Pos(), 1e-6) || + !iter->second.Rot().Equal(wp.pose.Rot(), 1e-6)) + { + _changedPoses.entries.push_back(wp); + newPoses[id] = wp.pose; + } + else + newPoses[id] = iter->second; + } + } + + // Save the new poses so that they can be used to check for updates in the + // next iteration. Re-setting this->prevLinkPoses with the contents of + // newPoses ensures that we aren't caching data for links that were removed + this->prevLinkPoses = std::move(newPoses); } } // namespace bullet diff --git a/bullet/src/SimulationFeatures.hh b/bullet/src/SimulationFeatures.hh index 286b5da7b..9df3337a4 100644 --- a/bullet/src/SimulationFeatures.hh +++ b/bullet/src/SimulationFeatures.hh @@ -18,7 +18,9 @@ #ifndef GZ_PHYSICS_BULLET_SRC_SIMULATIONFEATURES_HH_ #define GZ_PHYSICS_BULLET_SRC_SIMULATIONFEATURES_HH_ +#include #include + #include #include "Base.hh" @@ -40,6 +42,14 @@ class SimulationFeatures : ForwardStep::Output &_h, ForwardStep::State &_x, const ForwardStep::Input &_u) override; + + public: void Write(ChangedWorldPoses &_changedPoses) const; + + private: double stepSize = 0.001; + + /// \brief link poses from the most recent pose change/update. + /// The key is the link's ID, and the value is the link's pose + private: mutable std::unordered_map prevLinkPoses; }; } // namespace bullet diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc deleted file mode 100644 index dff5b6e21..000000000 --- a/dartsim/src/JointFeatures_TEST.cc +++ /dev/null @@ -1,1536 +0,0 @@ -/* - * Copyright (C) 2019 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 - -// Features -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "gz/physics/Geometry.hh" -#include "test/Utils.hh" - -using namespace gz; - -using TestFeatureList = gz::physics::FeatureList< - physics::dartsim::RetrieveWorld, - physics::AttachFixedJointFeature, - physics::DetachJointFeature, - physics::SetJointTransformFromParentFeature, - physics::ForwardStep, - physics::FreeJointCast, - physics::SetFreeGroupWorldPose, - physics::GetBasicJointState, - physics::GetEntities, - physics::GetJointTransmittedWrench, - physics::JointFrameSemantics, - physics::LinkFrameSemantics, - physics::RevoluteJointCast, - physics::SetBasicJointState, - physics::SetJointVelocityCommandFeature, - physics::sdf::ConstructSdfModel, - physics::sdf::ConstructSdfWorld, - physics::SetJointPositionLimitsFeature, - physics::SetJointVelocityLimitsFeature, - physics::SetJointEffortLimitsFeature ->; - -using TestEnginePtr = physics::Engine3dPtr; - -class JointFeaturesFixture : public ::testing::Test -{ - protected: void SetUp() override - { - gz::plugin::Loader loader; - loader.LoadLib(dartsim_plugin_LIB); - - gz::plugin::PluginPtr dartsim = - loader.Instantiate("gz::physics::dartsim::Plugin"); - - this->engine = - gz::physics::RequestEngine3d::From(dartsim); - ASSERT_NE(nullptr, this->engine); - } - protected: TestEnginePtr engine; -}; - -// Test setting joint commands and verify that the joint type is set accordingly -// and that the commanded velocity is acheived -TEST_F(JointFeaturesFixture, JointSetCommand) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - const std::string modelName{"double_pendulum_with_base"}; - const std::string jointName{"upper_joint"}; - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - - auto model = world->GetModel(modelName); - auto joint = model->GetJoint(jointName); - - dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); - ASSERT_NE(nullptr, dartWorld); - - const dart::dynamics::SkeletonPtr skeleton = - dartWorld->getSkeleton(modelName); - ASSERT_NE(nullptr, skeleton); - - const auto *dartBaseLink = skeleton->getBodyNode("base"); - ASSERT_NE(nullptr, dartBaseLink); - const auto *dartJoint = skeleton->getJoint(jointName); - - // Default actuatore type - EXPECT_EQ(dart::dynamics::Joint::FORCE, dartJoint->getActuatorType()); - - // Test joint velocity command - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - - // Expect negative joint velocity after 1 step without joint command - world->Step(output, state, input); - EXPECT_LT(joint->GetVelocity(0), 0.0); - - // Check that invalid velocity commands don't cause collisions to fail - for (std::size_t i = 0; i < 1000; ++i) - { - joint->SetForce(0, std::numeric_limits::quiet_NaN()); - // expect the position of the pendulum to stay aabove ground - world->Step(output, state, input); - EXPECT_NEAR(0.0, dartBaseLink->getWorldTransform().translation().z(), 1e-3); - } - - joint->SetVelocityCommand(0, 1); - world->Step(output, state, input); - // Setting a velocity command changes the actuator type to SERVO - EXPECT_EQ(dart::dynamics::Joint::SERVO, dartJoint->getActuatorType()); - - const std::size_t numSteps = 10; - for (std::size_t i = 0; i < numSteps; ++i) - { - // Call SetVelocityCommand before each step - joint->SetVelocityCommand(0, 1); - world->Step(output, state, input); - EXPECT_NEAR(1.0, joint->GetVelocity(0), 1e-6); - } - - for (std::size_t i = 0; i < numSteps; ++i) - { - // expect joint to freeze in subsequent steps without SetVelocityCommand - world->Step(output, state, input); - EXPECT_NEAR(0.0, joint->GetVelocity(0), 1e-6); - } - - // Check that invalid velocity commands don't cause collisions to fail - for (std::size_t i = 0; i < 1000; ++i) - { - joint->SetVelocityCommand(0, std::numeric_limits::quiet_NaN()); - // expect the position of the pendulum to stay aabove ground - world->Step(output, state, input); - EXPECT_NEAR(0.0, dartBaseLink->getWorldTransform().translation().z(), 1e-3); - } -} - -TEST_F(JointFeaturesFixture, JointSetPositionLimitsWithForceControl) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - auto model = world->GetModel("simple_joint_test"); - auto joint = model->GetJoint("j1"); - - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - - world->Step(output, state, input); - - auto pos = joint->GetPosition(0); - - joint->SetMinPosition(0, pos - 0.1); - joint->SetMaxPosition(0, pos + 0.1); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetForce(0, 100); - world->Step(output, state, input); - } - EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-3); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetForce(0, -100); - world->Step(output, state, input); - } - EXPECT_NEAR(pos - 0.1, joint->GetPosition(0), 1e-3); - - joint->SetMinPosition(0, pos - 0.5); - joint->SetMaxPosition(0, pos + 0.5); - - for (std::size_t i = 0; i < 300; ++i) - { - joint->SetForce(0, 100); - world->Step(output, state, input); - } - EXPECT_NEAR(pos + 0.5, joint->GetPosition(0), 1e-2); - - for (std::size_t i = 0; i < 300; ++i) - { - joint->SetForce(0, -100); - world->Step(output, state, input); - } - EXPECT_NEAR(pos - 0.5, joint->GetPosition(0), 1e-2); - - joint->SetMinPosition(0, -math::INF_D); - joint->SetMaxPosition(0, math::INF_D); - joint->SetPosition(0, pos); - - for (std::size_t i = 0; i < 300; ++i) - { - joint->SetForce(0, 100); - world->Step(output, state, input); - } - EXPECT_LT(pos + 0.5, joint->GetPosition(0)); -} - -#if DART_VERSION_AT_LEAST(6, 10, 0) -TEST_F(JointFeaturesFixture, JointSetVelocityLimitsWithForceControl) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - auto model = world->GetModel("simple_joint_test"); - auto joint = model->GetJoint("j1"); - - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - - world->Step(output, state, input); - - joint->SetMinVelocity(0, -0.25); - joint->SetMaxVelocity(0, 0.5); - - for (std::size_t i = 0; i < 10; ++i) - { - joint->SetForce(0, 1000); - world->Step(output, state, input); - } - EXPECT_NEAR(0.5, joint->GetVelocity(0), 1e-6); - - for (std::size_t i = 0; i < 10; ++i) - { - joint->SetForce(0, -1000); - world->Step(output, state, input); - } - EXPECT_NEAR(-0.25, joint->GetVelocity(0), 1e-6); - - // set minimum velocity above zero - joint->SetMinVelocity(0, 0.25); - - for (std::size_t i = 0; i < 10; ++i) - { - joint->SetForce(0, 0); - world->Step(output, state, input); - } - EXPECT_NEAR(0.25, joint->GetVelocity(0), 1e-6); - - for (std::size_t i = 0; i < 10; ++i) - { - // make sure the minimum velocity is kept even without velocity commands - world->Step(output, state, input); - } - EXPECT_NEAR(0.25, joint->GetVelocity(0), 1e-6); - - joint->SetMinVelocity(0, -0.25); - joint->SetPosition(0, 0); - joint->SetVelocity(0, 0); - - for (std::size_t i = 0; i < 10; ++i) - { - joint->SetForce(0, 0); - world->Step(output, state, input); - } - EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); - - joint->SetMinVelocity(0, -math::INF_D); - joint->SetMaxVelocity(0, math::INF_D); - - for (std::size_t i = 0; i < 10; ++i) - { - joint->SetForce(0, 1000); - world->Step(output, state, input); - } - EXPECT_LT(0.5, joint->GetVelocity(0)); -} - -TEST_F(JointFeaturesFixture, JointSetEffortLimitsWithForceControl) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - auto model = world->GetModel("simple_joint_test"); - auto joint = model->GetJoint("j1"); - - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - - world->Step(output, state, input); - - auto pos = joint->GetPosition(0); - - joint->SetMinEffort(0, -1e-6); - joint->SetMaxEffort(0, 1e-6); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetForce(0, 1); - world->Step(output, state, input); - } - EXPECT_NEAR(pos, joint->GetPosition(0), 1e-3); - EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetForce(0, -1); - world->Step(output, state, input); - } - EXPECT_NEAR(pos, joint->GetPosition(0), 1e-3); - EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); - - joint->SetMinEffort(0, -80); - joint->SetMaxEffort(0, 80); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetForce(0, 1); - world->Step(output, state, input); - } - EXPECT_LT(pos, joint->GetPosition(0)); - EXPECT_LT(0, joint->GetVelocity(0)); - - joint->SetMinEffort(0, -math::INF_D); - joint->SetMaxEffort(0, math::INF_D); - joint->SetPosition(0, 0); - joint->SetVelocity(0, 0); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetForce(0, 1); - world->Step(output, state, input); - } - EXPECT_LT(pos, joint->GetPosition(0)); - EXPECT_LT(0, joint->GetVelocity(0)); -} - -TEST_F(JointFeaturesFixture, JointSetCombinedLimitsWithForceControl) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - auto model = world->GetModel("simple_joint_test"); - auto joint = model->GetJoint("j1"); - - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - - world->Step(output, state, input); - - auto pos = joint->GetPosition(0); - - joint->SetMinPosition(0, pos - 0.1); - joint->SetMaxPosition(0, pos + 0.1); - joint->SetMinVelocity(0, -0.25); - joint->SetMaxVelocity(0, 0.5); - joint->SetMinEffort(0, -1e-6); - joint->SetMaxEffort(0, 1e-6); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetForce(0, 100); - world->Step(output, state, input); - } - EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); - EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetForce(0, -100); - world->Step(output, state, input); - } - EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); - EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); - - joint->SetMinEffort(0, -500); - joint->SetMaxEffort(0, 1000); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetForce(0, 1000); - world->Step(output, state, input); - } - // 0.05 because we go 0.1 s with max speed 0.5 - EXPECT_NEAR(pos + 0.05, joint->GetPosition(0), 1e-2); - EXPECT_NEAR(0.5, joint->GetVelocity(0), 1e-6); - - for (std::size_t i = 0; i < 200; ++i) - { - joint->SetForce(0, 1000); - world->Step(output, state, input); - } - EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-2); - EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); - - joint->SetPosition(0, pos); - EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); - - joint->SetMinVelocity(0, -1); - joint->SetMaxVelocity(0, 1); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetForce(0, 1000); - world->Step(output, state, input); - } - EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-2); - EXPECT_NEAR(1, joint->GetVelocity(0), 1e-6); - - joint->SetPosition(0, pos); - EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); - - joint->SetMinPosition(0, -1e6); - joint->SetMaxPosition(0, 1e6); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetForce(0, 1000); - world->Step(output, state, input); - } - EXPECT_LT(pos + 0.1, joint->GetPosition(0)); - EXPECT_NEAR(1, joint->GetVelocity(0), 1e-6); -} - -// TODO(anyone): position limits do not work very well with velocity control -// bug https://github.com/dartsim/dart/issues/1583 -// resolved in DART 6.11.0 -TEST_F(JointFeaturesFixture, DISABLED_JointSetPositionLimitsWithVelocityControl) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - const std::string modelName{"simple_joint_test"}; - const std::string jointName{"j1"}; - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - - auto model = world->GetModel(modelName); - auto joint = model->GetJoint(jointName); - - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - - world->Step(output, state, input); - - auto pos = joint->GetPosition(0); - - joint->SetMinPosition(0, pos - 0.1); - joint->SetMaxPosition(0, pos + 0.1); - for (std::size_t i = 0; i < 1000; ++i) - { - joint->SetVelocityCommand(0, 1); - world->Step(output, state, input); - - if (i % 500 == 499) - { - EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-2); - EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); - } - } -} - -TEST_F(JointFeaturesFixture, JointSetVelocityLimitsWithVelocityControl) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - auto model = world->GetModel("simple_joint_test"); - auto joint = model->GetJoint("j1"); - - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - - joint->SetMinVelocity(0, -0.1); - joint->SetMaxVelocity(0, 0.1); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetVelocityCommand(0, 1); - world->Step(output, state, input); - } - EXPECT_NEAR(0.1, joint->GetVelocity(0), 1e-6); - - for (std::size_t i = 0; i < 10; ++i) - { - joint->SetVelocityCommand(0, 0.1); - world->Step(output, state, input); - } - EXPECT_NEAR(0.1, joint->GetVelocity(0), 1e-6); - - for (std::size_t i = 0; i < 10; ++i) - { - joint->SetVelocityCommand(0, -0.025); - world->Step(output, state, input); - } - EXPECT_NEAR(-0.025, joint->GetVelocity(0), 1e-6); - - for (std::size_t i = 0; i < 10; ++i) - { - joint->SetVelocityCommand(0, -1); - world->Step(output, state, input); - } - EXPECT_NEAR(-0.1, joint->GetVelocity(0), 1e-6); - - joint->SetMinVelocity(0, -math::INF_D); - joint->SetMaxVelocity(0, math::INF_D); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetVelocityCommand(0, 1); - world->Step(output, state, input); - } - EXPECT_NEAR(1, joint->GetVelocity(0), 1e-6); -} - -TEST_F(JointFeaturesFixture, JointSetEffortLimitsWithVelocityControl) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - auto model = world->GetModel("simple_joint_test"); - auto joint = model->GetJoint("j1"); - - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - - joint->SetMinEffort(0, -1e-6); - joint->SetMaxEffort(0, 1e-6); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetVelocityCommand(0, 1); - world->Step(output, state, input); - } - EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); - - joint->SetMinEffort(0, -80); - joint->SetMaxEffort(0, 80); - - for (std::size_t i = 0; i < 100; ++i) - { - joint->SetVelocityCommand(0, -1); - world->Step(output, state, input); - } - EXPECT_NEAR(-1, joint->GetVelocity(0), 1e-6); - - joint->SetMinEffort(0, -math::INF_D); - joint->SetMaxEffort(0, math::INF_D); - - for (std::size_t i = 0; i < 10; ++i) - { - joint->SetVelocityCommand(0, -100); - world->Step(output, state, input); - } - EXPECT_NEAR(-100, joint->GetVelocity(0), 1e-6); -} - -TEST_F(JointFeaturesFixture, JointSetCombinedLimitsWithVelocityControl) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - auto model = world->GetModel("simple_joint_test"); - auto joint = model->GetJoint("j1"); - - // Test joint velocity command - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - - joint->SetMinVelocity(0, -0.5); - joint->SetMaxVelocity(0, 0.5); - joint->SetMinEffort(0, -1e-6); - joint->SetMaxEffort(0, 1e-6); - - for (std::size_t i = 0; i < 1000; ++i) - { - joint->SetVelocityCommand(0, 1); - world->Step(output, state, input); - } - EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); - - joint->SetMinEffort(0, -1e6); - joint->SetMaxEffort(0, 1e6); - - for (std::size_t i = 0; i < 1000; ++i) - { - joint->SetVelocityCommand(0, -1); - world->Step(output, state, input); - } - EXPECT_NEAR(-0.5, joint->GetVelocity(0), 1e-6); -} -#endif - -// Test detaching joints. -TEST_F(JointFeaturesFixture, JointDetach) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - const std::string modelName{"double_pendulum_with_base"}; - const std::string upperJointName{"upper_joint"}; - const std::string lowerJointName{"lower_joint"}; - const std::string upperLinkName{"upper_link"}; - const std::string lowerLinkName{"lower_link"}; - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - - auto model = world->GetModel(modelName); - auto upperLink = model->GetLink(upperLinkName); - auto lowerLink = model->GetLink(lowerLinkName); - auto upperJoint = model->GetJoint(upperJointName); - auto lowerJoint = model->GetJoint(lowerJointName); - - // test Cast*Joint functions - EXPECT_NE(nullptr, upperJoint->CastToRevoluteJoint()); - EXPECT_NE(nullptr, lowerJoint->CastToRevoluteJoint()); - EXPECT_EQ(nullptr, upperJoint->CastToFreeJoint()); - EXPECT_EQ(nullptr, lowerJoint->CastToFreeJoint()); - - dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); - ASSERT_NE(nullptr, dartWorld); - - const dart::dynamics::SkeletonPtr skeleton = - dartWorld->getSkeleton(modelName); - ASSERT_NE(nullptr, skeleton); - - const auto *dartUpperLink = skeleton->getBodyNode(upperLinkName); - const auto *dartLowerLink = skeleton->getBodyNode(lowerLinkName); - EXPECT_EQ("RevoluteJoint", dartUpperLink->getParentJoint()->getType()); - EXPECT_EQ("RevoluteJoint", dartLowerLink->getParentJoint()->getType()); - - const math::Pose3d initialUpperLinkPose(1, 0, 2.1, -GZ_PI/2, 0, 0); - const math::Pose3d initialLowerLinkPose(1.25, 1, 2.1, -2, 0, 0); - - EXPECT_EQ(initialUpperLinkPose, - math::eigen3::convert(dartUpperLink->getWorldTransform())); - EXPECT_EQ(initialLowerLinkPose, - math::eigen3::convert(dartLowerLink->getWorldTransform())); - - // detach lower joint - lowerJoint->Detach(); - EXPECT_EQ("FreeJoint", dartLowerLink->getParentJoint()->getType()); - EXPECT_NE(nullptr, lowerJoint->CastToFreeJoint()); - EXPECT_EQ(nullptr, lowerJoint->CastToRevoluteJoint()); - - // Detach() can be called again though it has no effect - lowerJoint->Detach(); - EXPECT_EQ("FreeJoint", dartLowerLink->getParentJoint()->getType()); - EXPECT_NE(nullptr, lowerJoint->CastToFreeJoint()); - EXPECT_EQ(nullptr, lowerJoint->CastToRevoluteJoint()); - - // expect poses to remain unchanged - EXPECT_EQ(initialUpperLinkPose, - math::eigen3::convert(dartUpperLink->getWorldTransform())); - EXPECT_EQ(initialLowerLinkPose, - math::eigen3::convert(dartLowerLink->getWorldTransform())); - - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - - const std::size_t numSteps = 10; - for (std::size_t i = 0; i < numSteps; ++i) - { - // step forward and expect lower link to fall - world->Step(output, state, input); - - // expect upper link to rotate - EXPECT_LT(upperJoint->GetVelocity(0), 0.0); - - // expect lower link to fall down without rotating - math::Vector3d lowerLinkLinearVelocity = - math::eigen3::convert(dartLowerLink->getLinearVelocity()); - EXPECT_NEAR(0.0, lowerLinkLinearVelocity.X(), 1e-10); - EXPECT_NEAR(0.0, lowerLinkLinearVelocity.Y(), 1e-10); - EXPECT_GT(0.0, lowerLinkLinearVelocity.Z()); - math::Vector3d lowerLinkAngularVelocity = - math::eigen3::convert(dartLowerLink->getAngularVelocity()); - EXPECT_EQ(math::Vector3d::Zero, lowerLinkAngularVelocity); - } - - // now detach the upper joint too, and ensure that velocity is preserved - math::Pose3d upperLinkPose = - math::eigen3::convert(dartUpperLink->getWorldTransform()); - math::Vector3d upperLinkLinearVelocity = - math::eigen3::convert(dartUpperLink->getLinearVelocity()); - math::Vector3d upperLinkAngularVelocity = - math::eigen3::convert(dartUpperLink->getAngularVelocity()); - // sanity check on velocity values - EXPECT_LT(1e-5, upperLinkLinearVelocity.Z()); - EXPECT_GT(-0.03, upperLinkAngularVelocity.X()); - EXPECT_NEAR(0.0, upperLinkLinearVelocity.X(), 1e-6); - EXPECT_NEAR(0.0, upperLinkLinearVelocity.Y(), 1e-6); - EXPECT_NEAR(0.0, upperLinkAngularVelocity.Y(), 1e-6); - EXPECT_NEAR(0.0, upperLinkAngularVelocity.Z(), 1e-6); - - upperJoint->Detach(); - EXPECT_EQ("FreeJoint", dartUpperLink->getParentJoint()->getType()); - EXPECT_NE(nullptr, upperJoint->CastToFreeJoint()); - EXPECT_EQ(nullptr, upperJoint->CastToRevoluteJoint()); - - EXPECT_EQ(upperLinkPose, - math::eigen3::convert(dartUpperLink->getWorldTransform())); - EXPECT_EQ(upperLinkLinearVelocity, - math::eigen3::convert(dartUpperLink->getLinearVelocity())); - EXPECT_EQ(upperLinkAngularVelocity, - math::eigen3::convert(dartUpperLink->getAngularVelocity())); -} - -///////////////////////////////////////////////// -// Attach a fixed joint between links that belong to different models -TEST_F(JointFeaturesFixture, JointAttachDetach) -{ - sdf::Root root; - const sdf::Errors errors = - root.Load(TEST_WORLD_DIR "joint_across_models.sdf"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); - ASSERT_NE(nullptr, dartWorld); - - const std::string modelName1{"M1"}; - const std::string modelName2{"M2"}; - const std::string bodyName{"body"}; - - auto model1 = world->GetModel(modelName1); - auto model2 = world->GetModel(modelName2); - auto model1Body = model1->GetLink(bodyName); - auto model2Body = model2->GetLink(bodyName); - - const dart::dynamics::SkeletonPtr skeleton1 = - dartWorld->getSkeleton(modelName1); - const dart::dynamics::SkeletonPtr skeleton2 = - dartWorld->getSkeleton(modelName2); - ASSERT_NE(nullptr, skeleton1); - ASSERT_NE(nullptr, skeleton2); - - auto *dartBody1 = skeleton1->getBodyNode(bodyName); - auto *dartBody2 = skeleton2->getBodyNode(bodyName); - - ASSERT_NE(nullptr, dartBody1); - ASSERT_NE(nullptr, dartBody2); - - const math::Pose3d initialModel1Pose(0, 0, 0.25, 0, 0, 0); - const math::Pose3d initialModel2Pose(0, 0, 3.0, 0, 0, 0); - - EXPECT_EQ(initialModel1Pose, - math::eigen3::convert(dartBody1->getWorldTransform())); - EXPECT_EQ(initialModel2Pose, - math::eigen3::convert(dartBody2->getWorldTransform())); - - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - - const std::size_t numSteps = 100; - for (std::size_t i = 0; i < numSteps; ++i) - { - world->Step(output, state, input); - - // Expect the model1 to stay at rest (since it's on the ground) and model2 - // to start falling - math::Vector3d body1LinearVelocity = - math::eigen3::convert(dartBody1->getLinearVelocity()); - math::Vector3d body2LinearVelocity = - math::eigen3::convert(dartBody2->getLinearVelocity()); - EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-7); - // Negative z velocity - EXPECT_GT(0.0, body2LinearVelocity.Z()); - } - - const auto poseParent = dartBody1->getTransform(); - const auto poseChild = dartBody2->getTransform(); - auto poseParentChild = poseParent.inverse() * poseChild; - - auto fixedJoint = model2Body->AttachFixedJoint(model1Body); - - // AttachFixedJoint snaps the child body to the origin of the parent, so we - // set a transform on the joint to keep the transform between the two bodies - // the same as it was before they were attached - fixedJoint->SetTransformFromParent(poseParentChild); - - // The name of the link obtained using the gz-physics API should remain the - // same even though AttachFixedJoint renames the associated BodyNode. - EXPECT_EQ(bodyName, model2Body->GetName()); - - for (std::size_t i = 0; i < numSteps; ++i) - { - world->Step(output, state, input); - - // Expect the model1 to remain at rest and model2 - // to stop moving - math::Vector3d body1LinearVelocity = - math::eigen3::convert(dartBody1->getLinearVelocity()); - math::Vector3d body2LinearVelocity = - math::eigen3::convert(dartBody2->getLinearVelocity()); - EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-7); - EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-7); - } - - // now detach joint and expect model2 to start moving again - fixedJoint->Detach(); - - // The name of the link obtained using the gz-physics API should remain the - // same even though Detach renames the associated BodyNode. - EXPECT_EQ(bodyName, model2Body->GetName()); - - for (std::size_t i = 0; i < numSteps; ++i) - { - world->Step(output, state, input); - - // Expect the model1 to remain at rest and model2 - // to start moving again - math::Vector3d body1LinearVelocity = - math::eigen3::convert(dartBody1->getLinearVelocity()); - math::Vector3d body2LinearVelocity = - math::eigen3::convert(dartBody2->getLinearVelocity()); - EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-7); - // Negative z velocity - EXPECT_GT(0.0, body2LinearVelocity.Z()); - } - - // After a while, body2 should reach the ground and come to a stop - for (std::size_t i = 0; i < 1000; ++i) - { - world->Step(output, state, input); - } - - EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3); -} - -//////////////////////////////////////////////// -// Essentially what happens is there are two floating boxes and a box in the -// middle that's resting. We start the system out by creating the two -// fixed joints between the boxes resting on the big box. The middle box will -// now have two parents. However there should be no movement as the middle box -// will be holding the other two boxes that are floating in mid air. We run -// this for 100 steps to make sure that there is no movement. This is because -// the middle box is holding on to the two side boxes. Then we release the -// joints the two boxes should fall away. -TEST_F(JointFeaturesFixture, JointAttachMultiple) -{ - sdf::Root root; - const sdf::Errors errors = - root.Load(TEST_WORLD_DIR "joint_constraint.sdf"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); - ASSERT_NE(nullptr, dartWorld); - - // M1 and M3 are floating boxes - const std::string modelName1{"M1"}; - const std::string modelName2{"M2"}; - const std::string modelName3{"M3"}; - const std::string bodyName{"link"}; - - auto model1 = world->GetModel(modelName1); - auto model2 = world->GetModel(modelName2); - auto model3 = world->GetModel(modelName3); - - auto model1Body = model1->GetLink(bodyName); - auto model2Body = model2->GetLink(bodyName); - auto model3Body = model3->GetLink(bodyName); - - const dart::dynamics::SkeletonPtr skeleton1 = - dartWorld->getSkeleton(modelName1); - const dart::dynamics::SkeletonPtr skeleton2 = - dartWorld->getSkeleton(modelName2); - const dart::dynamics::SkeletonPtr skeleton3 = - dartWorld->getSkeleton(modelName3); - ASSERT_NE(nullptr, skeleton1); - ASSERT_NE(nullptr, skeleton2); - ASSERT_NE(nullptr, skeleton3); - - auto *dartBody1 = skeleton1->getBodyNode(bodyName); - auto *dartBody2 = skeleton2->getBodyNode(bodyName); - auto *dartBody3 = skeleton3->getBodyNode(bodyName); - - ASSERT_NE(nullptr, dartBody1); - ASSERT_NE(nullptr, dartBody2); - ASSERT_NE(nullptr, dartBody3); - - const math::Pose3d initialModel1Pose(0, -0.2, 0.45, 0, 0, 0); - const math::Pose3d initialModel2Pose(0, 0.2, 0.45, 0, 0, 0); - const math::Pose3d initialModel3Pose(0, 0.6, 0.45, 0, 0, 0); - - EXPECT_EQ(initialModel1Pose, - math::eigen3::convert(dartBody1->getWorldTransform())); - EXPECT_EQ(initialModel2Pose, - math::eigen3::convert(dartBody2->getWorldTransform())); - EXPECT_EQ(initialModel3Pose, - math::eigen3::convert(dartBody3->getWorldTransform())); - - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - // 1 ms time step - const double dt = 0.001; - auto dur = std::chrono::duration(dt); - input.Get() = - std::chrono::duration_cast(dur); - - // Create the first joint. This should be a normal fixed joint. - const auto poseParent1 = dartBody1->getTransform(); - const auto poseChild1 = dartBody2->getTransform(); - auto poseParentChild1 = poseParent1.inverse() * poseChild1; - auto fixedJoint1 = model2Body->AttachFixedJoint(model1Body); - fixedJoint1->SetTransformFromParent(poseParentChild1); - - EXPECT_EQ(initialModel1Pose, - math::eigen3::convert(dartBody1->getWorldTransform())); - EXPECT_EQ(initialModel2Pose, - math::eigen3::convert(dartBody2->getWorldTransform())); - EXPECT_EQ(initialModel3Pose, - math::eigen3::convert(dartBody3->getWorldTransform())); - - // Create the second joint. This should be a WeldJoint constraint - const auto poseParent2 = dartBody3->getTransform(); - const auto poseChild2 = dartBody2->getTransform(); - auto poseParentChild2 = poseParent2.inverse() * poseChild2; - auto fixedJoint2 = model2Body->AttachFixedJoint(model3Body); - fixedJoint2->SetTransformFromParent(poseParentChild2); - - EXPECT_EQ(initialModel1Pose, - math::eigen3::convert(dartBody1->getWorldTransform())); - EXPECT_EQ(initialModel2Pose, - math::eigen3::convert(dartBody2->getWorldTransform())); - EXPECT_EQ(initialModel3Pose, - math::eigen3::convert(dartBody3->getWorldTransform())); - - const std::size_t numSteps = 100; - for (std::size_t i = 0; i < numSteps; ++i) - { - world->Step(output, state, input); - - // Expect all the bodies to be at rest. - // (since they're held in place by the joints) - math::Vector3d body1LinearVelocity = - math::eigen3::convert(dartBody1->getLinearVelocity()); - math::Vector3d body2LinearVelocity = - math::eigen3::convert(dartBody2->getLinearVelocity()); - math::Vector3d body3LinearVelocity = - math::eigen3::convert(dartBody3->getLinearVelocity()); - EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-7); - EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-7); - EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-7); - } - - // Detach the joints. M1 and M3 should fall as there is now nothing stopping - // them from falling. - fixedJoint1->Detach(); - fixedJoint2->Detach(); - - for (std::size_t i = 0; i < numSteps; ++i) - { - world->Step(output, state, input); - - // Expect the middle box to be still as it is already at rest. - // Expect the two side boxes to fall away. - math::Vector3d body1LinearVelocity = - math::eigen3::convert(dartBody1->getLinearVelocity()); - math::Vector3d body2LinearVelocity = - math::eigen3::convert(dartBody2->getLinearVelocity()); - math::Vector3d body3LinearVelocity = - math::eigen3::convert(dartBody3->getLinearVelocity()); - EXPECT_NEAR(dt * (i + 1) * -9.81, body1LinearVelocity.Z(), 1e-3); - EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-7); - EXPECT_NEAR(dt * (i + 1) * -9.81, body3LinearVelocity.Z(), 1e-3); - } -} - -///////////////////////////////////////////////// -// Expectations on number of links before/after attach/detach -TEST_F(JointFeaturesFixture, LinkCountsInJointAttachDetach) -{ - sdf::Root root; - const sdf::Errors errors = - root.Load(TEST_WORLD_DIR "joint_across_models.sdf"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); - ASSERT_NE(nullptr, dartWorld); - - const std::string modelName1{"M1"}; - const std::string modelName2{"M2"}; - const std::string bodyName{"body"}; - - auto model1 = world->GetModel(modelName1); - auto model2 = world->GetModel(modelName2); - auto model1Body = model1->GetLink(bodyName); - auto model2Body = model2->GetLink(bodyName); - - // Before attaching we expect each model to have 1 link - EXPECT_EQ(1u, model1->GetLinkCount()); - EXPECT_EQ(1u, model2->GetLinkCount()); - - auto fixedJoint = model2Body->AttachFixedJoint(model1Body); - - // After attaching we expect each model to have 1 link - EXPECT_EQ(1u, model1->GetLinkCount()); - EXPECT_EQ(1u, model2->GetLinkCount()); - - // now detach joint and expect model2 to start moving again - fixedJoint->Detach(); - // After detaching we expect each model to have 1 link - EXPECT_EQ(1u, model1->GetLinkCount()); - EXPECT_EQ(1u, model2->GetLinkCount()); - - // Test that a model with the same name as a link doesn't cause problems - const std::string modelName3{"body"}; - auto model3 = world->GetModel(modelName3); - EXPECT_EQ(1u, model3->GetLinkCount()); - - auto model3Body = model3->GetLink(bodyName); - auto fixedJoint2 = model3Body->AttachFixedJoint(model2Body); - EXPECT_EQ(1u, model2->GetLinkCount()); - fixedJoint2->Detach(); - // After detaching we expect each model to have 1 link - EXPECT_EQ(1u, model2->GetLinkCount()); - EXPECT_EQ(1u, model3->GetLinkCount()); -} - -///////////////////////////////////////////////// -// Attach a fixed joint between links that belong to different models where one -// of the models is created after a step is called -TEST_F(JointFeaturesFixture, JointAttachDetachSpawnedModel) -{ - std::string model1Str = R"( - - - 0 0 0.1 0 0 0 - - - - - 0.2 0.2 0.2 - - - - - - )"; - - std::string model2Str = R"( - - - 1 0 0.1 0 0 0 - - - - - 0.1 - - - - - - )"; - - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - - physics::World3dPtr world; - { - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "ground.sdf"); - ASSERT_TRUE(errors.empty()) << errors.front(); - world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - ASSERT_NE(nullptr, world); - } - - { - sdf::Root root; - sdf::Errors errors = root.LoadSdfString(model1Str); - ASSERT_TRUE(errors.empty()) << errors.front(); - ASSERT_NE(nullptr, root.Model()); - world->ConstructModel(*root.Model()); - } - - world->Step(output, state, input); - - { - sdf::Root root; - sdf::Errors errors = root.LoadSdfString(model2Str); - ASSERT_TRUE(errors.empty()) << errors.front(); - ASSERT_NE(nullptr, root.Model()); - world->ConstructModel(*root.Model()); - } - - const std::string modelName1{"M1"}; - const std::string modelName2{"M2"}; - const std::string bodyName1{"body"}; - const std::string bodyName2{"chassis"}; - - auto model1 = world->GetModel(modelName1); - auto model2 = world->GetModel(modelName2); - auto model1Body = model1->GetLink(bodyName1); - auto model2Body = model2->GetLink(bodyName2); - - dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); - ASSERT_NE(nullptr, dartWorld); - - const auto skeleton1 = dartWorld->getSkeleton(modelName1); - const auto skeleton2 = dartWorld->getSkeleton(modelName2); - ASSERT_NE(nullptr, skeleton1); - ASSERT_NE(nullptr, skeleton2); - - auto *dartBody1 = skeleton1->getBodyNode(bodyName1); - auto *dartBody2 = skeleton2->getBodyNode(bodyName2); - - ASSERT_NE(nullptr, dartBody1); - ASSERT_NE(nullptr, dartBody2); - - const auto poseParent = dartBody1->getTransform(); - const auto poseChild = dartBody2->getTransform(); - - // Before gz-physics PR #31, uncommenting the following `step` call makes - // this test pass, but commenting it out makes it fail. - // world->Step(output, state, input); - auto fixedJoint = model2Body->AttachFixedJoint(model1Body); - - // Pose of child relative to parent - auto poseParentChild = poseParent.inverse() * poseChild; - - // We let the joint be at the origin of the child link. - fixedJoint->SetTransformFromParent(poseParentChild); - - const std::size_t numSteps = 100; - - for (std::size_t i = 0; i < numSteps; ++i) - { - world->Step(output, state, input); - } - - // Expect both bodies to hit the ground and stop - EXPECT_NEAR(0.0, dartBody1->getLinearVelocity().z(), 1e-3); - EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3); - - fixedJoint->Detach(); - - for (std::size_t i = 0; i < numSteps; ++i) - { - world->Step(output, state, input); - } - - // Expect both bodies to remain in contact with the ground with zero velocity. - EXPECT_NEAR(0.0, dartBody1->getLinearVelocity().z(), 1e-3); - EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3); -} - -class JointTransmittedWrenchFixture : public JointFeaturesFixture -{ - public: using WorldPtr = physics::World3dPtr; - public: using ModelPtr = physics::Model3dPtr; - public: using JointPtr = physics::Joint3dPtr; - public: using LinkPtr = physics::Link3dPtr; - public: using Vector3d = physics::Vector3d; - public: using Wrench3d = physics::Wrench3d; - - protected: void SetUp() override - { - JointFeaturesFixture::SetUp(); - sdf::Root root; - const sdf::Errors errors = - root.Load(TEST_WORLD_DIR "pendulum_joint_wrench.sdf"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - this->world = this->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: physics::ForwardStep::Output output; - public: physics::ForwardStep::State state; - public: 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; -}; - -TEST_F(JointTransmittedWrenchFixture , PendulumAtZeroAngle) -{ - namespace test = physics::test; - - // Run a few steps for the constraint forces to stabilize - this->Step(10); - - // Test wrench expressed in different frames - { - auto wrenchAtMotorJoint = this->motorJoint->GetTransmittedWrench(); - Wrench3d expectedWrenchAtMotorJoint{ - Vector3d::Zero(), {-kGravity * (kArmLinkMass + kSensorLinkMass), 0, 0}}; - - EXPECT_TRUE( - test::Equal(expectedWrenchAtMotorJoint, wrenchAtMotorJoint, 1e-4)); - } - { - auto wrenchAtMotorJointInWorld = this->motorJoint->GetTransmittedWrench( - this->motorJoint->GetFrameID(), physics::FrameID::World()); - Wrench3d expectedWrenchAtMotorJointInWorld{ - Vector3d::Zero(), {0, 0, kGravity * (kArmLinkMass + kSensorLinkMass)}}; - - EXPECT_TRUE(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. - Wrench3d expectedWrenchAtMotorJointInArm{ - Vector3d::Zero(), {0, 0, kGravity * (kArmLinkMass + kSensorLinkMass)}}; - - EXPECT_TRUE(test::Equal(expectedWrenchAtMotorJointInArm, - wrenchAtMotorJointInArm, 1e-4)); - } -} - -TEST_F(JointTransmittedWrenchFixture, PendulumInMotion) -{ - namespace test = physics::test; - // 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 = - kArmLinkMass * ((alpha * kArmLength / 2.0) + (kGravity * sin(theta))); - - const double motorLinkTangentForce = - kSensorLinkMass * kGravity * sin(theta); - - const double armNormalForce = - -kArmLinkMass * - ((std::pow(omega, 2) * kArmLength / 2.0) + (kGravity * cos(theta))); - - const double motorLinkNormalForce = - -kSensorLinkMass * 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. - Wrench3d expectedWrenchAtMotorJointInJoint{ - Vector3d::Zero(), {normalForce, tangentForce, 0}}; - - EXPECT_TRUE(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(), 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)); - - Wrench3d expectedWrenchAtMotorJointInWorld{ - Vector3d::Zero(), R_WJ * wrenchAtMotorJointInJoint.force}; - EXPECT_TRUE(test::Equal(expectedWrenchAtMotorJointInWorld, - wrenchAtMotorJointInWorld, 1e-4)); - - // This moves the point of application and changes the coordinate frame - Wrench3d wrenchAtArmInArm = this->motorJoint->GetTransmittedWrench( - armLink->GetFrameID(), 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() = Vector3d(0, 0, kArmLength / 2.0); - Wrench3d expectedWrenchAtArmInArm; - - expectedWrenchAtArmInArm.force = - X_AJ.linear() * wrenchAtMotorJointInJoint.force; - - expectedWrenchAtArmInArm.torque = - X_AJ.linear() * wrenchAtMotorJointInJoint.torque + - X_AJ.translation().cross(expectedWrenchAtArmInArm.force); - - EXPECT_TRUE(test::Equal(expectedWrenchAtArmInArm, wrenchAtArmInArm, 1e-4)); - } -} - -// Compare wrench at the motor joint with wrench from the sensor joint (a -// fixed joint measuring only constraint forces). -TEST_F(JointTransmittedWrenchFixture, ValidateWrenchWithSecondaryJoint) -{ - namespace test = physics::test; - // 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. - Vector3d expectedTorqueDiff{0, 0, kSensorLinkMOI * alpha}; - Vector3d expectedForceDiff{-kSensorLinkMass * kGravity * cos(theta), - kSensorLinkMass * kGravity * sin(theta), 0}; - - Vector3d torqueDiff = - wrenchAtMotorJointInJoint.torque - wrenchAtSensorInSensor.torque; - Vector3d forceDiff = - wrenchAtMotorJointInJoint.force - wrenchAtSensorInSensor.force; - EXPECT_TRUE(test::Equal(expectedTorqueDiff, torqueDiff, 1e-4)); - EXPECT_TRUE(test::Equal(expectedForceDiff, forceDiff, 1e-4)); -} - -// Check that the transmitted wrench is affected by joint friction, stiffness -// and damping -TEST_F(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 -TEST_F(JointTransmittedWrenchFixture, ContactForces) -{ - auto box = this->world->GetModel("box"); - ASSERT_NE(nullptr, box); - auto boxFreeGroup = box->FindFreeGroup(); - ASSERT_NE(nullptr, boxFreeGroup); - 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 * kGravity * (kArmLinkMass + 2 * kSensorLinkMass); - - Wrench3d expectedWrenchAtMotorJointInJoint{ - Vector3d::Zero(), - {-reactionForceAtP * cos(theta), reactionForceAtP * sin(theta), 0}}; - - EXPECT_TRUE(physics::test::Equal(expectedWrenchAtMotorJointInJoint, - wrenchAtMotorJointInJoint, 1e-4)); -} diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index c8c4bff4a..d561e2a59 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -4,6 +4,7 @@ set(tests basic_test construct_empty_world free_joint_features + joint_features simulation_features ) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc new file mode 100644 index 000000000..468bd548f --- /dev/null +++ b/test/common_test/joint_features.cc @@ -0,0 +1,1830 @@ +/* + * 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 "../helpers/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 JointFeaturesTest: + public testing::Test, public gz::physics::TestLibLoader +{ + // Documentation inherited + public: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + + std::cerr << "JointFeaturesTest::GetLibToTest() " << JointFeaturesTest::GetLibToTest() << '\n'; + + loader.LoadLib(JointFeaturesTest::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 JointFeatureList : 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::sdf::ConstructSdfWorld +> { }; + +using JointFeaturesTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(JointFeaturesTest, + JointFeaturesTestTypes); + +TYPED_TEST(JointFeaturesTest, JointSetCommand) +{ + for (const std::string &name : this->pluginNames) + { + // TODO(ahcorde): reactive this test when test.world is working with bullet + if(this->PhysicsEngineName(name) == "bullet") + { + 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, "test.world")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + const std::string modelName{"double_pendulum_with_base"}; + const std::string jointName{"upper_joint"}; + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + auto model = world->GetModel(modelName); + auto joint = model->GetJoint(jointName); + + // EXPECT_EQ(dart::dynamics::Joint::FORCE, dartJoint->getActuatorType()); + + // Test joint velocity command + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + // Expect negative joint velocity after 1 step without joint command + world->Step(output, state, input); + EXPECT_LT(joint->GetVelocity(0), 0.0); + + auto base_link = model->GetLink("base"); + ASSERT_NE(nullptr, base_link); + + // Check that invalid velocity commands don't cause collisions to fail + for (std::size_t i = 0; i < 1000; ++i) + { + joint->SetForce(0, std::numeric_limits::quiet_NaN()); + // expect the position of the pendulum to stay above ground + world->Step(output, state, input); + auto frameData = base_link->FrameDataRelativeToWorld(); + EXPECT_NEAR(0.0, frameData.pose.translation().z(), 1e-3); + } + + joint->SetVelocityCommand(0, 1); + world->Step(output, state, input); + // Setting a velocity command changes the actuator type to SERVO + // EXPECT_EQ(dart::dynamics::Joint::SERVO, dartJoint->getActuatorType()); + + const std::size_t numSteps = 10; + for (std::size_t i = 0; i < numSteps; ++i) + { + // Call SetVelocityCommand before each step + joint->SetVelocityCommand(0, 1); + world->Step(output, state, input); + EXPECT_NEAR(1.0, joint->GetVelocity(0), 1e-6); + } + + for (std::size_t i = 0; i < numSteps; ++i) + { + // expect joint to freeze in subsequent steps without SetVelocityCommand + world->Step(output, state, input); + EXPECT_NEAR(0.0, joint->GetVelocity(0), 1e-6); + } + + // Check that invalid velocity commands don't cause collisions to fail + for (std::size_t i = 0; i < 1000; ++i) + { + joint->SetVelocityCommand(0, std::numeric_limits::quiet_NaN()); + // expect the position of the pendulum to stay aabove ground + world->Step(output, state, input); + auto frameData = base_link->FrameDataRelativeToWorld(); + EXPECT_NEAR(0.0, frameData.pose.translation().z(), 1e-3); + } + } +} + +struct JointFeaturePositionLimitsList : gz::physics::FeatureList< + gz::physics::ForwardStep, + gz::physics::GetBasicJointProperties, + gz::physics::GetBasicJointState, + gz::physics::GetEngineInfo, + gz::physics::GetJointFromModel, + gz::physics::GetModelFromWorld, + gz::physics::SetBasicJointState, + gz::physics::SetJointPositionLimitsFeature, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::sdf::ConstructSdfWorld +> { }; + +template +class JointFeaturesPositionLimitsTest : + public JointFeaturesTest{}; +using JointFeaturesPositionLimitsTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(JointFeaturesPositionLimitsTest, + JointFeaturesPositionLimitsTestTypes); + +TYPED_TEST(JointFeaturesPositionLimitsTest, JointSetPositionLimitsWithForceControl) +{ + 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, "test.world")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("simple_joint_test"); + auto joint = model->GetJoint("j1"); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + world->Step(output, state, input); + + auto pos = joint->GetPosition(0); + + joint->SetMinPosition(0, pos - 0.1); + joint->SetMaxPosition(0, pos + 0.1); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 100); + world->Step(output, state, input); + } + EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-3); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, -100); + world->Step(output, state, input); + } + EXPECT_NEAR(pos - 0.1, joint->GetPosition(0), 1e-3); + + joint->SetMinPosition(0, pos - 0.5); + joint->SetMaxPosition(0, pos + 0.5); + + for (std::size_t i = 0; i < 300; ++i) + { + joint->SetForce(0, 100); + world->Step(output, state, input); + } + EXPECT_NEAR(pos + 0.5, joint->GetPosition(0), 1e-2); + + for (std::size_t i = 0; i < 300; ++i) + { + joint->SetForce(0, -100); + world->Step(output, state, input); + } + EXPECT_NEAR(pos - 0.5, joint->GetPosition(0), 1e-2); + + joint->SetMinPosition(0, -gz::math::INF_D); + joint->SetMaxPosition(0, gz::math::INF_D); + joint->SetPosition(0, pos); + + for (std::size_t i = 0; i < 300; ++i) + { + joint->SetForce(0, 100); + world->Step(output, state, input); + } + EXPECT_LT(pos + 0.5, joint->GetPosition(0)); + } +} + +struct JointFeaturePositionLimitsForceControlList : gz::physics::FeatureList< + gz::physics::ForwardStep, + gz::physics::GetBasicJointProperties, + gz::physics::GetBasicJointState, + gz::physics::GetEngineInfo, + gz::physics::GetJointFromModel, + gz::physics::GetModelFromWorld, + // This feature is not requited but it will force to use dart6.10 which is required + // to run these tests + gz::physics::GetShapeFrictionPyramidSlipCompliance, + gz::physics::SetBasicJointState, + gz::physics::SetJointEffortLimitsFeature, + gz::physics::SetJointPositionLimitsFeature, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::SetJointVelocityLimitsFeature, + gz::physics::sdf::ConstructSdfWorld +> { }; + +template +class JointFeaturesPositionLimitsForceControlTest : + public JointFeaturesTest{}; +using JointFeaturesPositionLimitsForceControlTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(JointFeaturesPositionLimitsForceControlTest, + JointFeaturesPositionLimitsForceControlTestTypes); + +///////////// DARTSIM > 6.10 +TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetVelocityLimitsWithForceControl) +{ + 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, "test.world")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("simple_joint_test"); + auto joint = model->GetJoint("j1"); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + world->Step(output, state, input); + + joint->SetMinVelocity(0, -0.25); + joint->SetMaxVelocity(0, 0.5); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetForce(0, 1000); + world->Step(output, state, input); + } + EXPECT_NEAR(0.5, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetForce(0, -1000); + world->Step(output, state, input); + } + EXPECT_NEAR(-0.25, joint->GetVelocity(0), 1e-6); + + // set minimum velocity above zero + joint->SetMinVelocity(0, 0.25); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetForce(0, 0); + world->Step(output, state, input); + } + EXPECT_NEAR(0.25, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 10; ++i) + { + // make sure the minimum velocity is kept even without velocity commands + world->Step(output, state, input); + } + EXPECT_NEAR(0.25, joint->GetVelocity(0), 1e-6); + + joint->SetMinVelocity(0, -0.25); + joint->SetPosition(0, 0); + joint->SetVelocity(0, 0); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetForce(0, 0); + world->Step(output, state, input); + } + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + joint->SetMinVelocity(0, -gz::math::INF_D); + joint->SetMaxVelocity(0, gz::math::INF_D); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetForce(0, 1000); + world->Step(output, state, input); + } + EXPECT_LT(0.5, joint->GetVelocity(0)); + } +} + +TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetEffortLimitsWithForceControl) +{ + 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, "test.world")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("simple_joint_test"); + auto joint = model->GetJoint("j1"); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + world->Step(output, state, input); + + auto pos = joint->GetPosition(0); + + joint->SetMinEffort(0, -1e-6); + joint->SetMaxEffort(0, 1e-6); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 1); + world->Step(output, state, input); + } + EXPECT_NEAR(pos, joint->GetPosition(0), 1e-3); + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, -1); + world->Step(output, state, input); + } + EXPECT_NEAR(pos, joint->GetPosition(0), 1e-3); + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + joint->SetMinEffort(0, -80); + joint->SetMaxEffort(0, 80); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 1); + world->Step(output, state, input); + } + EXPECT_LT(pos, joint->GetPosition(0)); + EXPECT_LT(0, joint->GetVelocity(0)); + + joint->SetMinEffort(0, -gz::math::INF_D); + joint->SetMaxEffort(0, gz::math::INF_D); + joint->SetPosition(0, 0); + joint->SetVelocity(0, 0); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 1); + world->Step(output, state, input); + } + EXPECT_LT(pos, joint->GetPosition(0)); + EXPECT_LT(0, joint->GetVelocity(0)); + } +} + +TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetCombinedLimitsWithForceControl) +{ + 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, "test.world")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("simple_joint_test"); + auto joint = model->GetJoint("j1"); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + world->Step(output, state, input); + + auto pos = joint->GetPosition(0); + + joint->SetMinPosition(0, pos - 0.1); + joint->SetMaxPosition(0, pos + 0.1); + joint->SetMinVelocity(0, -0.25); + joint->SetMaxVelocity(0, 0.5); + joint->SetMinEffort(0, -1e-6); + joint->SetMaxEffort(0, 1e-6); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 100); + world->Step(output, state, input); + } + EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, -100); + world->Step(output, state, input); + } + EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + joint->SetMinEffort(0, -500); + joint->SetMaxEffort(0, 1000); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 1000); + world->Step(output, state, input); + } + // 0.05 because we go 0.1 s with max speed 0.5 + EXPECT_NEAR(pos + 0.05, joint->GetPosition(0), 1e-2); + EXPECT_NEAR(0.5, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 200; ++i) + { + joint->SetForce(0, 1000); + world->Step(output, state, input); + } + EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-2); + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + joint->SetPosition(0, pos); + EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); + + joint->SetMinVelocity(0, -1); + joint->SetMaxVelocity(0, 1); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 1000); + world->Step(output, state, input); + } + EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-2); + EXPECT_NEAR(1, joint->GetVelocity(0), 1e-6); + + joint->SetPosition(0, pos); + EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); + + joint->SetMinPosition(0, -1e6); + joint->SetMaxPosition(0, 1e6); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetForce(0, 1000); + world->Step(output, state, input); + } + EXPECT_LT(pos + 0.1, joint->GetPosition(0)); + EXPECT_NEAR(1, joint->GetVelocity(0), 1e-6); + } +} + +// TODO(anyone): position limits do not work very well with velocity control +// bug https://github.com/dartsim/dart/issues/1583 +// resolved in DART 6.11.0 +TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, DISABLED_JointSetPositionLimitsWithVelocityControl) +{ + 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, "test.world")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + const std::string modelName{"simple_joint_test"}; + const std::string jointName{"j1"}; + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + auto model = world->GetModel(modelName); + auto joint = model->GetJoint(jointName); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + world->Step(output, state, input); + + auto pos = joint->GetPosition(0); + + joint->SetMinPosition(0, pos - 0.1); + joint->SetMaxPosition(0, pos + 0.1); + for (std::size_t i = 0; i < 1000; ++i) + { + joint->SetVelocityCommand(0, 1); + world->Step(output, state, input); + + if (i % 500 == 499) + { + EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-2); + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + } + } + } +} + +TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetVelocityLimitsWithVelocityControl) +{ + 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, "test.world")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("simple_joint_test"); + auto joint = model->GetJoint("j1"); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + joint->SetMinVelocity(0, -0.1); + joint->SetMaxVelocity(0, 0.1); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetVelocityCommand(0, 1); + world->Step(output, state, input); + } + EXPECT_NEAR(0.1, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetVelocityCommand(0, 0.1); + world->Step(output, state, input); + } + EXPECT_NEAR(0.1, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetVelocityCommand(0, -0.025); + world->Step(output, state, input); + } + EXPECT_NEAR(-0.025, joint->GetVelocity(0), 1e-6); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetVelocityCommand(0, -1); + world->Step(output, state, input); + } + EXPECT_NEAR(-0.1, joint->GetVelocity(0), 1e-6); + + joint->SetMinVelocity(0, -gz::math::INF_D); + joint->SetMaxVelocity(0, gz::math::INF_D); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetVelocityCommand(0, 1); + world->Step(output, state, input); + } + EXPECT_NEAR(1, joint->GetVelocity(0), 1e-6); + } +} + +TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetEffortLimitsWithVelocityControl) +{ + 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, "test.world")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("simple_joint_test"); + auto joint = model->GetJoint("j1"); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + joint->SetMinEffort(0, -1e-6); + joint->SetMaxEffort(0, 1e-6); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetVelocityCommand(0, 1); + world->Step(output, state, input); + } + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + joint->SetMinEffort(0, -80); + joint->SetMaxEffort(0, 80); + + for (std::size_t i = 0; i < 100; ++i) + { + joint->SetVelocityCommand(0, -1); + world->Step(output, state, input); + } + EXPECT_NEAR(-1, joint->GetVelocity(0), 1e-6); + + joint->SetMinEffort(0, -gz::math::INF_D); + joint->SetMaxEffort(0, gz::math::INF_D); + + for (std::size_t i = 0; i < 10; ++i) + { + joint->SetVelocityCommand(0, -100); + world->Step(output, state, input); + } + EXPECT_NEAR(-100, joint->GetVelocity(0), 1e-6); + } +} + +TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetCombinedLimitsWithVelocityControl) +{ + 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, "test.world")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("simple_joint_test"); + auto joint = model->GetJoint("j1"); + + // Test joint velocity command + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + joint->SetMinVelocity(0, -0.5); + joint->SetMaxVelocity(0, 0.5); + joint->SetMinEffort(0, -1e-6); + joint->SetMaxEffort(0, 1e-6); + + for (std::size_t i = 0; i < 1000; ++i) + { + joint->SetVelocityCommand(0, 1); + world->Step(output, state, input); + } + EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); + + joint->SetMinEffort(0, -1e6); + joint->SetMaxEffort(0, 1e6); + + for (std::size_t i = 0; i < 1000; ++i) + { + joint->SetVelocityCommand(0, -1); + world->Step(output, state, input); + } + EXPECT_NEAR(-0.5, joint->GetVelocity(0), 1e-6); + } +} +///////////// DARTSIM > 6.10 end + + +struct JointFeatureDetachList : gz::physics::FeatureList< + gz::physics::DetachJointFeature, + gz::physics::ForwardStep, + gz::physics::FreeJointCast, + gz::physics::GetBasicJointProperties, + gz::physics::GetBasicJointState, + gz::physics::GetEngineInfo, + gz::physics::GetJointFromModel, + gz::physics::GetLinkFromModel, + gz::physics::GetModelFromWorld, + gz::physics::LinkFrameSemantics, + gz::physics::RevoluteJointCast, + gz::physics::SetBasicJointState, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::sdf::ConstructSdfWorld +> { }; + +template +class JointFeaturesDetachTest : + public JointFeaturesTest{}; +using JointFeaturesDetachTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(JointFeaturesDetachTest, + JointFeaturesDetachTestTypes); + +// Test detaching joints. +TYPED_TEST(JointFeaturesDetachTest, JointDetach) +{ + 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, "test.world")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + const std::string modelName{"double_pendulum_with_base"}; + const std::string upperJointName{"upper_joint"}; + const std::string lowerJointName{"lower_joint"}; + const std::string upperLinkName{"upper_link"}; + const std::string lowerLinkName{"lower_link"}; + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + auto model = world->GetModel(modelName); + auto upperLink = model->GetLink(upperLinkName); + auto lowerLink = model->GetLink(lowerLinkName); + auto upperJoint = model->GetJoint(upperJointName); + auto lowerJoint = model->GetJoint(lowerJointName); + + // test Cast*Joint functions + EXPECT_NE(nullptr, upperJoint->CastToRevoluteJoint()); + EXPECT_NE(nullptr, lowerJoint->CastToRevoluteJoint()); + EXPECT_EQ(nullptr, upperJoint->CastToFreeJoint()); + EXPECT_EQ(nullptr, lowerJoint->CastToFreeJoint()); + // + // dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); + // ASSERT_NE(nullptr, dartWorld); + // + // const dart::dynamics::SkeletonPtr skeleton = + // dartWorld->getSkeleton(modelName); + // ASSERT_NE(nullptr, skeleton); + // + // const auto *dartUpperLink = skeleton->getBodyNode(upperLinkName); + // const auto *dartLowerLink = skeleton->getBodyNode(lowerLinkName); + // EXPECT_EQ("RevoluteJoint", dartUpperLink->getParentJoint()->getType()); + // EXPECT_EQ("RevoluteJoint", dartLowerLink->getParentJoint()->getType()); + + const gz::math::Pose3d initialUpperLinkPose(1, 0, 2.1, -GZ_PI/2, 0, 0); + const gz::math::Pose3d initialLowerLinkPose(1.25, 1, 2.1, -2, 0, 0); + + auto frameDataUpperLink = upperLink->FrameDataRelativeToWorld(); + auto frameDataLowerLink = lowerLink->FrameDataRelativeToWorld(); + + EXPECT_EQ(initialUpperLinkPose, + gz::math::eigen3::convert(frameDataUpperLink.pose)); + EXPECT_EQ(initialLowerLinkPose, + gz::math::eigen3::convert(frameDataLowerLink.pose)); + + // detach lower joint + lowerJoint->Detach(); + // EXPECT_EQ("FreeJoint", dartLowerLink->getParentJoint()->getType()); + EXPECT_NE(nullptr, lowerJoint->CastToFreeJoint()); + EXPECT_EQ(nullptr, lowerJoint->CastToRevoluteJoint()); + + // Detach() can be called again though it has no effect + lowerJoint->Detach(); + // EXPECT_EQ("FreeJoint", dartLowerLink->getParentJoint()->getType()); + EXPECT_NE(nullptr, lowerJoint->CastToFreeJoint()); + EXPECT_EQ(nullptr, lowerJoint->CastToRevoluteJoint()); + + frameDataUpperLink = upperLink->FrameDataRelativeToWorld(); + frameDataLowerLink = lowerLink->FrameDataRelativeToWorld(); + + EXPECT_EQ(initialUpperLinkPose, + gz::math::eigen3::convert(frameDataUpperLink.pose)); + EXPECT_EQ(initialLowerLinkPose, + gz::math::eigen3::convert(frameDataLowerLink.pose)); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + const std::size_t numSteps = 10; + for (std::size_t i = 0; i < numSteps; ++i) + { + // step forward and expect lower link to fall + world->Step(output, state, input); + + // expect upper link to rotate + EXPECT_LT(upperJoint->GetVelocity(0), 0.0); + + frameDataLowerLink = lowerLink->FrameDataRelativeToWorld(); + + // expect lower link to fall down without rotating + gz::math::Vector3d lowerLinkLinearVelocity = + gz::math::eigen3::convert(frameDataLowerLink.linearVelocity); + EXPECT_NEAR(0.0, lowerLinkLinearVelocity.X(), 1e-10); + EXPECT_NEAR(0.0, lowerLinkLinearVelocity.Y(), 1e-10); + EXPECT_GT(0.0, lowerLinkLinearVelocity.Z()); + gz::math::Vector3d lowerLinkAngularVelocity = + gz::math::eigen3::convert(frameDataLowerLink.angularVelocity); + EXPECT_EQ(gz::math::Vector3d::Zero, lowerLinkAngularVelocity); + } + + frameDataUpperLink = upperLink->FrameDataRelativeToWorld(); + + // now detach the upper joint too, and ensure that velocity is preserved + gz::math::Pose3d upperLinkPose = + gz::math::eigen3::convert(frameDataUpperLink.pose); + gz::math::Vector3d upperLinkLinearVelocity = + gz::math::eigen3::convert(frameDataUpperLink.linearVelocity); + gz::math::Vector3d upperLinkAngularVelocity = + gz::math::eigen3::convert(frameDataUpperLink.angularVelocity); + // sanity check on velocity values + EXPECT_LT(1e-5, upperLinkLinearVelocity.Z()); + EXPECT_GT(-0.03, upperLinkAngularVelocity.X()); + EXPECT_NEAR(0.0, upperLinkLinearVelocity.X(), 1e-6); + EXPECT_NEAR(0.0, upperLinkLinearVelocity.Y(), 1e-6); + EXPECT_NEAR(0.0, upperLinkAngularVelocity.Y(), 1e-6); + EXPECT_NEAR(0.0, upperLinkAngularVelocity.Z(), 1e-6); + + upperJoint->Detach(); + // EXPECT_EQ("FreeJoint", dartUpperLink->getParentJoint()->getType()); + EXPECT_NE(nullptr, upperJoint->CastToFreeJoint()); + EXPECT_EQ(nullptr, upperJoint->CastToRevoluteJoint()); + + EXPECT_EQ(upperLinkPose, + gz::math::eigen3::convert(frameDataUpperLink.pose)); + EXPECT_EQ(upperLinkLinearVelocity, + gz::math::eigen3::convert(frameDataUpperLink.linearVelocity)); + EXPECT_EQ(upperLinkAngularVelocity, + gz::math::eigen3::convert(frameDataUpperLink.angularVelocity)); + } +} + +struct JointFeatureAttachDetachList : gz::physics::FeatureList< + gz::physics::AttachFixedJointFeature, + gz::physics::DetachJointFeature, + gz::physics::ForwardStep, + gz::physics::FreeJointCast, + gz::physics::GetBasicJointProperties, + gz::physics::GetBasicJointState, + gz::physics::GetEngineInfo, + gz::physics::GetJointFromModel, + gz::physics::GetLinkFromModel, + gz::physics::GetModelFromWorld, + gz::physics::LinkFrameSemantics, + gz::physics::RevoluteJointCast, + gz::physics::SetBasicJointState, + gz::physics::SetJointTransformFromParentFeature, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld +> { }; + +template +class JointFeaturesAttachDetachTest : + public JointFeaturesTest{}; +using JointFeaturesAttachDetachTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(JointFeaturesAttachDetachTest, + JointFeaturesAttachDetachTestTypes); +///////////////////////////////////////////////// +// Attach a fixed joint between links that belong to different models +TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetach) +{ + 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, "joint_across_models.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string bodyName{"body"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model1Body = model1->GetLink(bodyName); + auto model2Body = model2->GetLink(bodyName); + + auto frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + auto frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + const gz::math::Pose3d initialModel1Pose(0, 0, 0.25, 0, 0, 0); + const gz::math::Pose3d initialModel2Pose(0, 0, 3.0, 0, 0, 0); + + EXPECT_EQ(initialModel1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(initialModel2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + const std::size_t numSteps = 100; + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + // Expect the model1 to stay at rest (since it's on the ground) and model2 + // to start falling + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-7); + // Negative z velocity + EXPECT_GT(0.0, body2LinearVelocity.Z()); + } + + const auto poseParent = frameDataModel1Body.pose; + const auto poseChild = frameDataModel2Body.pose; + auto poseParentChild = poseParent.inverse() * poseChild; + + auto fixedJoint = model2Body->AttachFixedJoint(model1Body); + + // AttachFixedJoint snaps the child body to the origin of the parent, so we + // set a transform on the joint to keep the transform between the two bodies + // the same as it was before they were attached + fixedJoint->SetTransformFromParent(poseParentChild); + + // The name of the link obtained using the gz-physics API should remain the + // same even though AttachFixedJoint renames the associated BodyNode. + EXPECT_EQ(bodyName, model2Body->GetName()); + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + // Expect the model1 to remain at rest and model2 + // to stop moving + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-7); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-7); + } + + // now detach joint and expect model2 to start moving again + fixedJoint->Detach(); + + // The name of the link obtained using the gz-physics API should remain the + // same even though Detach renames the associated BodyNode. + EXPECT_EQ(bodyName, model2Body->GetName()); + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + // Expect the model1 to remain at rest and model2 + // to start moving again + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-7); + // Negative z velocity + EXPECT_GT(0.0, body2LinearVelocity.Z()); + } + + // After a while, body2 should reach the ground and come to a stop + for (std::size_t i = 0; i < 1000; ++i) + { + world->Step(output, state, input); + } + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); + } +} + +//////////////////////////////////////////////// +// Essentially what happens is there are two floating boxes and a box in the +// middle that's resting. We start the system out by creating the two +// fixed joints between the boxes resting on the big box. The middle box will +// now have two parents. However there should be no movement as the middle box +// will be holding the other two boxes that are floating in mid air. We run +// this for 100 steps to make sure that there is no movement. This is because +// the middle box is holding on to the two side boxes. Then we release the +// joints the two boxes should fall away. +TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) +{ + 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, "joint_constraint.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + // M1 and M3 are floating boxes + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string modelName3{"M3"}; + const std::string bodyName{"link"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model3 = world->GetModel(modelName3); + + auto model1Body = model1->GetLink(bodyName); + auto model2Body = model2->GetLink(bodyName); + auto model3Body = model3->GetLink(bodyName); + + const gz::math::Pose3d initialModel1Pose(0, -0.2, 0.45, 0, 0, 0); + const gz::math::Pose3d initialModel2Pose(0, 0.2, 0.45, 0, 0, 0); + const gz::math::Pose3d initialModel3Pose(0, 0.6, 0.45, 0, 0, 0); + + auto frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + auto frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + auto frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + EXPECT_EQ(initialModel1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(initialModel2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + EXPECT_EQ(initialModel3Pose, + gz::math::eigen3::convert(frameDataModel3Body.pose)); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + // 1 ms time step + const double dt = 0.001; + auto dur = std::chrono::duration(dt); + input.Get() = + std::chrono::duration_cast(dur); + + // Create the first joint. This should be a normal fixed joint. + const auto poseParent1 = frameDataModel1Body.pose; + const auto poseChild1 = frameDataModel2Body.pose; + auto poseParentChild1 = poseParent1.inverse() * poseChild1; + auto fixedJoint1 = model2Body->AttachFixedJoint(model1Body); + fixedJoint1->SetTransformFromParent(poseParentChild1); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + EXPECT_EQ(initialModel1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(initialModel2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + EXPECT_EQ(initialModel3Pose, + gz::math::eigen3::convert(frameDataModel3Body.pose)); + + // Create the second joint. This should be a WeldJoint constraint + const auto poseParent2 = frameDataModel3Body.pose; + const auto poseChild2 = frameDataModel2Body.pose; + auto poseParentChild2 = poseParent2.inverse() * poseChild2; + auto fixedJoint2 = model2Body->AttachFixedJoint(model3Body); + fixedJoint2->SetTransformFromParent(poseParentChild2); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + EXPECT_EQ(initialModel1Pose, + gz::math::eigen3::convert(frameDataModel1Body.pose)); + EXPECT_EQ(initialModel2Pose, + gz::math::eigen3::convert(frameDataModel2Body.pose)); + EXPECT_EQ(initialModel3Pose, + gz::math::eigen3::convert(frameDataModel3Body.pose)); + + const std::size_t numSteps = 100; + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect all the bodies to be at rest. + // (since they're held in place by the joints) + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-7); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-7); + EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-7); + } + + // Detach the joints. M1 and M3 should fall as there is now nothing stopping + // them from falling. + fixedJoint1->Detach(); + fixedJoint2->Detach(); + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + frameDataModel3Body = model3Body->FrameDataRelativeToWorld(); + + // Expect the middle box to be still as it is already at rest. + // Expect the two side boxes to fall away. + gz::math::Vector3d body1LinearVelocity = + gz::math::eigen3::convert(frameDataModel1Body.linearVelocity); + gz::math::Vector3d body2LinearVelocity = + gz::math::eigen3::convert(frameDataModel2Body.linearVelocity); + gz::math::Vector3d body3LinearVelocity = + gz::math::eigen3::convert(frameDataModel3Body.linearVelocity); + EXPECT_NEAR(dt * (i + 1) * -9.81, body1LinearVelocity.Z(), 1e-3); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-7); + EXPECT_NEAR(dt * (i + 1) * -9.81, body3LinearVelocity.Z(), 1e-3); + } + } +} + +///////////////////////////////////////////////// +// Expectations on number of links before/after attach/detach +TYPED_TEST(JointFeaturesAttachDetachTest, LinkCountsInJointAttachDetach) +{ + 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, "joint_across_models.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string bodyName{"body"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model1Body = model1->GetLink(bodyName); + auto model2Body = model2->GetLink(bodyName); + + // Before attaching we expect each model to have 1 link + EXPECT_EQ(1u, model1->GetLinkCount()); + EXPECT_EQ(1u, model2->GetLinkCount()); + + auto fixedJoint = model2Body->AttachFixedJoint(model1Body); + + // After attaching we expect each model to have 1 link + EXPECT_EQ(1u, model1->GetLinkCount()); + EXPECT_EQ(1u, model2->GetLinkCount()); + + // now detach joint and expect model2 to start moving again + fixedJoint->Detach(); + // After detaching we expect each model to have 1 link + EXPECT_EQ(1u, model1->GetLinkCount()); + EXPECT_EQ(1u, model2->GetLinkCount()); + + // Test that a model with the same name as a link doesn't cause problems + const std::string modelName3{"body"}; + auto model3 = world->GetModel(modelName3); + EXPECT_EQ(1u, model3->GetLinkCount()); + + auto model3Body = model3->GetLink(bodyName); + auto fixedJoint2 = model3Body->AttachFixedJoint(model2Body); + EXPECT_EQ(1u, model2->GetLinkCount()); + fixedJoint2->Detach(); + // After detaching we expect each model to have 1 link + EXPECT_EQ(1u, model2->GetLinkCount()); + EXPECT_EQ(1u, model3->GetLinkCount()); + } +} + +///////////////////////////////////////////////// +// Attach a fixed joint between links that belong to different models where one +// of the models is created after a step is called +TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachSpawnedModel) +{ + std::string model1Str = R"( + + + 0 0 0.1 0 0 0 + + + + + 0.2 0.2 0.2 + + + + + + )"; + + std::string model2Str = R"( + + + 1 0 0.1 0 0 0 + + + + + 0.1 + + + + + + )"; + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + gz::physics::World3dPtr world; + { + 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, "ground.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + world = engine->ConstructWorld(*root.WorldByIndex(0)); + ASSERT_NE(nullptr, world); + } + } + + { + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(model1Str); + ASSERT_TRUE(errors.empty()) << errors.front(); + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + } + + world->Step(output, state, input); + + { + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(model2Str); + ASSERT_TRUE(errors.empty()) << errors.front(); + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + } + + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string bodyName1{"body"}; + const std::string bodyName2{"chassis"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model1Body = model1->GetLink(bodyName1); + auto model2Body = model2->GetLink(bodyName2); + + auto frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + auto frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + const auto poseParent = frameDataModel1Body.pose; + const auto poseChild = frameDataModel2Body.pose; + + // Before gz-physics PR #31, uncommenting the following `step` call makes + // this test pass, but commenting it out makes it fail. + // world->Step(output, state, input); + auto fixedJoint = model2Body->AttachFixedJoint(model1Body); + + // Pose of child relative to parent + auto poseParentChild = poseParent.inverse() * poseChild; + + // We let the joint be at the origin of the child link. + fixedJoint->SetTransformFromParent(poseParentChild); + + const std::size_t numSteps = 100; + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + } + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + // Expect both bodies to hit the ground and stop + EXPECT_NEAR(0.0, frameDataModel1Body.linearVelocity.z(), 1e-3); + EXPECT_NEAR(0.0, frameDataModel2Body.linearVelocity.z(), 1e-3); + + fixedJoint->Detach(); + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + } + + frameDataModel1Body = model1Body->FrameDataRelativeToWorld(); + frameDataModel2Body = model2Body->FrameDataRelativeToWorld(); + + // Expect both bodies to remain in contact with the ground with zero velocity. + EXPECT_NEAR(0.0, frameDataModel1Body.linearVelocity.z(), 1e-3); + 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)); +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + if (!JointFeaturesTest::init( + argc, argv)) + return -1; + return RUN_ALL_TESTS(); +} diff --git a/dartsim/worlds/ground.sdf b/test/common_test/worlds/ground.sdf similarity index 100% rename from dartsim/worlds/ground.sdf rename to test/common_test/worlds/ground.sdf diff --git a/dartsim/worlds/joint_across_models.sdf b/test/common_test/worlds/joint_across_models.sdf similarity index 100% rename from dartsim/worlds/joint_across_models.sdf rename to test/common_test/worlds/joint_across_models.sdf diff --git a/dartsim/worlds/joint_constraint.sdf b/test/common_test/worlds/joint_constraint.sdf similarity index 100% rename from dartsim/worlds/joint_constraint.sdf rename to test/common_test/worlds/joint_constraint.sdf diff --git a/dartsim/worlds/pendulum_joint_wrench.sdf b/test/common_test/worlds/pendulum_joint_wrench.sdf similarity index 100% rename from dartsim/worlds/pendulum_joint_wrench.sdf rename to test/common_test/worlds/pendulum_joint_wrench.sdf 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 + + + + From af451941dd96f746ebe9927a1553634d94c49f3e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 5 Sep 2022 15:33:30 +0200 Subject: [PATCH 05/13] Added kinematic features common tests (#409) Signed-off-by: ahcorde --- dartsim/src/KinematicsFeatures_TEST.cc | 126 -------------- test/common_test/CMakeLists.txt | 1 + test/common_test/kinematic_features.cc | 157 ++++++++++++++++++ .../common_test}/worlds/string_pendulum.sdf | 0 4 files changed, 158 insertions(+), 126 deletions(-) delete mode 100644 dartsim/src/KinematicsFeatures_TEST.cc create mode 100644 test/common_test/kinematic_features.cc rename {dartsim => test/common_test}/worlds/string_pendulum.sdf (100%) diff --git a/dartsim/src/KinematicsFeatures_TEST.cc b/dartsim/src/KinematicsFeatures_TEST.cc deleted file mode 100644 index 632a3d877..000000000 --- a/dartsim/src/KinematicsFeatures_TEST.cc +++ /dev/null @@ -1,126 +0,0 @@ -/* - * Copyright (C) 2021 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 - -// Features -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "test/Utils.hh" - -using namespace gz; - -using TestFeatureList = gz::physics::FeatureList< - physics::ForwardStep, - physics::GetEntities, - physics::JointFrameSemantics, - physics::LinkFrameSemantics, - physics::sdf::ConstructSdfModel, - physics::sdf::ConstructSdfWorld ->; - -using TestEnginePtr = physics::Engine3dPtr; - -class KinematicsFeaturesFixture : public ::testing::Test -{ - protected: void SetUp() override - { - gz::plugin::Loader loader; - loader.LoadLib(dartsim_plugin_LIB); - - gz::plugin::PluginPtr dartsim = - loader.Instantiate("gz::physics::dartsim::Plugin"); - - this->engine = - gz::physics::RequestEngine3d::From(dartsim); - ASSERT_NE(nullptr, this->engine); - } - protected: TestEnginePtr engine; -}; - -// Test joint frame semantics -TEST_F(KinematicsFeaturesFixture, JointFrameSemantics) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "string_pendulum.sdf"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - ASSERT_NE(nullptr, world); - - auto model = world->GetModel("pendulum"); - ASSERT_NE(nullptr, model); - auto pivotJoint = model->GetJoint("pivot"); - ASSERT_NE(nullptr, pivotJoint); - auto childLink = model->GetLink("bob"); - ASSERT_NE(nullptr, childLink); - - physics::ForwardStep::Output output; - physics::ForwardStep::State state; - physics::ForwardStep::Input input; - - for (std::size_t i = 0; i < 100; ++i) - { - world->Step(output, state, input); - } - // Pose of Child link (C) in Joint frame (J) - physics::Pose3d X_JC = physics::Pose3d::Identity(); - X_JC.translate(physics::Vector3d(0, 0, -1)); - - // Notation: Using F_WJ for the frame data of frame J (joint) relative to - // frame W (world). - auto F_WJ = pivotJoint->FrameDataRelativeToWorld(); - physics::FrameData3d F_WCexpected = F_WJ; - - physics::Vector3d pendulumArmInWorld = - F_WJ.pose.rotation() * X_JC.translation(); - - F_WCexpected.pose = F_WJ.pose * X_JC; - // angular acceleration of the child link is the same as the joint, so we - // don't need to assign a new value. - - // Note that the joint's linear velocity and linear acceleration are zero, so - // they are omitted here. - F_WCexpected.linearAcceleration = - F_WJ.angularAcceleration.cross(pendulumArmInWorld) + - F_WJ.angularVelocity.cross( - F_WJ.angularVelocity.cross(pendulumArmInWorld)); - - F_WCexpected.linearVelocity = F_WJ.angularVelocity.cross(pendulumArmInWorld); - - auto childLinkFrameData = childLink->FrameDataRelativeToWorld(); - EXPECT_TRUE( - physics::test::Equal(F_WCexpected, childLinkFrameData, 1e-6)); -} diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index d561e2a59..152128e7f 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -5,6 +5,7 @@ set(tests construct_empty_world free_joint_features joint_features + kinematic_features simulation_features ) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc new file mode 100644 index 000000000..19baff522 --- /dev/null +++ b/test/common_test/kinematic_features.cc @@ -0,0 +1,157 @@ +/* + * 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 +#include +#include + +#include + +template +class KinematicFeaturesTest: + public testing::Test, public gz::physics::TestLibLoader +{ + // Documentation inherited + public: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + + loader.LoadLib(KinematicFeaturesTest::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(); + } + // TODO(ahcorde): SKIP bullet, review this test again. + for (const std::string &name : this->pluginNames) + { + if(this->PhysicsEngineName(name) == "bullet") + { + GTEST_SKIP(); + } + } + } + + public: std::set pluginNames; + public: gz::plugin::Loader loader; +}; + +struct KinematicFeaturesList : gz::physics::FeatureList< + gz::physics::GetEngineInfo, + gz::physics::ForwardStep, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetModelFromWorld, + gz::physics::GetLinkFromModel, + gz::physics::GetJointFromModel, + gz::physics::LinkFrameSemantics, + gz::physics::JointFrameSemantics +> { }; + +using KinematicFeaturesTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(KinematicFeaturesTest, + KinematicFeaturesTestTypes); + +TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics) +{ + 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, "string_pendulum.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + ASSERT_NE(nullptr, world); + + auto model = world->GetModel("pendulum"); + ASSERT_NE(nullptr, model); + auto pivotJoint = model->GetJoint("pivot"); + ASSERT_NE(nullptr, pivotJoint); + auto childLink = model->GetLink("bob"); + ASSERT_NE(nullptr, childLink); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + for (std::size_t i = 0; i < 100; ++i) + { + world->Step(output, state, input); + } + // Pose of Child link (C) in Joint frame (J) + gz::physics::Pose3d X_JC = gz::physics::Pose3d::Identity(); + X_JC.translate(gz::physics::Vector3d(0, 0, -1)); + + // Notation: Using F_WJ for the frame data of frame J (joint) relative to + // frame W (world). + auto F_WJ = pivotJoint->FrameDataRelativeToWorld(); + gz::physics::FrameData3d F_WCexpected = F_WJ; + + gz::physics::Vector3d pendulumArmInWorld = + F_WJ.pose.rotation() * X_JC.translation(); + + F_WCexpected.pose = F_WJ.pose * X_JC; + // angular acceleration of the child link is the same as the joint, so we + // don't need to assign a new value. + + // Note that the joint's linear velocity and linear acceleration are zero, so + // they are omitted here. + F_WCexpected.linearAcceleration = + F_WJ.angularAcceleration.cross(pendulumArmInWorld) + + F_WJ.angularVelocity.cross( + F_WJ.angularVelocity.cross(pendulumArmInWorld)); + + F_WCexpected.linearVelocity = F_WJ.angularVelocity.cross(pendulumArmInWorld); + + auto childLinkFrameData = childLink->FrameDataRelativeToWorld(); + EXPECT_TRUE( + gz::physics::test::Equal(F_WCexpected, childLinkFrameData, 1e-6)); + } +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + if (!KinematicFeaturesTest::init( + argc, argv)) + return -1; + return RUN_ALL_TESTS(); +} diff --git a/dartsim/worlds/string_pendulum.sdf b/test/common_test/worlds/string_pendulum.sdf similarity index 100% rename from dartsim/worlds/string_pendulum.sdf rename to test/common_test/worlds/string_pendulum.sdf From 0a9ee2b7095a289d16c5b07c6761284bd49e5694 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 6 Sep 2022 18:29:22 +0200 Subject: [PATCH 06/13] Added collisions common tests (#410) * Added collisions common tests Signed-off-by: ahcorde --- dartsim/src/Collisions_TEST.cc | 157 -------------------------------- test/common_test/CMakeLists.txt | 5 + test/common_test/collisions.cc | 151 ++++++++++++++++++++++++++++++ 3 files changed, 156 insertions(+), 157 deletions(-) delete mode 100644 dartsim/src/Collisions_TEST.cc create mode 100644 test/common_test/collisions.cc diff --git a/dartsim/src/Collisions_TEST.cc b/dartsim/src/Collisions_TEST.cc deleted file mode 100644 index f5176fd49..000000000 --- a/dartsim/src/Collisions_TEST.cc +++ /dev/null @@ -1,157 +0,0 @@ -/* - * Copyright (C) 2019 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 - -// Features -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -using TestFeatureList = gz::physics::FeatureList< - gz::physics::LinkFrameSemantics, - gz::physics::ForwardStep, - gz::physics::GetEntities, - gz::physics::ConstructEmptyWorldFeature, - gz::physics::ConstructEmptyModelFeature, - gz::physics::ConstructEmptyLinkFeature, - gz::physics::mesh::AttachMeshShapeFeature, - gz::physics::AttachPlaneShapeFeature, - gz::physics::SetFreeJointRelativeTransformFeature, - gz::physics::AttachFixedJointFeature ->; - -using TestWorldPtr = gz::physics::World3dPtr; -using TestEnginePtr = gz::physics::Engine3dPtr; - -using WorldConstructor = std::function; - -std::unordered_set LoadWorlds( - const std::string &_library, - const WorldConstructor &_constructor) -{ - gz::plugin::Loader loader; - loader.LoadLib(_library); - - const std::set pluginNames = - gz::physics::FindFeatures3d::From(loader); - - std::unordered_set worlds; - for (const std::string &name : pluginNames) - { - gz::plugin::PluginPtr plugin = loader.Instantiate(name); - - std::cout << " -- Plugin name: " << name << std::endl; - - auto engine = - gz::physics::RequestEngine3d::From(plugin); - EXPECT_NE(nullptr, engine); - - worlds.insert(_constructor(engine)); - } - - return worlds; -} - -class Collisions_TEST - : public ::testing::Test, - public ::testing::WithParamInterface -{}; - -INSTANTIATE_TEST_SUITE_P(PhysicsPlugins, Collisions_TEST, - ::testing::ValuesIn(gz::physics::test::g_PhysicsPluginLibraries)); - -TestWorldPtr ConstructMeshPlaneWorld( - const gz::physics::Engine3dPtr &_engine, - const gz::common::Mesh &_mesh) -{ - auto world = _engine->ConstructEmptyWorld("world"); - - Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); - tf.translation()[2] = 2.0; - - auto model = world->ConstructEmptyModel("mesh"); - auto link = model->ConstructEmptyLink("link"); - // TODO(anyone): This test is somewhat awkward because we lift up the mesh - // from the center of the link instead of lifting up the link or the model. - // We're doing this because we don't currently have an API for moving models - // or links around. See the conversation here for more: - // https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/46/page/1#comment-87592809 - link->AttachMeshShape("mesh", _mesh, tf); - - model = world->ConstructEmptyModel("plane"); - link = model->ConstructEmptyLink("link"); - - link->AttachPlaneShape("plane", gz::physics::LinearVector3d::UnitZ()); - link->AttachFixedJoint(nullptr); - - return world; -} - -TEST_P(Collisions_TEST, MeshAndPlane) -{ - const std::string library = GetParam(); - if (library.empty()) - return; - - const std::string meshFilename = GZ_PHYSICS_RESOURCE_DIR "/chassis.dae"; - auto &meshManager = *gz::common::MeshManager::Instance(); - auto *mesh = meshManager.Load(meshFilename); - - std::cout << "Testing library " << library << std::endl; - auto worlds = LoadWorlds(library, [&](const TestEnginePtr &_engine) - { - return ConstructMeshPlaneWorld(_engine, *mesh); - }); - - for (const auto &world : worlds) - { - const auto link = world->GetModel(0)->GetLink(0); - - EXPECT_NEAR( - 0.0, link->FrameDataRelativeToWorld().pose.translation()[2], 1e-6); - - gz::physics::ForwardStep::Output output; - gz::physics::ForwardStep::State state; - gz::physics::ForwardStep::Input input; - for (std::size_t i = 0; i < 1000; ++i) - { - world->Step(output, state, input); - } - - // Make sure the mesh was stopped by the plane. In 2000 time steps at the - // default step size of 0.001, a free falling body should drop approximately - // 19.6 meters. As long as the body is somewhere near 1.91, then it has been - // stopped by the plane (the exact value might vary because the body might - // be rocking side-to-side after falling). - EXPECT_NEAR( - -1.91, link->FrameDataRelativeToWorld().pose.translation()[2], 0.05); - } -} diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 152128e7f..61de58950 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -2,6 +2,7 @@ set(TEST_TYPE "COMMON_TEST") set(tests basic_test + collisions construct_empty_world free_joint_features joint_features @@ -17,6 +18,8 @@ function(configure_common_test PHYSICS_ENGINE_NAME test_name) ) endfunction() +set(GZ_PHYSICS_RESOURCE_DIR "${CMAKE_SOURCE_DIR}/resources") + foreach(test ${tests}) add_executable(${TEST_TYPE}_${test} ${test}.cc) @@ -26,6 +29,7 @@ foreach(test ${tests}) gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} ${PROJECT_LIBRARY_TARGET_NAME} ${PROJECT_LIBRARY_TARGET_NAME}-sdf + ${PROJECT_LIBRARY_TARGET_NAME}-mesh gtest gtest_main ${PROJECT_NAME}_test_lib_loader @@ -33,6 +37,7 @@ foreach(test ${tests}) target_compile_definitions(${TEST_TYPE}_${test} PRIVATE "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\"" + "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\"" ) if (${BULLET_FOUND}) diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc new file mode 100644 index 000000000..cec88e309 --- /dev/null +++ b/test/common_test/collisions.cc @@ -0,0 +1,151 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +// #include + +template +class CollisionTest: + public testing::Test, public gz::physics::TestLibLoader +{ + // Documentation inherited + public: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + + loader.LoadLib(CollisionTest::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; +}; + + +struct CollisionFeaturesList : gz::physics::FeatureList< + gz::physics::sdf::ConstructSdfWorld, + gz::physics::LinkFrameSemantics, + gz::physics::ForwardStep, + gz::physics::GetEntities, + gz::physics::ConstructEmptyWorldFeature, + gz::physics::ConstructEmptyModelFeature, + gz::physics::ConstructEmptyLinkFeature, + gz::physics::mesh::AttachMeshShapeFeature, + gz::physics::AttachPlaneShapeFeature, + gz::physics::SetFreeJointRelativeTransformFeature, + gz::physics::AttachFixedJointFeature +> { }; + +using CollisionTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(CollisionTest, + CollisionTestTypes); + +TYPED_TEST(CollisionTest, MeshAndPlane) +{ + 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); + + auto world = engine->ConstructEmptyWorld("world"); + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + tf.translation()[2] = 2.0; + + auto model = world->ConstructEmptyModel("mesh"); + auto link = model->ConstructEmptyLink("link"); + + const std::string meshFilename = gz::common::joinPaths( + GZ_PHYSICS_RESOURCE_DIR, "chassis.dae"); + auto &meshManager = *gz::common::MeshManager::Instance(); + auto *mesh = meshManager.Load(meshFilename); + + // TODO(anyone): This test is somewhat awkward because we lift up the mesh + // from the center of the link instead of lifting up the link or the model. + // We're doing this because we don't currently have an API for moving models + // or links around. See the conversation here for more: + // https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/46/page/1#comment-87592809 + link->AttachMeshShape("mesh", *mesh, tf); + + model = world->ConstructEmptyModel("plane"); + link = model->ConstructEmptyLink("link"); + + link->AttachPlaneShape("plane", gz::physics::LinearVector3d::UnitZ()); + link->AttachFixedJoint(nullptr); + + const auto link2 = world->GetModel(0)->GetLink(0); + + EXPECT_NEAR( + 0.0, link2->FrameDataRelativeToWorld().pose.translation()[2], 1e-6); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + for (std::size_t i = 0; i < 1000; ++i) + { + world->Step(output, state, input); + } + + // Make sure the mesh was stopped by the plane. In 2000 time steps at the + // default step size of 0.001, a free falling body should drop approximately + // 19.6 meters. As long as the body is somewhere near 1.91, then it has been + // stopped by the plane (the exact value might vary because the body might + // be rocking side-to-side after falling). + EXPECT_NEAR( + -1.91, link2->FrameDataRelativeToWorld().pose.translation()[2], 0.05); + } +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + if (!CollisionTest::init( + argc, argv)) + return -1; + return RUN_ALL_TESTS(); +} From e026533c996e866b5e0361e570b35ec4858311f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 7 Sep 2022 15:11:34 +0200 Subject: [PATCH 07/13] Revert "UPdate Codecov ignored files (#406)" (#415) This reverts commit e36166d969ceb04fe7e677cc6805cbbb9eb4728e. --- codecov.yml | 3 --- 1 file changed, 3 deletions(-) diff --git a/codecov.yml b/codecov.yml index 7604339af..df92ff2d0 100644 --- a/codecov.yml +++ b/codecov.yml @@ -1,5 +1,2 @@ ignore: - "tpe/lib/src/aabb_tree" - - test/* - - examples/* - - "**/*_TEST.cc" From 7aa85e0b3dd99218f8ef44f3ea2cd2a00316af21 Mon Sep 17 00:00:00 2001 From: Joan Aguilar Mayans Date: Wed, 7 Sep 2022 16:45:44 -0700 Subject: [PATCH 08/13] Updates to installation documentation (#416) - Added instructions to install packages `gnupg`, `wget`, and `cppcheck`. - Updated references to packages/branches. Note that since this is pointing to a pre-release, I had to use `ubuntu-nightly` rather than `ubuntu-stable` for the instructions to work. I'm assuming that this will resolve itself once the prerelease becomes a proper release. Signed-off-by: Joan Aguilar Mayans Co-authored-by: Michael Carroll --- tutorials/02_installation.md | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/tutorials/02_installation.md b/tutorials/02_installation.md index 5de68e93f..6c724d0f8 100644 --- a/tutorials/02_installation.md +++ b/tutorials/02_installation.md @@ -13,10 +13,10 @@ The Source Installation instructions are generally recommended for developers wh Ubuntu Focal or later. -If you don't already have the `lsb-release` package installed, please do so now: +If you don't already have the packages `gnupg`, `lsb-release`, or `wget` installed, please do so now: ``` sudo apt-get update -sudo apt-get install lsb-release +sudo apt-get install gnupg lsb-release wget ``` Setup your computer to accept software from @@ -44,15 +44,17 @@ Be sure to replace `<#>` with a number value, such as `1` or `2`, depending on w 1. Install dependencies ``` sudo apt-add-repository -s "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -c -s) main" - sudo apt-get build-dep -y gz-physics<#>-dev + sudo apt-get build-dep -y libgz-physics<#>-dev ``` - Be sure to replace `<#>` with a number value, such as `1` or `2`, depending on which version you need. + Be sure to replace `<#>` with a number value, such as `5` or `6`, depending on which version you need. + From version `6` onward, you should use `libgz-physics<#>-dev`; for lower versions, `libignition-physics<#>-dev`. 2. Clone the repository ``` - git clone https://github.com/gazebosim/gz-physics -b ign-physics<#> + git clone https://github.com/gazebosim/gz-physics -b gz-physics<#> ``` - Be sure to replace `<#>` with a number value, such as `1` or `2`, depending on which version you need. + Be sure to replace `<#>` with a number value, such as `5` or `6`, depending on which version you need. + From version `6` onward, you should use `gz-physics<#>`; for lower versions, `ign-physics<#>`. 3. Configure and build ``` @@ -225,7 +227,16 @@ Follow these steps to run tests and static code analysis in your clone of this r make test ``` -3. Static code checker. +3. You will need Cppcheck in order to run static code checks. On Ubuntu Cppcheck can be installed using + ``` + sudo apt-get install cppcheck + ``` + +4. Configure and run the static code checker. ``` + cd gz-physics + mkdir build + cd build + cmake .. make codecheck ``` From 01654fc14cff2e653359c4cc7a3e8d552f38326e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 8 Sep 2022 20:54:28 +0200 Subject: [PATCH 09/13] Added link features common tests (#411) Signed-off-by: ahcorde --- dartsim/src/LinkFeatures_TEST.cc | 355 ------------------------- dartsim/worlds/contact.sdf | 93 ------- test/common_test/CMakeLists.txt | 1 + test/common_test/link_features.cc | 416 ++++++++++++++++++++++++++++++ test/common_test/worlds/empty.sdf | 5 + 5 files changed, 422 insertions(+), 448 deletions(-) delete mode 100644 dartsim/src/LinkFeatures_TEST.cc delete mode 100644 dartsim/worlds/contact.sdf create mode 100644 test/common_test/link_features.cc create mode 100644 test/common_test/worlds/empty.sdf diff --git a/dartsim/src/LinkFeatures_TEST.cc b/dartsim/src/LinkFeatures_TEST.cc deleted file mode 100644 index 28f47556e..000000000 --- a/dartsim/src/LinkFeatures_TEST.cc +++ /dev/null @@ -1,355 +0,0 @@ -/* - * Copyright (C) 2019 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 - -// Features -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "test/Utils.hh" - -struct TestFeatureList : gz::physics::FeatureList< - gz::physics::AddLinkExternalForceTorque, - gz::physics::ForwardStep, - gz::physics::Gravity, - gz::physics::sdf::ConstructSdfWorld, - gz::physics::sdf::ConstructSdfModel, - gz::physics::sdf::ConstructSdfLink, - gz::physics::GetEntities, - gz::physics::GetLinkBoundingBox, - gz::physics::GetModelBoundingBox -> { }; - -using namespace gz; - -using TestEnginePtr = physics::Engine3dPtr; -using TestWorldPtr = physics::World3dPtr; - -class LinkFeaturesFixture : public ::testing::Test -{ - protected: void SetUp() override - { - gz::plugin::Loader loader; - loader.LoadLib(dartsim_plugin_LIB); - - gz::plugin::PluginPtr dartsim = - loader.Instantiate("gz::physics::dartsim::Plugin"); - - this->engine = - gz::physics::RequestEngine3d::From(dartsim); - ASSERT_NE(nullptr, this->engine); - } - protected: TestEnginePtr engine; -}; - -// 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; -}; - -TestWorldPtr LoadWorld( - const TestEnginePtr &_engine, - const std::string &_sdfFile, - const Eigen::Vector3d &_gravity = Eigen::Vector3d{0, 0, -9.8}) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(_sdfFile); - 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; - - TestWorldPtr world = _engine->ConstructWorld(*sdfWorld); - EXPECT_NE(nullptr, world); - world->SetGravity(_gravity); - - AssertVectorApprox vectorPredicate(1e-10); - EXPECT_PRED_FORMAT2(vectorPredicate, _gravity, - world->GetGravity()); - - return world; -} - -// Test setting force and torque. -TEST_F(LinkFeaturesFixture, LinkForceTorque) -{ - auto world = LoadWorld(this->engine, TEST_WORLD_DIR "/empty.sdf", - Eigen::Vector3d::Zero()); - { - AssertVectorApprox vectorPredicate(1e-10); - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - world->GetGravity()); - } - - // Add a sphere - sdf::Model modelSDF; - modelSDF.SetName("sphere"); - modelSDF.SetRawPose(math::Pose3d(0, 0, 2, 0, 0, GZ_PI)); - auto model = world->ConstructModel(modelSDF); - - const double mass = 1.0; - math::MassMatrix3d massMatrix{mass, math::Vector3d{0.4, 0.4, 0.4}, - math::Vector3d::Zero}; - - sdf::Link linkSDF; - linkSDF.SetName("sphere_link"); - linkSDF.SetInertial({massMatrix, math::Pose3d::Zero}); - auto link = model->ConstructLink(linkSDF); - - gz::physics::ForwardStep::Input input; - gz::physics::ForwardStep::State state; - gz::physics::ForwardStep::Output output; - - AssertVectorApprox vectorPredicate(1e-4); - - // Check that link is at rest - { - const auto frameData = link->FrameDataRelativeToWorld(); - - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - frameData.linearVelocity); - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - frameData.angularVelocity); - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - frameData.linearAcceleration); - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - frameData.angularAcceleration); - } - - // The moment of inertia of the sphere is a multiple of the identity matrix. - // This means that the moi is invariant to rotation so we can use this matrix - // without converting it to the world frame. - Eigen::Matrix3d moi = math::eigen3::convert(massMatrix.Moi()); - - // Apply forces in the world frame at zero offset - // API: AddExternalForce(relForce, relPosition) - // API: AddExternalTorque(relTorque) - - const Eigen::Vector3d cmdForce{1, -1, 0}; - link->AddExternalForce( - physics::RelativeForce3d(physics::FrameID::World(), cmdForce), - physics::RelativePosition3d(*link, Eigen::Vector3d::Zero())); - - const Eigen::Vector3d cmdTorque{0, 0, 0.1 * GZ_PI}; - link->AddExternalTorque( - physics::RelativeTorque3d(physics::FrameID::World(), cmdTorque)); - - world->Step(output, state, input); - - { - const auto frameData = link->FrameDataRelativeToWorld(); - EXPECT_PRED_FORMAT2(vectorPredicate, cmdForce, - mass * (frameData.linearAcceleration)); - - // The moment of inertia of the sphere is a multiple of the identity matrix. - // Hence the gyroscopic coupling terms are zero - EXPECT_PRED_FORMAT2(vectorPredicate, cmdTorque, - moi * frameData.angularAcceleration); - } - - world->Step(output, state, input); - - // Check that the forces and torques are reset - { - const auto frameData = link->FrameDataRelativeToWorld(); - - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - frameData.linearAcceleration); - - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - frameData.angularAcceleration); - } - - // Apply forces in the local frame - // The sphere is rotated by pi in the +z so the local x and y axes are in - // the -x and -y of the world frame respectively - const Eigen::Vector3d cmdLocalForce{1, -1, 0}; - link->AddExternalForce( - physics::RelativeForce3d(*link, cmdLocalForce), - physics::RelativePosition3d(*link, Eigen::Vector3d::Zero())); - - const Eigen::Vector3d cmdLocalTorque{0.1 * GZ_PI, 0, 0}; - link->AddExternalTorque(physics::RelativeTorque3d(*link, cmdLocalTorque)); - - world->Step(output, state, input); - - { - const Eigen::Vector3d expectedForce = - Eigen::AngleAxisd(GZ_PI, Eigen::Vector3d::UnitZ()) * cmdLocalForce; - - const Eigen::Vector3d expectedTorque = - Eigen::AngleAxisd(GZ_PI, Eigen::Vector3d::UnitZ()) * cmdLocalTorque; - - const auto frameData = link->FrameDataRelativeToWorld(); - - EXPECT_PRED_FORMAT2(vectorPredicate, expectedForce, - mass * (frameData.linearAcceleration)); - - // The moment of inertia of the sphere is a multiple of the identity matrix. - // Hence the gyroscopic coupling terms are zero - EXPECT_PRED_FORMAT2(vectorPredicate, expectedTorque, - moi * frameData.angularAcceleration); - } - - // Test the other AddExternalForce and AddExternalTorque APIs - // API: AddExternalForce(force) - // API: AddExternalTorque(torque) - link->AddExternalForce(cmdForce); - link->AddExternalTorque(cmdTorque); - - world->Step(output, state, input); - - { - const auto frameData = link->FrameDataRelativeToWorld(); - EXPECT_PRED_FORMAT2(vectorPredicate, cmdForce, - mass * (frameData.linearAcceleration)); - - // The moment of inertia of the sphere is a multiple of the identity matrix. - // Hence the gyroscopic coupling terms are zero - EXPECT_PRED_FORMAT2(vectorPredicate, cmdTorque, - moi * frameData.angularAcceleration); - } - - // Apply the force at an offset - // API: AddExternalForce(relForce, relPosition) - Eigen::Vector3d offset{0.1, 0.2, 0.3}; - link->AddExternalForce(physics::RelativeForce3d(*link, cmdLocalForce), - physics::RelativePosition3d(*link, offset)); - - world->Step(output, state, input); - { - const auto frameData = link->FrameDataRelativeToWorld(); - EXPECT_PRED_FORMAT2(vectorPredicate, - frameData.pose.linear() * cmdLocalForce, - mass * (frameData.linearAcceleration)); - - // The moment of inertia of the sphere is a multiple of the identity matrix. - // Hence the gyroscopic coupling terms are zero - EXPECT_PRED_FORMAT2(vectorPredicate, - frameData.pose.linear() * offset.cross(cmdLocalForce), - moi * frameData.angularAcceleration); - } - - // Apply force at an offset using the more convenient API - // API: AddExternalForce(force, frame, position) - link->AddExternalForce(cmdLocalForce, *link, offset); - - world->Step(output, state, input); - { - const auto frameData = link->FrameDataRelativeToWorld(); - EXPECT_PRED_FORMAT2(vectorPredicate, - frameData.pose.linear() * cmdLocalForce, - mass * (frameData.linearAcceleration)); - - // The moment of inertia of the sphere is a multiple of the identity matrix. - // Hence the gyroscopic coupling terms are zero - EXPECT_PRED_FORMAT2(vectorPredicate, - frameData.pose.linear() * offset.cross(cmdLocalForce), - moi * frameData.angularAcceleration); - } -} - -TEST_F(LinkFeaturesFixture, AxisAlignedBoundingBox) -{ - auto world = - LoadWorld(this->engine, TEST_WORLD_DIR "test.world"); - auto model = world->GetModel("double_pendulum_with_base"); - auto baseLink = model->GetLink("base"); - auto bbox = baseLink->GetAxisAlignedBoundingBox(); - AssertVectorApprox vectorPredicate(1e-4); - EXPECT_PRED_FORMAT2( - vectorPredicate, physics::Vector3d(0.2, -0.8, 0), bbox.min()); - EXPECT_PRED_FORMAT2( - vectorPredicate, physics::Vector3d(1.8, 0.8, 2.2), bbox.max()); - - // test with non-world frame - auto bboxModelFrame = baseLink->GetAxisAlignedBoundingBox( - model->GetFrameID()); - EXPECT_PRED_FORMAT2( - vectorPredicate, physics::Vector3d(-0.8, -0.8, 0), bboxModelFrame.min()); - EXPECT_PRED_FORMAT2( - vectorPredicate, physics::Vector3d(0.8, 0.8, 2.2), bboxModelFrame.max()); - - // test with non-world rotated frame - auto upperLink = model->GetLink("upper_link"); - auto bboxUpperLinkFrame = baseLink->GetAxisAlignedBoundingBox( - upperLink->GetFrameID()); - EXPECT_PRED_FORMAT2(vectorPredicate, - physics::Vector3d(-0.8, -0.1, -0.8), bboxUpperLinkFrame.min()); - EXPECT_PRED_FORMAT2(vectorPredicate, - physics::Vector3d(0.8, 2.1, 0.8), bboxUpperLinkFrame.max()); -} - -TEST_F(LinkFeaturesFixture, ModelAxisAlignedBoundingBox) -{ - auto world = - LoadWorld(this->engine, TEST_WORLD_DIR "contact.sdf"); - auto model = world->GetModel("sphere"); - auto bbox = model->GetAxisAlignedBoundingBox(); - AssertVectorApprox vectorPredicate(1e-4); - EXPECT_PRED_FORMAT2( - vectorPredicate, physics::Vector3d(-1, -1, -0.5), bbox.min()); - EXPECT_PRED_FORMAT2( - vectorPredicate, physics::Vector3d(2, 2, 1.5), bbox.max()); - - // test with non-world frame - auto link = model->GetLink("link0"); - auto bboxLinkFrame = model->GetAxisAlignedBoundingBox( - link->GetFrameID()); - EXPECT_PRED_FORMAT2( - vectorPredicate, physics::Vector3d(-1, -1, -1.0), bboxLinkFrame.min()); - EXPECT_PRED_FORMAT2( - vectorPredicate, physics::Vector3d(2, 2, 1.0), bboxLinkFrame.max()); -} diff --git a/dartsim/worlds/contact.sdf b/dartsim/worlds/contact.sdf deleted file mode 100644 index 25bc3be83..000000000 --- a/dartsim/worlds/contact.sdf +++ /dev/null @@ -1,93 +0,0 @@ - - - - - true - - - - - 0 0 1 - 100 100 - - - - - false - - - 0 0 1 - 100 100 - - - - - - - - 0 0 0.5 0 0 0 - - - 1 - - - - - 1 - - - - 0.1 - - - - 0 1 0.5 0 0 0 - - - 1 - - - - - 1 - - - - 1 - - - - 1 0 0.5 0 0 0 - - - 1 - - - - - 1 - - - - 2 - - - - 1 1 0.5 0 0 0 - - - 1 - - - - - 1 - - - - 3 - - - - - diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 61de58950..981b0c7ee 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -7,6 +7,7 @@ set(tests free_joint_features joint_features kinematic_features + link_features simulation_features ) diff --git a/test/common_test/link_features.cc b/test/common_test/link_features.cc new file mode 100644 index 000000000..7db992e68 --- /dev/null +++ b/test/common_test/link_features.cc @@ -0,0 +1,416 @@ +/* + * 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 "../helpers/TestLibLoader.hh" +#include "../Utils.hh" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +template +class LinkFeaturesTest: + public testing::Test, public gz::physics::TestLibLoader +{ + // Documentation inherited + public: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + + loader.LoadLib(LinkFeaturesTest::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; +}; + +// 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; +}; + +struct LinkFeaturesList : gz::physics::FeatureList< + gz::physics::AddLinkExternalForceTorque, + gz::physics::ForwardStep, + gz::physics::Gravity, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfLink, + gz::physics::GetEntities, + gz::physics::GetLinkBoundingBox, + gz::physics::GetModelBoundingBox +> { }; + +using LinkFeaturesTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(LinkFeaturesTest, + LinkFeaturesTestTypes); + +TYPED_TEST(LinkFeaturesTest, JointSetCommand) +{ + 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, "empty.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + const std::string modelName{"double_pendulum_with_base"}; + const std::string jointName{"upper_joint"}; + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + EXPECT_NE(nullptr, world); + + EXPECT_NE(nullptr, world); + world->SetGravity(Eigen::Vector3d::Zero()); + + AssertVectorApprox vectorPredicateGravity(1e-10); + EXPECT_PRED_FORMAT2(vectorPredicateGravity, Eigen::Vector3d::Zero(), + world->GetGravity()); + + // Add a sphere + sdf::Model modelSDF; + modelSDF.SetName("sphere"); + modelSDF.SetRawPose(gz::math::Pose3d(0, 0, 2, 0, 0, GZ_PI)); + auto model = world->ConstructModel(modelSDF); + + const double mass = 1.0; + gz::math::MassMatrix3d massMatrix{ + mass, + gz::math::Vector3d{0.4, 0.4, 0.4}, + gz::math::Vector3d::Zero}; + + sdf::Link linkSDF; + linkSDF.SetName("sphere_link"); + linkSDF.SetInertial({massMatrix, gz::math::Pose3d::Zero}); + auto link = model->ConstructLink(linkSDF); + + gz::physics::ForwardStep::Input input; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Output output; + + AssertVectorApprox vectorPredicate(1e-4); + + // Check that link is at rest + { + const auto frameData = link->FrameDataRelativeToWorld(); + + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), + frameData.linearVelocity); + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), + frameData.angularVelocity); + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), + frameData.linearAcceleration); + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), + frameData.angularAcceleration); + } + + // The moment of inertia of the sphere is a multiple of the identity matrix. + // This means that the moi is invariant to rotation so we can use this matrix + // without converting it to the world frame. + Eigen::Matrix3d moi = gz::math::eigen3::convert(massMatrix.Moi()); + + // Apply forces in the world frame at zero offset + // API: AddExternalForce(relForce, relPosition) + // API: AddExternalTorque(relTorque) + + const Eigen::Vector3d cmdForce{1, -1, 0}; + link->AddExternalForce( + gz::physics::RelativeForce3d(gz::physics::FrameID::World(), cmdForce), + gz::physics::RelativePosition3d(*link, Eigen::Vector3d::Zero())); + + const Eigen::Vector3d cmdTorque{0, 0, 0.1 * GZ_PI}; + link->AddExternalTorque( + gz::physics::RelativeTorque3d(gz::physics::FrameID::World(), cmdTorque)); + + world->Step(output, state, input); + + { + const auto frameData = link->FrameDataRelativeToWorld(); + EXPECT_PRED_FORMAT2(vectorPredicate, cmdForce, + mass * (frameData.linearAcceleration)); + + // The moment of inertia of the sphere is a multiple of the identity matrix. + // Hence the gyroscopic coupling terms are zero + EXPECT_PRED_FORMAT2(vectorPredicate, cmdTorque, + moi * frameData.angularAcceleration); + } + + world->Step(output, state, input); + + // Check that the forces and torques are reset + { + const auto frameData = link->FrameDataRelativeToWorld(); + + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), + frameData.linearAcceleration); + + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), + frameData.angularAcceleration); + } + + // Apply forces in the local frame + // The sphere is rotated by pi in the +z so the local x and y axes are in + // the -x and -y of the world frame respectively + const Eigen::Vector3d cmdLocalForce{1, -1, 0}; + link->AddExternalForce( + gz::physics::RelativeForce3d(*link, cmdLocalForce), + gz::physics::RelativePosition3d(*link, Eigen::Vector3d::Zero())); + + const Eigen::Vector3d cmdLocalTorque{0.1 * GZ_PI, 0, 0}; + link->AddExternalTorque(gz::physics::RelativeTorque3d(*link, cmdLocalTorque)); + + world->Step(output, state, input); + + { + const Eigen::Vector3d expectedForce = + Eigen::AngleAxisd(GZ_PI, Eigen::Vector3d::UnitZ()) * cmdLocalForce; + + const Eigen::Vector3d expectedTorque = + Eigen::AngleAxisd(GZ_PI, Eigen::Vector3d::UnitZ()) * cmdLocalTorque; + + const auto frameData = link->FrameDataRelativeToWorld(); + + EXPECT_PRED_FORMAT2(vectorPredicate, expectedForce, + mass * (frameData.linearAcceleration)); + + // The moment of inertia of the sphere is a multiple of the identity matrix. + // Hence the gyroscopic coupling terms are zero + EXPECT_PRED_FORMAT2(vectorPredicate, expectedTorque, + moi * frameData.angularAcceleration); + } + + // Test the other AddExternalForce and AddExternalTorque APIs + // API: AddExternalForce(force) + // API: AddExternalTorque(torque) + link->AddExternalForce(cmdForce); + link->AddExternalTorque(cmdTorque); + + world->Step(output, state, input); + + { + const auto frameData = link->FrameDataRelativeToWorld(); + EXPECT_PRED_FORMAT2(vectorPredicate, cmdForce, + mass * (frameData.linearAcceleration)); + + // The moment of inertia of the sphere is a multiple of the identity matrix. + // Hence the gyroscopic coupling terms are zero + EXPECT_PRED_FORMAT2(vectorPredicate, cmdTorque, + moi * frameData.angularAcceleration); + } + + // Apply the force at an offset + // API: AddExternalForce(relForce, relPosition) + Eigen::Vector3d offset{0.1, 0.2, 0.3}; + link->AddExternalForce(gz::physics::RelativeForce3d(*link, cmdLocalForce), + gz::physics::RelativePosition3d(*link, offset)); + + world->Step(output, state, input); + { + const auto frameData = link->FrameDataRelativeToWorld(); + EXPECT_PRED_FORMAT2(vectorPredicate, + frameData.pose.linear() * cmdLocalForce, + mass * (frameData.linearAcceleration)); + + // The moment of inertia of the sphere is a multiple of the identity matrix. + // Hence the gyroscopic coupling terms are zero + EXPECT_PRED_FORMAT2(vectorPredicate, + frameData.pose.linear() * offset.cross(cmdLocalForce), + moi * frameData.angularAcceleration); + } + + // Apply force at an offset using the more convenient API + // API: AddExternalForce(force, frame, position) + link->AddExternalForce(cmdLocalForce, *link, offset); + + world->Step(output, state, input); + { + const auto frameData = link->FrameDataRelativeToWorld(); + EXPECT_PRED_FORMAT2(vectorPredicate, + frameData.pose.linear() * cmdLocalForce, + mass * (frameData.linearAcceleration)); + + // The moment of inertia of the sphere is a multiple of the identity matrix. + // Hence the gyroscopic coupling terms are zero + EXPECT_PRED_FORMAT2(vectorPredicate, + frameData.pose.linear() * offset.cross(cmdLocalForce), + moi * frameData.angularAcceleration); + } + } +} + +TYPED_TEST(LinkFeaturesTest, AxisAlignedBoundingBox) +{ + 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, "test.world")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + const std::string modelName{"double_pendulum_with_base"}; + const std::string jointName{"upper_joint"}; + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + EXPECT_NE(nullptr, world); + + EXPECT_NE(nullptr, world); + world->SetGravity(Eigen::Vector3d{0, 0, -9.8}); + + auto model = world->GetModel("double_pendulum_with_base"); + auto baseLink = model->GetLink("base"); + auto bbox = baseLink->GetAxisAlignedBoundingBox(); + AssertVectorApprox vectorPredicate(1e-4); + EXPECT_PRED_FORMAT2( + vectorPredicate, gz::physics::Vector3d(0.2, -0.8, 0), bbox.min()); + EXPECT_PRED_FORMAT2( + vectorPredicate, gz::physics::Vector3d(1.8, 0.8, 2.2), bbox.max()); + + // test with non-world frame + auto bboxModelFrame = baseLink->GetAxisAlignedBoundingBox( + model->GetFrameID()); + EXPECT_PRED_FORMAT2( + vectorPredicate, gz::physics::Vector3d(-0.8, -0.8, 0), bboxModelFrame.min()); + EXPECT_PRED_FORMAT2( + vectorPredicate, gz::physics::Vector3d(0.8, 0.8, 2.2), bboxModelFrame.max()); + + // test with non-world rotated frame + auto upperLink = model->GetLink("upper_link"); + auto bboxUpperLinkFrame = baseLink->GetAxisAlignedBoundingBox( + upperLink->GetFrameID()); + EXPECT_PRED_FORMAT2(vectorPredicate, + gz::physics::Vector3d(-0.8, -0.1, -0.8), bboxUpperLinkFrame.min()); + EXPECT_PRED_FORMAT2(vectorPredicate, + gz::physics::Vector3d(0.8, 2.1, 0.8), bboxUpperLinkFrame.max()); + } +} + +TYPED_TEST(LinkFeaturesTest, ModelAxisAlignedBoundingBox) +{ + 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, "contact.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + const std::string modelName{"double_pendulum_with_base"}; + const std::string jointName{"upper_joint"}; + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + EXPECT_NE(nullptr, world); + + EXPECT_NE(nullptr, world); + world->SetGravity(Eigen::Vector3d{0, 0, -9.8}); + + auto model = world->GetModel("sphere"); + auto bbox = model->GetAxisAlignedBoundingBox(); + AssertVectorApprox vectorPredicate(1e-4); + EXPECT_PRED_FORMAT2( + vectorPredicate, gz::physics::Vector3d(-1, -1, -0.5), bbox.min()); + EXPECT_PRED_FORMAT2( + vectorPredicate, gz::physics::Vector3d(2, 2, 1.5), bbox.max()); + + // test with non-world frame + auto link = model->GetLink("link0"); + auto bboxLinkFrame = model->GetAxisAlignedBoundingBox( + link->GetFrameID()); + EXPECT_PRED_FORMAT2( + vectorPredicate, gz::physics::Vector3d(-1, -1, -1.0), bboxLinkFrame.min()); + EXPECT_PRED_FORMAT2( + vectorPredicate, gz::physics::Vector3d(2, 2, 1.0), bboxLinkFrame.max()); + } +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + if (!LinkFeaturesTest::init( + argc, argv)) + return -1; + return RUN_ALL_TESTS(); +} diff --git a/test/common_test/worlds/empty.sdf b/test/common_test/worlds/empty.sdf new file mode 100644 index 000000000..62150c14a --- /dev/null +++ b/test/common_test/worlds/empty.sdf @@ -0,0 +1,5 @@ + + + + + From 3323825b45e280bee0c5af177073923ab5c7fe39 Mon Sep 17 00:00:00 2001 From: Joan Aguilar Mayans Date: Thu, 8 Sep 2022 15:08:25 -0700 Subject: [PATCH 10/13] Fixed broken link in the physics plugin tutorial. (#417) Signed-off-by: Joan Aguilar Mayans --- tutorials/03_physics_plugins.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tutorials/03_physics_plugins.md b/tutorials/03_physics_plugins.md index 1e2017b79..49024fb83 100644 --- a/tutorials/03_physics_plugins.md +++ b/tutorials/03_physics_plugins.md @@ -78,12 +78,12 @@ Users do not need to organize their own plugin implementations this way. Dart ([Dynamic Animation and Robotics Toolkit](https://dartsim.github.io/)) is an open source library that provides data structures and algorithms for kinematic and dynamic applications in robotics and computer animation. It is the default physics engine used in Gazebo Simulation. -The source code for Dartsim plugin can be found in [Gazebo Physics repository](https://github.com/gazebosim/gz-physics/tree/ign-physics6) under `dartsim` directory. +The source code for Dartsim plugin can be found in [Gazebo Physics repository](https://github.com/gazebosim/gz-physics/tree/gz-physics6) under `dartsim` directory. -TPE ([Trivial Physics Engine](https://github.com/gazebosim/gz-physics/tree/ign-physics6/tpe)) is an open source library created by Open Robotics that enables fast, inexpensive kinematics simulation for entities at large scale. +TPE ([Trivial Physics Engine](https://github.com/gazebosim/gz-physics/tree/gz-physics6/tpe)) is an open source library created by Open Robotics that enables fast, inexpensive kinematics simulation for entities at large scale. It supports higher-order fleet dynamics without real physics (eg. gravity, force, constraint etc.) and multi-machine synchronization. Gazebo support for TPE targets [Citadel](https://gazebosim.org/docs/citadel) and onward releases. -The source code for TPE plugin can be found in [Gazebo Physics repository](https://github.com/gazebosim/gz-physics/tree/ign-physics6) under the `tpe/plugin` directory. +The source code for TPE plugin can be found in [Gazebo Physics repository](https://github.com/gazebosim/gz-physics/tree/gz-physics6) under the `tpe/plugin` directory. The following is a list of features supported by each physics engine to help users select one that fits their needs. From f177e029a2724c43cbeb70eeb6f218c716d20155 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 12 Sep 2022 19:47:24 +0200 Subject: [PATCH 11/13] Added shape features common tests (#412) * Added shape features common tests Signed-off-by: ahcorde --- dartsim/src/ShapeFeatures_TEST.cc | 250 ---------------- test/common_test/CMakeLists.txt | 1 + test/common_test/shape_features.cc | 266 ++++++++++++++++++ .../common_test}/worlds/slip_compliance.sdf | 0 4 files changed, 267 insertions(+), 250 deletions(-) delete mode 100644 dartsim/src/ShapeFeatures_TEST.cc create mode 100644 test/common_test/shape_features.cc rename {dartsim => test/common_test}/worlds/slip_compliance.sdf (100%) diff --git a/dartsim/src/ShapeFeatures_TEST.cc b/dartsim/src/ShapeFeatures_TEST.cc deleted file mode 100644 index b6fe6c753..000000000 --- a/dartsim/src/ShapeFeatures_TEST.cc +++ /dev/null @@ -1,250 +0,0 @@ -/* - * Copyright (C) 2019 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 - -// Features -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "test/Utils.hh" -#include "ShapeFeatures.hh" - -using namespace gz; - -using TestFeatureList = gz::physics::FeatureList< - physics::dartsim::RetrieveWorld, - physics::AttachFixedJointFeature, - physics::AddLinkExternalForceTorque, - physics::LinkFrameSemantics, - physics::DetachJointFeature, - physics::SetJointTransformFromParentFeature, - physics::ForwardStep, - physics::FreeJointCast, - physics::GetBasicJointState, - physics::GetEntities, - physics::RevoluteJointCast, - physics::SetJointVelocityCommandFeature, -#if DART_VERSION_AT_LEAST(6, 10, 0) - physics::GetShapeFrictionPyramidSlipCompliance, - physics::SetShapeFrictionPyramidSlipCompliance, -#endif - physics::sdf::ConstructSdfModel, - physics::sdf::ConstructSdfWorld, - physics::sdf::ConstructSdfLink ->; - -using TestEnginePtr = physics::Engine3dPtr; -using TestWorldPtr = physics::World3dPtr; - -class ShapeFeaturesFixture : public ::testing::Test -{ - protected: void SetUp() override - { - gz::plugin::Loader loader; - loader.LoadLib(dartsim_plugin_LIB); - - gz::plugin::PluginPtr dartsim = - loader.Instantiate("gz::physics::dartsim::Plugin"); - - this->engine = - gz::physics::RequestEngine3d::From(dartsim); - ASSERT_NE(nullptr, this->engine); - } - protected: TestEnginePtr engine; -}; - -// 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; -}; - -#if DART_VERSION_AT_LEAST(6, 10, 0) -///////////////////////////////////////////////// -TEST_F(ShapeFeaturesFixture, PrimarySlipCompliance) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "slip_compliance.sdf"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - const std::string modelName{"box"}; - const std::string linkName{"box_link"}; - const std::string shapeName{"box_collision"}; - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - - auto model = world->GetModel(modelName); - auto boxLink = model->GetLink(linkName); - auto boxShape = boxLink->GetShape(shapeName); - - AssertVectorApprox vectorPredicate(1e-4); - - gz::physics::ForwardStep::Input input; - gz::physics::ForwardStep::State state; - gz::physics::ForwardStep::Output output; - - // Check that link is at rest - { - const auto frameData = boxLink->FrameDataRelativeToWorld(); - - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - frameData.linearVelocity); - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - frameData.angularVelocity); - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - frameData.linearAcceleration); - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - frameData.angularAcceleration); - } - - const Eigen::Vector3d cmdForce{1, 0, 0}; - const double primarySlip = 0.5; - - // expect 0.0 initial slip - EXPECT_DOUBLE_EQ(0.0, boxShape->GetPrimarySlipCompliance()); - - boxShape->SetPrimarySlipCompliance(primarySlip); - EXPECT_DOUBLE_EQ(primarySlip, boxShape->GetPrimarySlipCompliance()); - - const std::size_t numSteps = 10000; - for (std::size_t i = 0; i < numSteps; ++i) - { - world->Step(output, state, input); - boxLink->AddExternalForce( - physics::RelativeForce3d(physics::FrameID::World(), cmdForce), - physics::RelativePosition3d(*boxLink, Eigen::Vector3d::Zero())); - } - - { - // velocity = slip * applied force - const auto frameData = boxLink->FrameDataRelativeToWorld(); - EXPECT_PRED_FORMAT2(vectorPredicate, primarySlip * cmdForce, - (frameData.linearVelocity)); - } -} - -///////////////////////////////////////////////// -TEST_F(ShapeFeaturesFixture, SecondarySlipCompliance) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(TEST_WORLD_DIR "slip_compliance.sdf"); - ASSERT_TRUE(errors.empty()) << errors.front(); - - const std::string modelName{"box"}; - const std::string linkName{"box_link"}; - const std::string shapeName{"box_collision"}; - - auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); - - auto model = world->GetModel(modelName); - auto boxLink = model->GetLink(linkName); - auto boxShape = boxLink->GetShape(shapeName); - - AssertVectorApprox vectorPredicate(1e-4); - - gz::physics::ForwardStep::Input input; - gz::physics::ForwardStep::State state; - gz::physics::ForwardStep::Output output; - - // Check that link is at rest - { - const auto frameData = boxLink->FrameDataRelativeToWorld(); - - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - frameData.linearVelocity); - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - frameData.angularVelocity); - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - frameData.linearAcceleration); - EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), - frameData.angularAcceleration); - } - - const Eigen::Vector3d cmdForce{0, 1, 0}; - const double secondarySlip = 0.25; - - // expect 0.0 initial slip - EXPECT_DOUBLE_EQ(0.0, boxShape->GetSecondarySlipCompliance()); - - boxShape->SetSecondarySlipCompliance(secondarySlip); - EXPECT_DOUBLE_EQ(secondarySlip, boxShape->GetSecondarySlipCompliance()); - - const std::size_t numSteps = 10000; - for (std::size_t i = 0; i < numSteps; ++i) - { - world->Step(output, state, input); - boxLink->AddExternalForce( - physics::RelativeForce3d(physics::FrameID::World(), cmdForce), - physics::RelativePosition3d(*boxLink, Eigen::Vector3d::Zero())); - } - - { - // velocity = slip * applied force - const auto frameData = boxLink->FrameDataRelativeToWorld(); - EXPECT_PRED_FORMAT2(vectorPredicate, secondarySlip * cmdForce, - (frameData.linearVelocity)); - } -} -#endif diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 981b0c7ee..9670112b7 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -8,6 +8,7 @@ set(tests joint_features kinematic_features link_features + shape_features simulation_features ) diff --git a/test/common_test/shape_features.cc b/test/common_test/shape_features.cc new file mode 100644 index 000000000..6d9abe8c0 --- /dev/null +++ b/test/common_test/shape_features.cc @@ -0,0 +1,266 @@ +/* + * 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" + +// Features +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +template +class ShapeFeaturesTest: + public testing::Test, public gz::physics::TestLibLoader +{ + // Documentation inherited + public: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + + loader.LoadLib(ShapeFeaturesTest::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; +}; + +// 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; +}; + +struct ShapeFeaturesList : gz::physics::FeatureList< + gz::physics::AttachFixedJointFeature, + gz::physics::AddLinkExternalForceTorque, + gz::physics::LinkFrameSemantics, + gz::physics::DetachJointFeature, + gz::physics::SetJointTransformFromParentFeature, + gz::physics::ForwardStep, + gz::physics::FreeJointCast, + gz::physics::GetBasicJointState, + gz::physics::GetEntities, + gz::physics::RevoluteJointCast, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::GetShapeFrictionPyramidSlipCompliance, + gz::physics::SetShapeFrictionPyramidSlipCompliance, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::sdf::ConstructSdfLink +> { }; + +using ShapeFeaturesTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(ShapeFeaturesTest, + ShapeFeaturesTestTypes); + +TYPED_TEST(ShapeFeaturesTest, PrimarySlipCompliance) +{ + 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, "slip_compliance.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + const std::string modelName{"box"}; + const std::string linkName{"box_link"}; + const std::string shapeName{"box_collision"}; + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + auto model = world->GetModel(modelName); + auto boxLink = model->GetLink(linkName); + auto boxShape = boxLink->GetShape(shapeName); + + AssertVectorApprox vectorPredicate(1e-4); + + gz::physics::ForwardStep::Input input; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Output output; + + // Check that link is at rest + { + const auto frameData = boxLink->FrameDataRelativeToWorld(); + + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), + frameData.linearVelocity); + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), + frameData.angularVelocity); + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), + frameData.linearAcceleration); + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), + frameData.angularAcceleration); + } + + const Eigen::Vector3d cmdForce{1, 0, 0}; + const double primarySlip = 0.5; + + // expect 0.0 initial slip + EXPECT_DOUBLE_EQ(0.0, boxShape->GetPrimarySlipCompliance()); + + boxShape->SetPrimarySlipCompliance(primarySlip); + EXPECT_DOUBLE_EQ(primarySlip, boxShape->GetPrimarySlipCompliance()); + + const std::size_t numSteps = 10000; + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + boxLink->AddExternalForce( + gz::physics::RelativeForce3d(gz::physics::FrameID::World(), cmdForce), + gz::physics::RelativePosition3d(*boxLink, Eigen::Vector3d::Zero())); + } + + { + // velocity = slip * applied force + const auto frameData = boxLink->FrameDataRelativeToWorld(); + EXPECT_PRED_FORMAT2(vectorPredicate, primarySlip * cmdForce, + (frameData.linearVelocity)); + } + } +} + +///////////////////////////////////////////////// +TYPED_TEST(ShapeFeaturesTest, SecondarySlipCompliance) +{ + 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, "slip_compliance.sdf")); + ASSERT_TRUE(errors.empty()) << errors.front(); + + const std::string modelName{"box"}; + const std::string linkName{"box_link"}; + const std::string shapeName{"box_collision"}; + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + + auto model = world->GetModel(modelName); + auto boxLink = model->GetLink(linkName); + auto boxShape = boxLink->GetShape(shapeName); + + AssertVectorApprox vectorPredicate(1e-4); + + gz::physics::ForwardStep::Input input; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Output output; + + // Check that link is at rest + { + const auto frameData = boxLink->FrameDataRelativeToWorld(); + + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), + frameData.linearVelocity); + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), + frameData.angularVelocity); + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), + frameData.linearAcceleration); + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), + frameData.angularAcceleration); + } + + const Eigen::Vector3d cmdForce{0, 1, 0}; + const double secondarySlip = 0.25; + + // expect 0.0 initial slip + EXPECT_DOUBLE_EQ(0.0, boxShape->GetSecondarySlipCompliance()); + + boxShape->SetSecondarySlipCompliance(secondarySlip); + EXPECT_DOUBLE_EQ(secondarySlip, boxShape->GetSecondarySlipCompliance()); + + const std::size_t numSteps = 10000; + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + boxLink->AddExternalForce( + gz::physics::RelativeForce3d(gz::physics::FrameID::World(), cmdForce), + gz::physics::RelativePosition3d(*boxLink, Eigen::Vector3d::Zero())); + } + + { + // velocity = slip * applied force + const auto frameData = boxLink->FrameDataRelativeToWorld(); + EXPECT_PRED_FORMAT2(vectorPredicate, secondarySlip * cmdForce, + (frameData.linearVelocity)); + } + } +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + if (!ShapeFeaturesTest::init( + argc, argv)) + return -1; + return RUN_ALL_TESTS(); +} diff --git a/dartsim/worlds/slip_compliance.sdf b/test/common_test/worlds/slip_compliance.sdf similarity index 100% rename from dartsim/worlds/slip_compliance.sdf rename to test/common_test/worlds/slip_compliance.sdf From b2a8bc4c4ec9a75b65ec05c954f3f8a9d9bff255 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 12 Sep 2022 22:01:28 +0200 Subject: [PATCH 12/13] Added world features common tests (#414) Signed-off-by: ahcorde --- dartsim/src/WorldFeatures_TEST.cc | 77 +---------- test/common_test/CMakeLists.txt | 1 + test/common_test/world_features.cc | 208 +++++++++++++++++++++++++++++ 3 files changed, 214 insertions(+), 72 deletions(-) create mode 100644 test/common_test/world_features.cc diff --git a/dartsim/src/WorldFeatures_TEST.cc b/dartsim/src/WorldFeatures_TEST.cc index 6ad979eb1..c802dfbda 100644 --- a/dartsim/src/WorldFeatures_TEST.cc +++ b/dartsim/src/WorldFeatures_TEST.cc @@ -104,7 +104,8 @@ TestWorldPtr LoadWorld( ////////////////////////////////////////////////// TEST_F(WorldFeaturesFixture, CollisionDetector) { - auto world = LoadWorld(this->engine, TEST_WORLD_DIR "/empty.sdf"); + auto world = LoadWorld(this->engine, + gz::common::joinPaths(TEST_WORLD_DIR, "empty.sdf")); EXPECT_EQ("ode", world->GetCollisionDetector()); world->SetCollisionDetector("banana"); @@ -123,80 +124,12 @@ TEST_F(WorldFeaturesFixture, CollisionDetector) EXPECT_EQ("dart", world->GetCollisionDetector()); } -////////////////////////////////////////////////// -TEST_F(WorldFeaturesFixture, Gravity) -{ - auto world = LoadWorld(this->engine, TEST_WORLD_DIR "/falling.world"); - ASSERT_NE(nullptr, world); - - // Check default gravity value - AssertVectorApprox vectorPredicate10(1e-10); - EXPECT_PRED_FORMAT2(vectorPredicate10, Eigen::Vector3d(0, 0, -9.8), - world->GetGravity()); - - auto model = world->GetModel("sphere"); - ASSERT_NE(nullptr, model); - - auto link = model->GetLink(0); - ASSERT_NE(nullptr, link); - - // initial link pose - const Eigen::Vector3d initialLinkPosition(0, 0, 2); - { - Eigen::Vector3d pos = link->FrameDataRelativeToWorld().pose.translation(); - EXPECT_PRED_FORMAT2(vectorPredicate10, - initialLinkPosition, - pos); - } - - auto linkFrameID = link->GetFrameID(); - - // Get default gravity in link frame, which is pitched by pi/4 - EXPECT_PRED_FORMAT2(vectorPredicate10, - Eigen::Vector3d(6.92964645563, 0, -6.92964645563), - world->GetGravity(linkFrameID)); - - // set gravity along X axis of linked frame, which is pitched by pi/4 - world->SetGravity(Eigen::Vector3d(1.4142135624, 0, 0), linkFrameID); - - EXPECT_PRED_FORMAT2(vectorPredicate10, - Eigen::Vector3d(1, 0, -1), - world->GetGravity()); - - // test other SetGravity API - // set gravity along Z axis of linked frame, which is pitched by pi/4 - physics::RelativeForce3d relativeGravity( - linkFrameID, Eigen::Vector3d(0, 0, 1.4142135624)); - world->SetGravity(relativeGravity); - - EXPECT_PRED_FORMAT2(vectorPredicate10, - Eigen::Vector3d(1, 0, 1), - world->GetGravity()); - - // Confirm that changed gravity direction affects pose of link - 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); - } - - AssertVectorApprox vectorPredicate3(1e-3); - { - Eigen::Vector3d pos = link->FrameDataRelativeToWorld().pose.translation(); - EXPECT_PRED_FORMAT2(vectorPredicate3, - Eigen::Vector3d(0.5, 0, 2.5), - pos); - } -} - ////////////////////////////////////////////////// TEST_F(WorldFeaturesFixture, Solver) { - auto world = LoadWorld(this->engine, TEST_WORLD_DIR "/empty.sdf"); + auto world = LoadWorld(this->engine, + gz::common::joinPaths(TEST_WORLD_DIR, "empty.sdf")); + EXPECT_EQ("DantzigBoxedLcpSolver", world->GetSolver()); world->SetSolver("banana"); diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 9670112b7..c7b542b96 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -10,6 +10,7 @@ set(tests link_features shape_features 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..963e84b66 --- /dev/null +++ b/test/common_test/world_features.cc @@ -0,0 +1,208 @@ +/* + * 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 +#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, + gz::physics::LinkFrameSemantics, + gz::physics::GetModelFromWorld, + gz::physics::GetLinkFromModel, + gz::physics::ForwardStep +>; + +using GravityFeaturesTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(WorldFeaturesTest, + GravityFeatures); + +///////////////////////////////////////////////// +TYPED_TEST(WorldFeaturesTest, 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, "falling.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; + + Eigen::Vector3d gravity = {0, 0, -9.8}; + + 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()); + + world->SetGravity(gravity); + + auto model = world->GetModel("sphere"); + ASSERT_NE(nullptr, model); + + auto link = model->GetLink(0); + ASSERT_NE(nullptr, link); + + AssertVectorApprox vectorPredicate10(1e-10); + + // initial link pose + const Eigen::Vector3d initialLinkPosition(0, 0, 2); + { + Eigen::Vector3d pos = link->FrameDataRelativeToWorld().pose.translation(); + EXPECT_PRED_FORMAT2(vectorPredicate10, + initialLinkPosition, + pos); + } + + auto linkFrameID = link->GetFrameID(); + + // Get default gravity in link frame, which is pitched by pi/4 + EXPECT_PRED_FORMAT2(vectorPredicate10, + Eigen::Vector3d(6.92964645563, 0, -6.92964645563), + world->GetGravity(linkFrameID)); + + // set gravity along X axis of linked frame, which is pitched by pi/4 + world->SetGravity(Eigen::Vector3d(1.4142135624, 0, 0), linkFrameID); + + EXPECT_PRED_FORMAT2(vectorPredicate10, + Eigen::Vector3d(1, 0, -1), + world->GetGravity()); + + // test other SetGravity API + // set gravity along Z axis of linked frame, which is pitched by pi/4 + gz::physics::RelativeForce3d relativeGravity( + linkFrameID, Eigen::Vector3d(0, 0, 1.4142135624)); + world->SetGravity(relativeGravity); + + EXPECT_PRED_FORMAT2(vectorPredicate10, + Eigen::Vector3d(1, 0, 1), + world->GetGravity()); + + // Confirm that changed gravity direction affects pose of link + 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); + } + + AssertVectorApprox vectorPredicate3(1e-3); + { + Eigen::Vector3d pos = link->FrameDataRelativeToWorld().pose.translation(); + EXPECT_PRED_FORMAT2(vectorPredicate3, + Eigen::Vector3d(0.5, 0, 2.5), + pos); + } + } +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + if(!WorldFeaturesTest::init(argc, argv)) + return -1; + return RUN_ALL_TESTS(); +} From 169f16cf1065425722a531b8b9219c1cd706b431 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 14 Sep 2022 09:41:42 -0500 Subject: [PATCH 13/13] Update target to gz-physics6 Signed-off-by: Michael Carroll --- CMakeLists.txt | 2 +- Changelog.md | 4 ---- README.md | 4 ++-- 3 files changed, 3 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 635a10723..d79763a20 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-physics7 VERSION 7.0.0) +project(gz-physics6 VERSION 6.0.0) #============================================================================ # Find gz-cmake diff --git a/Changelog.md b/Changelog.md index c04b41799..27d5ad1c5 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,7 +1,3 @@ -## Gazebo Physics 7.x - -### Gazebo Physics 7.x.x (202X-XX-XX) - ## Gazebo Physics 6.x ### Gazebo Physics 6.x.x (202X-XX-XX) diff --git a/README.md b/README.md index 879d92ef7..5d9e24a7d 100644 --- a/README.md +++ b/README.md @@ -73,7 +73,7 @@ See the [installation tutorial](https://gazebosim.org/api/physics/5.0/installati # Usage -Please refer to the [examples directory](https://github.com/gazebosim/gz-physics/raw/main/examples/). +Please refer to the [examples directory](https://github.com/gazebosim/gz-physics/raw/gz-physics6/examples/). # Documentation @@ -90,7 +90,7 @@ You can also generate the documentation from a clone of this repository by follo 2. Clone the repository ``` - git clone https://github.com/gazebosim/gz-physics -b main + git clone https://github.com/gazebosim/gz-physics -b gz-physics6 ``` 3. Configure and build the documentation.