From 3bce17d988f460848007ee84d962f879cc2d8de6 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 9 Nov 2023 04:03:18 -0800 Subject: [PATCH 1/6] simulation_features test: struct for faster build Signed-off-by: Steve Peters --- test/common_test/simulation_features.cc | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 1087e2d5c..9dd285a9b 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -52,7 +52,7 @@ #include // The features that an engine must have to be loaded by this loader. -using Features = gz::physics::FeatureList< +struct Features : gz::physics::FeatureList< gz::physics::ConstructEmptyWorldFeature, gz::physics::FindFreeGroupFeature, @@ -84,7 +84,7 @@ using Features = gz::physics::FeatureList< gz::physics::GetCylinderShapeProperties, gz::physics::GetCapsuleShapeProperties, gz::physics::GetEllipsoidShapeProperties ->; +> {}; template class SimulationFeaturesTest: @@ -184,11 +184,11 @@ std::pair StepWorld( } // The features that an engine must have to be loaded by this loader. -using FeaturesContacts = gz::physics::FeatureList< +struct FeaturesContacts : gz::physics::FeatureList< gz::physics::sdf::ConstructSdfWorld, gz::physics::GetContactsFromLastStepFeature, gz::physics::ForwardStep ->; +> {}; template class SimulationFeaturesContactsTest : @@ -218,10 +218,10 @@ TYPED_TEST(SimulationFeaturesContactsTest, Contacts) // The features that an engine must have to be loaded by this loader. -using FeaturesStep = gz::physics::FeatureList< +struct FeaturesStep : gz::physics::FeatureList< gz::physics::sdf::ConstructSdfWorld, gz::physics::ForwardStep ->; +> {}; template class SimulationFeaturesStepTest : @@ -303,7 +303,7 @@ TYPED_TEST(SimulationFeaturesFallingTest, Falling) // The features that an engine must have to be loaded by this loader. -using FeaturesShapeFeatures = gz::physics::FeatureList< +struct FeaturesShapeFeatures : gz::physics::FeatureList< gz::physics::sdf::ConstructSdfWorld, gz::physics::GetModelFromWorld, gz::physics::GetLinkFromModel, @@ -321,7 +321,7 @@ using FeaturesShapeFeatures = gz::physics::FeatureList< gz::physics::GetCylinderShapeProperties, gz::physics::GetCapsuleShapeProperties, gz::physics::GetEllipsoidShapeProperties ->; +> {}; template class SimulationFeaturesShapeFeaturesTest : @@ -796,7 +796,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) } } -using FeaturesContactPropertiesCallback = gz::physics::FeatureList< +struct FeaturesContactPropertiesCallback : gz::physics::FeatureList< gz::physics::ConstructEmptyWorldFeature, gz::physics::FindFreeGroupFeature, @@ -833,7 +833,7 @@ using FeaturesContactPropertiesCallback = gz::physics::FeatureList< gz::physics::GetCylinderShapeProperties, gz::physics::GetCapsuleShapeProperties, gz::physics::GetEllipsoidShapeProperties ->; +> {}; #ifdef DART_HAS_CONTACT_SURFACE using ContactSurfaceParams = From 33e12fa800e31f563a6806aa0051c2607ccc1347 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 9 Nov 2023 04:07:14 -0800 Subject: [PATCH 2/6] Use separate FeatureList for FreeGroup test Signed-off-by: Steve Peters --- test/common_test/simulation_features.cc | 42 +++++++++++++++++++------ 1 file changed, 33 insertions(+), 9 deletions(-) diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 9dd285a9b..7332e32f2 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -490,19 +490,35 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) } } +struct FreeGroupFeatures : gz::physics::FeatureList< + gz::physics::FindFreeGroupFeature, + gz::physics::SetFreeGroupWorldPose, + gz::physics::SetFreeGroupWorldVelocity, + + gz::physics::GetModelFromWorld, + gz::physics::GetLinkFromModel, + + gz::physics::FreeGroupFrameSemantics, + gz::physics::LinkFrameSemantics, + + gz::physics::sdf::ConstructSdfWorld, + + gz::physics::ForwardStep +> {}; + template -class SimulationFeaturesTestBasic : +class SimulationFeaturesTestFreeGroup : public SimulationFeaturesTest{}; -using SimulationFeaturesTestBasicTypes = - ::testing::Types; -TYPED_TEST_SUITE(SimulationFeaturesTestBasic, - SimulationFeaturesTestBasicTypes); +using SimulationFeaturesTestFreeGroupTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesTestFreeGroup, + SimulationFeaturesTestFreeGroupTypes); -TYPED_TEST(SimulationFeaturesTestBasic, FreeGroup) +TYPED_TEST(SimulationFeaturesTestFreeGroup, FreeGroup) { for (const std::string &name : this->pluginNames) { - auto world = LoadPluginAndWorld( + auto world = LoadPluginAndWorld( this->loader, name, gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); @@ -520,7 +536,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, FreeGroup) auto freeGroupLink = link->FindFreeGroup(); ASSERT_NE(nullptr, freeGroupLink); - StepWorld(world, true); + StepWorld(world, true); freeGroup->SetWorldPose( gz::math::eigen3::convert( @@ -535,7 +551,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, 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( @@ -545,6 +561,14 @@ TYPED_TEST(SimulationFeaturesTestBasic, FreeGroup) } } +template +class SimulationFeaturesTestBasic : + public SimulationFeaturesTest{}; +using SimulationFeaturesTestBasicTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesTestBasic, + SimulationFeaturesTestBasicTypes); + TYPED_TEST(SimulationFeaturesTestBasic, ShapeBoundingBox) { for (const std::string &name : this->pluginNames) From a4359eff8b3dce704bb10387532d591ee876d4b2 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 9 Nov 2023 04:29:36 -0800 Subject: [PATCH 3/6] Refactor FreeGroup test * Use falling.world instead of shapes.world to avoid initial contact. * Verify free group pose and velocities in addition to link values. * Use AssertVectorApprox for velocity expectations, and use tighter tolerances. Signed-off-by: Steve Peters --- test/common_test/simulation_features.cc | 74 +++++++++++++++++++------ 1 file changed, 57 insertions(+), 17 deletions(-) diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 7332e32f2..2da71221a 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -139,6 +139,8 @@ gz::physics::World3dPtr LoadPluginAndWorld( return world; } +using AssertVectorApprox = gz::physics::test::AssertVectorApprox; + /// \brief Step forward in a world /// \param[in] _world The world to step in /// \param[in] _firstTime Whether this is the very first time this world is @@ -521,7 +523,7 @@ TYPED_TEST(SimulationFeaturesTestFreeGroup, FreeGroup) auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); + gz::common::joinPaths(TEST_WORLD_DIR, "falling.world")); // model free group test auto model = world->GetModel("sphere"); @@ -538,26 +540,64 @@ TYPED_TEST(SimulationFeaturesTestFreeGroup, FreeGroup) StepWorld(world, true); + // Set initial pose. + const gz::math::Pose3d initialPose{0, 0, 2, 0, 0, 0}; freeGroup->SetWorldPose( - gz::math::eigen3::convert( - gz::math::Pose3d(0, 0, 2, 0, 0, 0))); - freeGroup->SetWorldLinearVelocity( - gz::math::eigen3::convert(gz::math::Vector3d(0.1, 0.2, 0.3))); - freeGroup->SetWorldAngularVelocity( - gz::math::eigen3::convert(gz::math::Vector3d(0.4, 0.5, 0.6))); - - auto frameData = model->GetLink(0)->FrameDataRelativeToWorld(); - EXPECT_EQ(gz::math::Pose3d(0, 0, 2, 0, 0, 0), - gz::math::eigen3::convert(frameData.pose)); + gz::math::eigen3::convert(initialPose)); + + // Set initial velocities. + const Eigen::Vector3d initialLinearVelocity{0.1, 0.2, 0.3}; + const Eigen::Vector3d initialAngularVelocity{0.4, 0.5, 0.6}; + freeGroup->SetWorldLinearVelocity(initialLinearVelocity); + freeGroup->SetWorldAngularVelocity(initialAngularVelocity); + + auto freeGroupFrameData = freeGroup->FrameDataRelativeToWorld(); + auto linkFrameData = model->GetLink(0)->FrameDataRelativeToWorld(); + + // Before stepping, check that pose matches what was set. + EXPECT_EQ(initialPose, + gz::math::eigen3::convert(freeGroupFrameData.pose)); + EXPECT_EQ(initialPose, + gz::math::eigen3::convert(linkFrameData.pose)); + + // Before stepping, check that velocities match what was set. + AssertVectorApprox vectorPredicateVelocity(1e-7); + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + initialLinearVelocity, + freeGroupFrameData.linearVelocity); + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + initialLinearVelocity, + linkFrameData.linearVelocity); + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + initialAngularVelocity, + freeGroupFrameData.angularVelocity); + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + initialAngularVelocity, + linkFrameData.angularVelocity); // Step the world StepWorld(world, false); - // Check that the first link's velocities are updated - frameData = model->GetLink(0)->FrameDataRelativeToWorld(); - EXPECT_TRUE(gz::math::Vector3d(0.1, 0.2, 0.3).Equal( - gz::math::eigen3::convert(frameData.linearVelocity), 0.1)); - EXPECT_EQ(gz::math::Vector3d(0.4, 0.5, 0.6), - gz::math::eigen3::convert(frameData.angularVelocity)); + + // Check the velocities again. + freeGroupFrameData = freeGroup->FrameDataRelativeToWorld(); + linkFrameData = model->GetLink(0)->FrameDataRelativeToWorld(); + + // Expect linear velocity to be affected by gravity. + const Eigen::Vector3d linearVelocityAfterStep{0.1, 0.2, 0.3 - 9.8 * 0.001}; + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + linearVelocityAfterStep, + freeGroupFrameData.linearVelocity); + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + linearVelocityAfterStep, + linkFrameData.linearVelocity); + + // Expect angular velocity is unchanged. + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + initialAngularVelocity, + freeGroupFrameData.angularVelocity); + EXPECT_PRED_FORMAT2(vectorPredicateVelocity, + initialAngularVelocity, + linkFrameData.angularVelocity); } } From 28a0d15e56be9ba8a7700b1f6ec73820bf1c7ab0 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 9 Nov 2023 04:39:13 -0800 Subject: [PATCH 4/6] bullet-featherstone: fix setting angular velocity * Use btMultiBody::setBaseOmega API * Fix null pointer check Signed-off-by: Steve Peters --- bullet-featherstone/src/FreeGroupFeatures.cc | 24 +------------------ bullet-featherstone/src/KinematicsFeatures.cc | 2 +- 2 files changed, 2 insertions(+), 24 deletions(-) diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index 5fd18e9f3..c557d6465 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -68,29 +68,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( if (model != nullptr) { - // Set angular velocity the each one of the joints of the model - for (const auto& jointID : model->jointEntityIds) - { - auto jointInfo = this->joints[jointID]; - if (!jointInfo->motor) - { - auto *modelInfo = this->ReferenceInterface(jointInfo->model); - jointInfo->motor = std::make_shared( - modelInfo->body.get(), - std::get(jointInfo->identifier).indexInBtModel, - 0, - static_cast(0), - static_cast(jointInfo->effort)); - auto *world = this->ReferenceInterface(modelInfo->world); - world->world->addMultiBodyConstraint(jointInfo->motor.get()); - } - - if (jointInfo->motor) - { - jointInfo->motor->setVelocityTarget( - static_cast(_angularVelocity[2])); - } - } + model->body->setBaseOmega(convertVec(_angularVelocity)); } } diff --git a/bullet-featherstone/src/KinematicsFeatures.cc b/bullet-featherstone/src/KinematicsFeatures.cc index 74f16776d..85341ce6a 100644 --- a/bullet-featherstone/src/KinematicsFeatures.cc +++ b/bullet-featherstone/src/KinematicsFeatures.cc @@ -104,7 +104,7 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( } } - if (model->body == nullptr) + if (!model || model->body == nullptr) model = this->FrameInterface(_id); } From d604c92e83d587c26c67a5c9d6a37c3465961603 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 9 Nov 2023 05:24:39 -0800 Subject: [PATCH 5/6] Use test world without gravity to fix tpe Signed-off-by: Steve Peters --- test/common_test/simulation_features.cc | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 2da71221a..80c293ea3 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -523,7 +523,7 @@ TYPED_TEST(SimulationFeaturesTestFreeGroup, FreeGroup) auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "falling.world")); + gz::common::joinPaths(TEST_WORLD_DIR, "sphere.sdf")); // model free group test auto model = world->GetModel("sphere"); @@ -581,14 +581,11 @@ TYPED_TEST(SimulationFeaturesTestFreeGroup, FreeGroup) // Check the velocities again. freeGroupFrameData = freeGroup->FrameDataRelativeToWorld(); linkFrameData = model->GetLink(0)->FrameDataRelativeToWorld(); - - // Expect linear velocity to be affected by gravity. - const Eigen::Vector3d linearVelocityAfterStep{0.1, 0.2, 0.3 - 9.8 * 0.001}; EXPECT_PRED_FORMAT2(vectorPredicateVelocity, - linearVelocityAfterStep, + initialLinearVelocity, freeGroupFrameData.linearVelocity); EXPECT_PRED_FORMAT2(vectorPredicateVelocity, - linearVelocityAfterStep, + initialLinearVelocity, linkFrameData.linearVelocity); // Expect angular velocity is unchanged. From 018e8bdfad53b9b7b9ef411598633598c6ec04d2 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 9 Nov 2023 10:21:48 -0800 Subject: [PATCH 6/6] remove outdated comment Signed-off-by: Steve Peters Signed-off-by: Steve Peters --- test/common_test/simulation_features.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 80c293ea3..44eb69456 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -587,8 +587,6 @@ TYPED_TEST(SimulationFeaturesTestFreeGroup, FreeGroup) EXPECT_PRED_FORMAT2(vectorPredicateVelocity, initialLinearVelocity, linkFrameData.linearVelocity); - - // Expect angular velocity is unchanged. EXPECT_PRED_FORMAT2(vectorPredicateVelocity, initialAngularVelocity, freeGroupFrameData.angularVelocity);