Merge branch 'arjo/feat/backward_in_test' of https://github.com/gazeb…

…osim/gz-sim into arjo/feat/backward_in_test

README.md

@@ -12,7 +12,7 @@ Build | Status
 Test coverage | [![codecov](https://codecov.io/gh/gazebosim/gz-sim/tree/gz-sim9/graph/badge.svg)](https://codecov.io/gh/gazebosim/gz-sim/tree/gz-sim9)
 Ubuntu Noble  | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_sim-ci-gz-sim9-noble-amd64)](https://build.osrfoundation.org/job/gz_sim-ci-gz-sim9-noble-amd64)
 Homebrew      | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_sim-ci-gz-sim9-homebrew-amd64)](https://build.osrfoundation.org/job/gz_sim-ci-gz-sim9-homebrew-amd64)
-Windows       | [![Build Status](https://build.osrfoundation.org/job/gz_sim-9-win/badge/icon)](https://build.osrfoundation.org/job/gz_sim-9-win/)
+Windows       | [![Build Status](https://build.osrfoundation.org/job/gz_sim-9-clowin/badge/icon)](https://build.osrfoundation.org/job/gz_sim-9-clowin/)
 
 Gazebo Sim is an open source robotics simulator. Through Gazebo Sim, users have access to high fidelity physics, rendering, and sensor models. Additionally, users and developers have multiple points of entry to simulation including a graphical user interface, plugins, and asynchronous message passing and services.
 

include/gz/sim/Util.hh

@@ -250,18 +250,22 @@ namespace gz
     /// \param[in] _entity Entity whose component is being enabled
     /// \param[in] _enable True to enable (create), false to disable (remove).
     /// Defaults to true.
+    /// \param[in] _comp The component to create if neeeded. Defaults to a
+    /// default-constructed component.
     /// \return True if a component was created or removed, false if nothing
     /// changed.
     template <class ComponentType>
     bool enableComponent(EntityComponentManager &_ecm,
-        Entity _entity, bool _enable = true)
+        Entity _entity,
+        bool _enable = true,
+        const ComponentType &_comp = ComponentType())
     {
       bool changed{false};
 
       auto exists = _ecm.Component<ComponentType>(_entity);
       if (_enable && !exists)
       {
-        _ecm.CreateComponent(_entity, ComponentType());
+        _ecm.CreateComponent(_entity, _comp);
         changed = true;
       }
       else if (!_enable && exists)

python/test/gravity.sdf

@@ -17,6 +17,11 @@
           <mass>1.0</mass>
         </inertial>
       </link>
+      <plugin filename="gz-sim-python-system-loader-system"
+        name="gz::sim::systems::PythonSystemLoader">
+        <module_name>plugins.test_model_system</module_name>
+      </plugin>
     </model>
+
   </world>
 </sdf>

src/CMakeLists.txt

@@ -113,6 +113,7 @@ set (gtest_sources
   Joint_TEST.cc
   Light_TEST.cc
   Link_TEST.cc
+  MeshInertiaCalculator_TEST.cc
   Model_TEST.cc
   Primitives_TEST.cc
   SdfEntityCreator_TEST.cc

src/Link.cc

@@ -284,16 +284,37 @@ std::optional<math::Vector3d> Link::WorldAngularVelocity(
 void Link::EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable)
     const
 {
-  enableComponent<components::WorldLinearVelocity>(_ecm, this->dataPtr->id,
-      _enable);
-  enableComponent<components::WorldAngularVelocity>(_ecm, this->dataPtr->id,
-      _enable);
-  enableComponent<components::LinearVelocity>(_ecm, this->dataPtr->id,
-      _enable);
-  enableComponent<components::AngularVelocity>(_ecm, this->dataPtr->id,
-      _enable);
-  enableComponent<components::WorldPose>(_ecm, this->dataPtr->id,
-      _enable);
+  auto defaultWorldPose = math::Pose3d::Zero;
+  if (_enable)
+  {
+    defaultWorldPose = sim::worldPose(this->dataPtr->id, _ecm);
+  }
+
+  enableComponent(_ecm, this->dataPtr->id,
+    _enable, components::LinearVelocity());
+  enableComponent(_ecm, this->dataPtr->id,
+    _enable, components::AngularVelocity());
+  enableComponent(_ecm, this->dataPtr->id,
+    _enable, components::WorldPose(defaultWorldPose));
+
+  auto defaultWorldLinVel = math::Vector3d::Zero;
+  auto defaultWorldAngVel = math::Vector3d::Zero;
+  if (_enable)
+  {
+    // The WorldPose component is guaranteed to exist at this point
+    auto worldPose = this->WorldPose(_ecm).value();
+
+    // Compute the default world linear and angular velocities
+    defaultWorldLinVel = worldPose.Rot().RotateVector(
+      _ecm.Component<components::LinearVelocity>(this->dataPtr->id)->Data());
+    defaultWorldAngVel = worldPose.Rot().RotateVector(
+      _ecm.Component<components::AngularVelocity>(this->dataPtr->id)->Data());
+  }
+
+  enableComponent(_ecm, this->dataPtr->id,
+    _enable, components::WorldLinearVelocity(defaultWorldLinVel));
+  enableComponent(_ecm, this->dataPtr->id,
+    _enable, components::WorldAngularVelocity(defaultWorldAngVel));
 }
 
 //////////////////////////////////////////////////

src/Link_TEST.cc

@@ -19,9 +19,14 @@
 
 #include "gz/sim/EntityComponentManager.hh"
 #include "gz/sim/Link.hh"
+#include "gz/sim/Util.hh"
+#include "gz/sim/components/AngularVelocity.hh"
+#include "gz/sim/components/LinearVelocity.hh"
 #include "gz/sim/components/Link.hh"
+#include "gz/sim/components/Model.hh"
 #include "gz/sim/components/Name.hh"
 #include "gz/sim/components/ParentEntity.hh"
+#include "gz/sim/components/Pose.hh"
 #include "gz/sim/components/Sensor.hh"
 
 /////////////////////////////////////////////////
@@ -139,3 +144,67 @@ TEST(LinkTest, Sensors)
   EXPECT_EQ(0u, linkC.Sensors(ecm).size());
   EXPECT_EQ(gz::sim::kNullEntity, linkC.SensorByName(ecm, "invalid"));
 }
+
+TEST(LinkTest, EnableVelocityChecksCreatesAdequateWorldComponents)
+{
+  gz::sim::EntityComponentManager ecm;
+
+  gz::math::Pose3d modelWorldPose(-1.2, -3.4, -5.6, 0.1, 0.2, 0.3);
+  gz::math::Vector3d linkWorldLinvel(0.1, 0.2, -0.3);
+  gz::math::Vector3d linkWorldAngvel(0.04, -0.05, 0.06);
+
+  auto bodyLinvel = modelWorldPose.Rot().RotateVectorReverse(linkWorldLinvel);
+  auto bodyAngvel = modelWorldPose.Rot().RotateVectorReverse(linkWorldAngvel);
+
+  // Create a model with a child link
+  gz::sim::Entity modelEntity = ecm.CreateEntity();
+
+  ecm.CreateComponent(modelEntity,
+      gz::sim::components::Model());
+  ecm.CreateComponent(modelEntity,
+      gz::sim::components::Name("model_name"));
+  ecm.CreateComponent(modelEntity,
+      gz::sim::components::Pose(modelWorldPose));
+
+  gz::sim::Entity linkEntity = ecm.CreateEntity();
+
+  ecm.CreateComponent(linkEntity,
+      gz::sim::components::Link());
+  ecm.CreateComponent(linkEntity,
+      gz::sim::components::Name("link_name"));
+  ecm.CreateComponent(linkEntity,
+      gz::sim::components::ParentEntity(modelEntity));
+  ecm.CreateComponent(linkEntity,
+      gz::sim::components::Pose());
+  ecm.CreateComponent(linkEntity,
+      gz::sim::components::LinearVelocity(bodyLinvel));
+  ecm.CreateComponent(linkEntity,
+      gz::sim::components::AngularVelocity(bodyAngvel));
+
+  // The link's world pose should be correctly resolved
+  EXPECT_EQ(modelWorldPose, gz::sim::worldPose(linkEntity, ecm));
+
+  gz::sim::Link link(linkEntity);
+
+  // Body velocities should be preserved since they were in the ECM already
+  EXPECT_EQ(bodyLinvel,
+      ecm.Component<gz::sim::components::LinearVelocity>(linkEntity)->Data());
+  EXPECT_EQ(bodyAngvel,
+      ecm.Component<gz::sim::components::AngularVelocity>(linkEntity)->Data());
+  EXPECT_EQ(modelWorldPose, link.WorldPose(ecm).value());
+
+  // Enable velocity checks should provide the correct world components
+  // if they have not been computed and inserted in the ECM yet
+  link.EnableVelocityChecks(ecm, true);
+
+  EXPECT_EQ(bodyLinvel,
+      ecm.Component<gz::sim::components::LinearVelocity>(linkEntity)->Data());
+  EXPECT_EQ(bodyAngvel,
+      ecm.Component<gz::sim::components::AngularVelocity>(linkEntity)->Data());
+  EXPECT_EQ(modelWorldPose,
+      link.WorldPose(ecm).value());
+  EXPECT_EQ(linkWorldLinvel,
+      link.WorldLinearVelocity(ecm).value());
+  EXPECT_EQ(linkWorldAngvel,
+      link.WorldAngularVelocity(ecm).value());
+}

src/MeshInertiaCalculator.cc

@@ -17,6 +17,8 @@
 
 #include "MeshInertiaCalculator.hh"
 
+#include <algorithm>
+#include <numeric>
 #include <optional>
 #include <vector>
 
@@ -25,9 +27,8 @@
 
 #include <gz/sim/Util.hh>
 
-#include <gz/common/graphics.hh>
-#include <gz/common/Mesh.hh>
 #include <gz/common/MeshManager.hh>
+#include <gz/common/SubMesh.hh>
 
 #include <gz/math/Vector3.hh>
 #include <gz/math/Pose3.hh>
@@ -38,19 +39,69 @@
 using namespace gz;
 using namespace sim;
 
+//////////////////////////////////////////////////
+bool MeshInertiaCalculator::CorrectMassMatrix(
+    math::MassMatrix3d &_massMatrix, double _tol)
+{
+  if (_massMatrix.IsValid())
+    return true;
+
+  if (!_massMatrix.IsPositive())
+    return false;
+
+  math::Quaterniond principalAxesOffset = _massMatrix.PrincipalAxesOffset();
+  math::Vector3d principalMoments = _massMatrix.PrincipalMoments();
+
+  // Track elements in principalMoments in ascending order using a sorted
+  // indices array, i.e. sortedIndices[0] should point to the index in
+  // principalMoments containing the smallest value, while sortedIndices[2]
+  // should point to the index in principalMoments containing the largest value.
+  std::vector<int> sortedIndices(3);
+  std::iota(sortedIndices.begin(), sortedIndices.end(), 0);
+  std::sort(sortedIndices.begin(), sortedIndices.end(), [&](int i, int j)
+      { return principalMoments[i] < principalMoments[j]; } );
+
+  // Check if principal moments are within tol of satisfying the
+  // triangle inequality.
+  const double ratio =
+      (principalMoments[sortedIndices[0]] + principalMoments[sortedIndices[1]])
+      / principalMoments[sortedIndices[2]];
+  if ((1.0 - ratio) > _tol)
+  {
+    // The error is outside of tol so do not attempt to correct the mass matrix.
+    return false;
+  }
+  // Scale the 2 smaller elements in principalMoments so that they
+  // satisfy the triangle inequality
+  const double scale = 1.0 / ratio;
+  math::Vector3d correctedPrincipalMoments = principalMoments;
+  correctedPrincipalMoments[sortedIndices[0]] *= scale;
+  correctedPrincipalMoments[sortedIndices[1]] *= scale;
+
+  // Recompute mass matrix with corrected principal moments.
+  math::MassMatrix3d correctedPrincipalMassMatrix(_massMatrix.Mass(),
+      correctedPrincipalMoments, math::Vector3d::Zero);
+  math::Inertiald correctedPrincipalMassMatrixWithAxesOffset(
+      correctedPrincipalMassMatrix,
+      math::Pose3d(math::Vector3d::Zero, principalAxesOffset));
+  _massMatrix.SetMoi(correctedPrincipalMassMatrixWithAxesOffset.Moi());
+
+  return true;
+}
+
 //////////////////////////////////////////////////
 void MeshInertiaCalculator::GetMeshTriangles(
   std::vector<Triangle> &_triangles,
   const gz::math::Vector3d &_meshScale,
-  const gz::common::Mesh* _mesh)
+  const gz::common::SubMesh *_subMesh)
 {
   // Get the vertices & indices of the mesh
   double* vertArray = nullptr;
   int* indArray = nullptr;
-  _mesh->FillArrays(&vertArray, &indArray);
+  _subMesh->FillArrays(&vertArray, &indArray);
 
   // Add check to see if size of the ind array is divisible by 3
-  for (unsigned int i = 0; i < _mesh->IndexCount(); i += 3)
+  for (unsigned int i = 0; i < _subMesh->IndexCount(); i += 3)
   {
     Triangle triangle;
     triangle.v0.X() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i])];
@@ -187,62 +238,71 @@ void MeshInertiaCalculator::CalculateMassProperties(
 
 //////////////////////////////////////////////////
 std::optional<gz::math::Inertiald> MeshInertiaCalculator::operator()
-  (sdf::Errors& _errors,
+  (sdf::Errors &_errors,
   const sdf::CustomInertiaCalcProperties& _calculatorParams)
 {
-  const gz::common::Mesh *mesh = nullptr;
   const double density = _calculatorParams.Density();
 
   auto sdfMesh = _calculatorParams.Mesh();
 
   if (sdfMesh == std::nullopt)
   {
     gzerr << "Could not calculate inertia for mesh "
-    "as it std::nullopt" << std::endl;
+              "as mesh SDF is std::nullopt" << std::endl;
     _errors.push_back({sdf::ErrorCode::FATAL_ERROR,
-        "Could not calculate mesh inertia as mesh object is"
+        "Could not calculate mesh inertia as mesh SDF is"
         "std::nullopt"});
     return std::nullopt;
   }
 
-  auto fullPath = asFullPath(sdfMesh->Uri(), sdfMesh->FilePath());
+  const common::Mesh *mesh = loadMesh(*sdfMesh);
 
-  if (fullPath.empty())
+  // Compute inertia for each submesh then sum up to get the final inertia
+  // values.
+  gz::math::Inertiald meshInertial;
+  for (unsigned int i = 0; i < mesh->SubMeshCount(); ++i)
   {
-    gzerr << "Mesh geometry missing uri" << std::endl;
-    _errors.push_back({sdf::ErrorCode::URI_INVALID,
-        "Attempting to load the mesh but the URI seems to be incorrect"});
-    return std::nullopt;
+    std::vector<Triangle> meshTriangles;
+    gz::math::MassMatrix3d meshMassMatrix;
+    gz::math::Pose3d centreOfMass;
+
+    // Create a list of Triangle objects from the mesh vertices and indices
+    auto submesh = mesh->SubMeshByIndex(i).lock();
+    this->GetMeshTriangles(meshTriangles, sdfMesh->Scale(), submesh.get());
+
+    // Calculate mesh mass properties
+    this->CalculateMassProperties(meshTriangles, density,
+      meshMassMatrix, centreOfMass);
+
+    if (!meshMassMatrix.IsValid())
+    {
+      gzwarn << "Auto-calculated mass matrix is invalid for mesh: "
+             << mesh->Name() << ", submesh index: " << i << "."
+             << std::endl;
+      if (!this->CorrectMassMatrix(meshMassMatrix))
+      {
+        gzwarn << "  Unable to correct mass matrix. Skipping submesh."
+               << std::endl;
+        continue;
+      }
+      gzwarn << "  Successfully corrected mass matrix."
+             << std::endl;
+    }
+    meshInertial += gz::math::Inertiald(meshMassMatrix, centreOfMass);
   }
 
-  // Load the Mesh
-  gz::common::MeshManager *meshManager = gz::common::MeshManager::Instance();
-  mesh = meshManager->Load(fullPath);
-  if (!mesh)
+  if (meshInertial.MassMatrix().Mass() <= 0.0 ||
+      !meshInertial.MassMatrix().IsValid())
   {
-    gzerr << "Failed to load mesh: " << fullPath << std::endl;
+    gzerr << "Failed to computed valid inertia in MeshInertiaCalculator. "
+          << "Ensure that the mesh is water tight, or try optimizing the mesh "
+          << "by setting the //mesh/@optimization attribute in SDF to "
+          << "`convex_hull` or `convex_decomposition`."
+          << std::endl;
+    _errors.push_back({sdf::ErrorCode::WARNING,
+        "Could not calculate valid mesh inertia"});
     return std::nullopt;
   }
-  std::vector<Triangle> meshTriangles;
-  gz::math::MassMatrix3d meshMassMatrix;
-  gz::math::Pose3d centreOfMass;
-
-  // Create a list of Triangle objects from the mesh vertices and indices
-  this->GetMeshTriangles(meshTriangles, sdfMesh->Scale(), mesh);
 
-  // Calculate mesh mass properties
-  this->CalculateMassProperties(meshTriangles, density,
-    meshMassMatrix, centreOfMass);
-
-  gz::math::Inertiald meshInertial;
-
-  if (!meshInertial.SetMassMatrix(meshMassMatrix))
-  {
-    return std::nullopt;
-  }
-  else
-  {
-    meshInertial.SetPose(centreOfMass);
-    return meshInertial;
-  }
+  return meshInertial;
 }