bullet-featherstone: fix setting angular velocity

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<ModelInfo>(jointInfo->model);
-        jointInfo->motor = std::make_shared<btMultiBodyJointMotor>(
-          modelInfo->body.get(),
-          std::get<InternalJoint>(jointInfo->identifier).indexInBtModel,
-          0,
-          static_cast<btScalar>(0),
-          static_cast<btScalar>(jointInfo->effort));
-        auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
-        world->world->addMultiBodyConstraint(jointInfo->motor.get());
-      }
-
-      if (jointInfo->motor)
-      {
-        jointInfo->motor->setVelocityTarget(
-            static_cast<btScalar>(_angularVelocity[2]));
-      }
-    }
+    model->body->setBaseOmega(convertVec(_angularVelocity));
   }
 }
 

bullet-featherstone/src/KinematicsFeatures.cc

@@ -104,7 +104,7 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
       }
     }
 
-    if (model->body == nullptr)
+    if (!model || model->body == nullptr)
       model = this->FrameInterface<ModelInfo>(_id);
   }
 

test/common_test/simulation_features.cc

@@ -52,7 +52,7 @@
 #include <sdf/Root.hh>
 
 // 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 T>
 class SimulationFeaturesTest:
@@ -139,6 +139,8 @@ gz::physics::World3dPtr<T> 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<bool, gz::physics::ForwardStep::Output> 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 T>
 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 T>
 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 T>
 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 T>
-class SimulationFeaturesTestBasic :
+class SimulationFeaturesTestFreeGroup :
   public SimulationFeaturesTest<T>{};
-using SimulationFeaturesTestBasicTypes =
-  ::testing::Types<Features>;
-TYPED_TEST_SUITE(SimulationFeaturesTestBasic,
-                 SimulationFeaturesTestBasicTypes);
+using SimulationFeaturesTestFreeGroupTypes =
+  ::testing::Types<FreeGroupFeatures>;
+TYPED_TEST_SUITE(SimulationFeaturesTestFreeGroup,
+                 SimulationFeaturesTestFreeGroupTypes);
 
-TYPED_TEST(SimulationFeaturesTestBasic, FreeGroup)
+TYPED_TEST(SimulationFeaturesTestFreeGroup, FreeGroup)
 {
   for (const std::string &name : this->pluginNames)
   {
-    auto world = LoadPluginAndWorld<Features>(
+    auto world = LoadPluginAndWorld<FreeGroupFeatures>(
       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<Features>(world, true);
+    StepWorld<FreeGroupFeatures>(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<Features>(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<FreeGroupFeatures>(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 T>
+class SimulationFeaturesTestBasic :
+  public SimulationFeaturesTest<T>{};
+using SimulationFeaturesTestBasicTypes =
+  ::testing::Types<Features>;
+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 =