From 5e58f888ad50c31400b6a421a65133cf8ee568b0 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 9 Nov 2023 12:04:49 -0800 Subject: [PATCH] bullet-featherstone: fix setting angular velocity (#567) This fixes the SetFreeGroupWorldAngularVelocity implementation in bullet-featherstone and updates the FreeGroup test in the simulation_features common test with the following changes: * Use FeatureList struct for faster build * Use sphere.sdf instead of shapes.world to avoid initial contact and gravity. * Verify free group pose and velocities in addition to link values. * Use tighter tolerances for velocity expectations. Signed-off-by: Steve Peters --- bullet-featherstone/src/FreeGroupFeatures.cc | 24 +--- bullet-featherstone/src/KinematicsFeatures.cc | 2 +- test/common_test/simulation_features.cc | 131 +++++++++++++----- 3 files changed, 97 insertions(+), 60 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); } diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 1087e2d5c..44eb69456 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: @@ -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 @@ -184,11 +186,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 +220,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 +305,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 +323,7 @@ using FeaturesShapeFeatures = gz::physics::FeatureList< gz::physics::GetCylinderShapeProperties, gz::physics::GetCapsuleShapeProperties, gz::physics::GetEllipsoidShapeProperties ->; +> {}; template class SimulationFeaturesShapeFeaturesTest : @@ -490,22 +492,38 @@ 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")); + gz::common::joinPaths(TEST_WORLD_DIR, "sphere.sdf")); // model free group test auto model = world->GetModel("sphere"); @@ -520,31 +538,72 @@ TYPED_TEST(SimulationFeaturesTestBasic, FreeGroup) auto freeGroupLink = link->FindFreeGroup(); ASSERT_NE(nullptr, freeGroupLink); - StepWorld(world, true); + 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)); + StepWorld(world, false); + + // Check the velocities again. + freeGroupFrameData = freeGroup->FrameDataRelativeToWorld(); + linkFrameData = model->GetLink(0)->FrameDataRelativeToWorld(); + 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); } } +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) @@ -796,7 +855,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) } } -using FeaturesContactPropertiesCallback = gz::physics::FeatureList< +struct FeaturesContactPropertiesCallback : gz::physics::FeatureList< gz::physics::ConstructEmptyWorldFeature, gz::physics::FindFreeGroupFeature, @@ -833,7 +892,7 @@ using FeaturesContactPropertiesCallback = gz::physics::FeatureList< gz::physics::GetCylinderShapeProperties, gz::physics::GetCapsuleShapeProperties, gz::physics::GetEllipsoidShapeProperties ->; +> {}; #ifdef DART_HAS_CONTACT_SURFACE using ContactSurfaceParams =