From d00d62014ee22989a357123903de55c1596168dd Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 30 Aug 2022 13:52:04 +0200 Subject: [PATCH 1/4] Improved simulation features common tests 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 804f6cebeef04c2801673d85e55f8126ac6908a7 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 1 Sep 2022 11:35:57 +0200 Subject: [PATCH 2/4] Added joint_features common tests Signed-off-by: ahcorde --- 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 | 1820 +++++++++++++++++ .../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 ++++ 10 files changed, 2266 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/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..92e0601d5 --- /dev/null +++ b/test/common_test/joint_features.cc @@ -0,0 +1,1820 @@ +/* + * 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 +#include +#include "gz/physics/FrameSemantics.hh" +#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::GetEngineInfo, + gz::physics::ForwardStep, + gz::physics::GetBasicJointState, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetModelFromWorld, + gz::physics::GetLinkFromModel, + gz::physics::GetJointFromModel, + gz::physics::SetBasicJointState, + gz::physics::GetBasicJointProperties, + gz::physics::LinkFrameSemantics, + gz::physics::SetJointVelocityCommandFeature +> { }; + +using JointFeaturesTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(JointFeaturesTest, + JointFeaturesTestTypes); + +TYPED_TEST(JointFeaturesTest, 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, "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::GetEngineInfo, + gz::physics::ForwardStep, + gz::physics::GetBasicJointState, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetModelFromWorld, + gz::physics::GetJointFromModel, + gz::physics::SetBasicJointState, + gz::physics::GetBasicJointProperties, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::SetJointPositionLimitsFeature +> { }; + +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::GetEngineInfo, + gz::physics::ForwardStep, + gz::physics::GetBasicJointState, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetModelFromWorld, + gz::physics::GetJointFromModel, + gz::physics::SetBasicJointState, + gz::physics::GetBasicJointProperties, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::SetJointPositionLimitsFeature, + gz::physics::SetJointVelocityLimitsFeature, + gz::physics::SetJointEffortLimitsFeature +> { }; + +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::GetEngineInfo, + gz::physics::ForwardStep, + gz::physics::GetBasicJointState, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetModelFromWorld, + gz::physics::GetJointFromModel, + gz::physics::GetLinkFromModel, + gz::physics::SetBasicJointState, + gz::physics::GetBasicJointProperties, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::FreeJointCast, + gz::physics::LinkFrameSemantics, + gz::physics::RevoluteJointCast, + gz::physics::DetachJointFeature +> { }; + +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::GetEngineInfo, + gz::physics::ForwardStep, + gz::physics::GetBasicJointState, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetModelFromWorld, + gz::physics::GetJointFromModel, + gz::physics::GetLinkFromModel, + gz::physics::SetBasicJointState, + gz::physics::GetBasicJointProperties, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::FreeJointCast, + gz::physics::LinkFrameSemantics, + gz::physics::RevoluteJointCast, + gz::physics::DetachJointFeature, + gz::physics::AttachFixedJointFeature, + gz::physics::SetJointTransformFromParentFeature, + gz::physics::sdf::ConstructSdfModel +> { }; + +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::GetEngineInfo, + gz::physics::ForwardStep, + gz::physics::GetBasicJointState, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetModelFromWorld, + gz::physics::GetLinkFromModel, + gz::physics::GetJointFromModel, + gz::physics::SetBasicJointState, + gz::physics::LinkFrameSemantics, + gz::physics::SetFreeGroupWorldPose, + gz::physics::FreeGroupFrameSemantics, + gz::physics::GetJointTransmittedWrench +> { }; + +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 42ca06bd92d897ee5f7f3fce4501f44ef3efdb51 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 1 Sep 2022 18:12:17 +0200 Subject: [PATCH 3/4] Skip bullet tests Signed-off-by: ahcorde --- bullet/src/JointFeatures.cc | 9 +++++++++ test/common_test/joint_features.cc | 6 ++++++ 2 files changed, 15 insertions(+) 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/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 92e0601d5..97d8a91a7 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -101,6 +101,12 @@ 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); From 429e08b09496191b940bda8cee69297471dfb6be Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 2 Sep 2022 10:01:27 +0200 Subject: [PATCH 4/4] Fix test Signed-off-by: ahcorde --- test/common_test/joint_features.cc | 104 +++++++++++++++-------------- 1 file changed, 54 insertions(+), 50 deletions(-) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 97d8a91a7..468bd548f 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -27,19 +27,20 @@ #include "../helpers/TestLibLoader.hh" #include "../Utils.hh" -#include -#include #include "gz/physics/FrameSemantics.hh" -#include +#include #include -#include -#include +#include #include -#include +#include #include -#include +#include #include +#include #include +#include +#include +#include #include @@ -79,17 +80,17 @@ class JointFeaturesTest: }; struct JointFeatureList : gz::physics::FeatureList< - gz::physics::GetEngineInfo, gz::physics::ForwardStep, + gz::physics::GetBasicJointProperties, gz::physics::GetBasicJointState, - gz::physics::sdf::ConstructSdfWorld, - gz::physics::GetModelFromWorld, - gz::physics::GetLinkFromModel, + gz::physics::GetEngineInfo, gz::physics::GetJointFromModel, - gz::physics::SetBasicJointState, - gz::physics::GetBasicJointProperties, + gz::physics::GetLinkFromModel, + gz::physics::GetModelFromWorld, gz::physics::LinkFrameSemantics, - gz::physics::SetJointVelocityCommandFeature + gz::physics::SetBasicJointState, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::sdf::ConstructSdfWorld > { }; using JointFeaturesTestTypes = @@ -183,16 +184,16 @@ TYPED_TEST(JointFeaturesTest, JointSetCommand) } struct JointFeaturePositionLimitsList : gz::physics::FeatureList< - gz::physics::GetEngineInfo, gz::physics::ForwardStep, + gz::physics::GetBasicJointProperties, gz::physics::GetBasicJointState, - gz::physics::sdf::ConstructSdfWorld, - gz::physics::GetModelFromWorld, + gz::physics::GetEngineInfo, gz::physics::GetJointFromModel, + gz::physics::GetModelFromWorld, gz::physics::SetBasicJointState, - gz::physics::GetBasicJointProperties, + gz::physics::SetJointPositionLimitsFeature, gz::physics::SetJointVelocityCommandFeature, - gz::physics::SetJointPositionLimitsFeature + gz::physics::sdf::ConstructSdfWorld > { }; template @@ -277,18 +278,21 @@ TYPED_TEST(JointFeaturesPositionLimitsTest, JointSetPositionLimitsWithForceContr } struct JointFeaturePositionLimitsForceControlList : gz::physics::FeatureList< - gz::physics::GetEngineInfo, gz::physics::ForwardStep, + gz::physics::GetBasicJointProperties, gz::physics::GetBasicJointState, - gz::physics::sdf::ConstructSdfWorld, - gz::physics::GetModelFromWorld, + 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::GetBasicJointProperties, - gz::physics::SetJointVelocityCommandFeature, + gz::physics::SetJointEffortLimitsFeature, gz::physics::SetJointPositionLimitsFeature, + gz::physics::SetJointVelocityCommandFeature, gz::physics::SetJointVelocityLimitsFeature, - gz::physics::SetJointEffortLimitsFeature + gz::physics::sdf::ConstructSdfWorld > { }; template @@ -811,20 +815,20 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetCombinedLimitsWi struct JointFeatureDetachList : gz::physics::FeatureList< - gz::physics::GetEngineInfo, + gz::physics::DetachJointFeature, gz::physics::ForwardStep, + gz::physics::FreeJointCast, + gz::physics::GetBasicJointProperties, gz::physics::GetBasicJointState, - gz::physics::sdf::ConstructSdfWorld, - gz::physics::GetModelFromWorld, + gz::physics::GetEngineInfo, gz::physics::GetJointFromModel, gz::physics::GetLinkFromModel, - gz::physics::SetBasicJointState, - gz::physics::GetBasicJointProperties, - gz::physics::SetJointVelocityCommandFeature, - gz::physics::FreeJointCast, + gz::physics::GetModelFromWorld, gz::physics::LinkFrameSemantics, gz::physics::RevoluteJointCast, - gz::physics::DetachJointFeature + gz::physics::SetBasicJointState, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::sdf::ConstructSdfWorld > { }; template @@ -971,23 +975,23 @@ TYPED_TEST(JointFeaturesDetachTest, JointDetach) } struct JointFeatureAttachDetachList : gz::physics::FeatureList< - gz::physics::GetEngineInfo, + gz::physics::AttachFixedJointFeature, + gz::physics::DetachJointFeature, gz::physics::ForwardStep, + gz::physics::FreeJointCast, + gz::physics::GetBasicJointProperties, gz::physics::GetBasicJointState, - gz::physics::sdf::ConstructSdfWorld, - gz::physics::GetModelFromWorld, + gz::physics::GetEngineInfo, gz::physics::GetJointFromModel, gz::physics::GetLinkFromModel, - gz::physics::SetBasicJointState, - gz::physics::GetBasicJointProperties, - gz::physics::SetJointVelocityCommandFeature, - gz::physics::FreeJointCast, + gz::physics::GetModelFromWorld, gz::physics::LinkFrameSemantics, gz::physics::RevoluteJointCast, - gz::physics::DetachJointFeature, - gz::physics::AttachFixedJointFeature, + gz::physics::SetBasicJointState, gz::physics::SetJointTransformFromParentFeature, - gz::physics::sdf::ConstructSdfModel + gz::physics::SetJointVelocityCommandFeature, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld > { }; template @@ -1529,18 +1533,18 @@ class JointTransmittedWrenchFixture : public JointFeaturesTest }; struct JointTransmittedWrenchFeatureList : gz::physics::FeatureList< - gz::physics::GetEngineInfo, gz::physics::ForwardStep, + gz::physics::FreeGroupFrameSemantics, gz::physics::GetBasicJointState, - gz::physics::sdf::ConstructSdfWorld, - gz::physics::GetModelFromWorld, - gz::physics::GetLinkFromModel, + gz::physics::GetEngineInfo, gz::physics::GetJointFromModel, - gz::physics::SetBasicJointState, + gz::physics::GetJointTransmittedWrench, + gz::physics::GetLinkFromModel, + gz::physics::GetModelFromWorld, gz::physics::LinkFrameSemantics, + gz::physics::SetBasicJointState, gz::physics::SetFreeGroupWorldPose, - gz::physics::FreeGroupFrameSemantics, - gz::physics::GetJointTransmittedWrench + gz::physics::sdf::ConstructSdfWorld > { }; using JointTransmittedWrenchFeaturesTestTypes =