Skip to content

Commit

Permalink
Add automatic moment of inertia calculation for meshes (#2061)
Browse files Browse the repository at this point in the history 
The PR implements a Moment of Inertia Calculator based on the method described in this document.
The calculator works using the callback-based API from the sdformat made in this PR.

---------

Signed-off-by: Jasmeet Singh <[email protected]>
Signed-off-by: Addisu Z. Taddese <[email protected]>
Co-authored-by: Addisu Z. Taddese <[email protected]>
  • Loading branch information
@jasmeet0915 @azeey
jasmeet0915 and azeey authored Aug 31, 2023
1 parent 8e7f612 commit 52167a6
Show file tree
Hide file tree
Showing 10 changed files with 707 additions and 3 deletions.
2 changes: 2 additions & 0 deletions src/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,7 @@ set (sources
LevelManager.cc
Light.cc
Link.cc
MeshInertiaCalculator.cc
Model.cc
Primitives.cc
SdfEntityCreator.cc
Expand Down Expand Up @@ -206,6 +207,7 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME}
gz-math${GZ_MATH_VER}
gz-plugin${GZ_PLUGIN_VER}::core
gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER}
gz-common${GZ_COMMON_VER}::graphics
gz-common${GZ_COMMON_VER}::profiler
gz-fuel_tools${GZ_FUEL_TOOLS_VER}::gz-fuel_tools${GZ_FUEL_TOOLS_VER}
gz-gui${GZ_GUI_VER}::gz-gui${GZ_GUI_VER}
Expand Down
277 changes: 277 additions & 0 deletions src/MeshInertiaCalculator.cc
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,277 @@
/*
* Copyright (C) 2023 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 "MeshInertiaCalculator.hh"

#include <optional>
#include <vector>

#include <sdf/CustomInertiaCalcProperties.hh>
#include <sdf/Types.hh>

#include <gz/sim/Util.hh>

#include <gz/common/graphics.hh>
#include <gz/common/Mesh.hh>
#include <gz/common/MeshManager.hh>

#include <gz/math/Vector3.hh>
#include <gz/math/Pose3.hh>
#include <gz/math/MassMatrix3.hh>
#include <gz/math/Inertial.hh>

using namespace gz;
using namespace sim;

//////////////////////////////////////////////////
void MeshInertiaCalculator::GetMeshTriangles(
std::vector<Triangle>& _triangles,
const gz::common::Mesh* _mesh)
{
// Get the vertices & indices of the mesh
double* vertArray = nullptr;
int* indArray = nullptr;
_mesh->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)
{
Triangle triangle;
triangle.v0.X() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i])];
triangle.v0.Y() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i] + 1)];
triangle.v0.Z() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i] + 2)];
triangle.v1.X() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i+1])];
triangle.v1.Y() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i+1] + 1)];
triangle.v1.Z() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i+1] + 2)];
triangle.v2.X() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i+2])];
triangle.v2.Y() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i+2] + 1)];
triangle.v2.Z() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i+2] + 2)];
triangle.centroid = (triangle.v0 + triangle.v1 + triangle.v2) / 3;
_triangles.push_back(triangle);
}
}

//////////////////////////////////////////////////
void MeshInertiaCalculator::CalculateMeshCentroid(
gz::math::Pose3d &_centreOfMass,
std::vector<Triangle> &_triangles)
{
gz::math::Vector3d centroid = gz::math::Vector3d::Zero;
gz::math::Vector3d triangleCross = gz::math::Vector3d::Zero;
double totalMeshArea = 0.0;
double triangleArea = 0.0;

for (Triangle &triangle : _triangles)
{
// Calculate the area of the triangle using half of
// cross product magnitude
triangleCross =
(triangle.v1 - triangle.v0).Cross(triangle.v2 - triangle.v0);
triangleArea = triangleCross.Length() / 2;

centroid = centroid + (triangle.centroid * triangleArea);
totalMeshArea = totalMeshArea + triangleArea;
}

centroid = centroid / totalMeshArea;

_centreOfMass.SetX(centroid.X());
_centreOfMass.SetY(centroid.Y());
_centreOfMass.SetZ(centroid.Z());
}

//////////////////////////////////////////////////
void MeshInertiaCalculator::CalculateMassProperties(
const std::vector<Triangle>& _triangles,
double _density,
gz::math::MassMatrix3d& _massMatrix,
gz::math::Pose3d& _inertiaOrigin)
{
// Some coefficients for the calculation of integral terms
const double coefficients[10] = {1.0 / 6, 1.0 / 24, 1.0 / 24, 1.0 / 24,
1.0 / 60, 1.0 / 60, 1.0 / 60, 1.0 / 120,
1.0 / 120, 1.0 / 120};

// Number of triangles of in the mesh
std::size_t numTriangles = _triangles.size();

// Vector to store cross products of 2 vectors of the triangles
std::vector<gz::math::Vector3d> crosses(numTriangles);

// Caculating cross products of 2 vectors emerging from a common vertex
// This basically gives a vector normal to the plane of triangle
for (std::size_t i = 0; i < numTriangles; ++i)
{
crosses[i] =
(_triangles[i].v1 - _triangles[i].v0).Cross(
_triangles[i].v2 - _triangles[i].v0);
}

// Calculate subexpressions of the integral
std::vector<gz::math::Vector3d> f1(numTriangles), f2(numTriangles),
f3(numTriangles), g0(numTriangles), g1(numTriangles), g2(numTriangles);

for (std::size_t i = 0; i < numTriangles; ++i)
{
f1[i] = _triangles[i].v0 + _triangles[i].v1 + _triangles[i].v2;
f2[i] = _triangles[i].v0 * _triangles[i].v0 +
_triangles[i].v1 * _triangles[i].v1 +
_triangles[i].v0 * _triangles[i].v1 +
_triangles[i].v2 * f1[i];
f3[i] = _triangles[i].v0 * _triangles[i].v0 * _triangles[i].v0 +
_triangles[i].v0 * _triangles[i].v0 * _triangles[i].v1 +
_triangles[i].v0 * _triangles[i].v1 * _triangles[i].v1 +
_triangles[i].v1 * _triangles[i].v1 * _triangles[i].v1 +
_triangles[i].v2 * f2[i];
g0[i] = f2[i] + (_triangles[i].v0 + f1[i]) * (_triangles[i].v0);
g1[i] = f2[i] + (_triangles[i].v1 + f1[i]) * (_triangles[i].v1);
g2[i] = f2[i] + (_triangles[i].v2 + f1[i]) * (_triangles[i].v2);
}

// Calculate integral terms
std::vector<double> integral(10);
for (std::size_t i = 0; i < numTriangles; ++i)
{
double x0 = _triangles[i].v0.X();
double y0 = _triangles[i].v0.Y();
double z0 = _triangles[i].v0.Z();
double x1 = _triangles[i].v1.X();
double y1 = _triangles[i].v1.Y();
double z1 = _triangles[i].v1.Z();
double x2 = _triangles[i].v2.X();
double y2 = _triangles[i].v2.Y();
double z2 = _triangles[i].v2.Z();
integral[0] += crosses[i].X() * f1[i].X();
integral[1] += crosses[i].X() * f2[i].X();
integral[2] += crosses[i].Y() * f2[i].Y();
integral[3] += crosses[i].Z() * f2[i].Z();
integral[4] += crosses[i].X() * f3[i].X();
integral[5] += crosses[i].Y() * f3[i].Y();
integral[6] += crosses[i].Z() * f3[i].Z();
integral[7] += crosses[i].X() *
(y0 * g0[i].X() + y1 * g1[i].X() + y2 * g2[i].X());
integral[8] += crosses[i].Y() *
(z0 * g0[i].Y() + z1 * g1[i].Y() + z2 * g2[i].Y());
integral[9] += crosses[i].Z() *
(x0 * g0[i].Z() + x1 * g1[i].Z() + x2 * g2[i].Z());
}

for (int i = 0; i < 10; ++i)
{
integral[i] *= coefficients[i];
}

// Accumulate the result and add it to MassMatrix object of gz::math
double volume = integral[0];
double mass = volume * _density;
_inertiaOrigin.SetX(integral[1] / mass);
_inertiaOrigin.SetY(integral[2] / mass);
_inertiaOrigin.SetZ(integral[3] / mass);
gz::math::Vector3d ixxyyzz = gz::math::Vector3d();
gz::math::Vector3d ixyxzyz = gz::math::Vector3d();

// Diagonal Elements of the Mass Matrix
ixxyyzz.X() = (integral[5] + integral[6] - mass *
(_inertiaOrigin.Y() * _inertiaOrigin.Y() +
_inertiaOrigin.Z() * _inertiaOrigin.Z()));
ixxyyzz.Y() = (integral[4] + integral[6] - mass *
(_inertiaOrigin.Z() * _inertiaOrigin.Z() +
_inertiaOrigin.X() * _inertiaOrigin.X()));
ixxyyzz.Z() = integral[4] + integral[5] - mass *
(_inertiaOrigin.X() * _inertiaOrigin.X() +
_inertiaOrigin.Y() * _inertiaOrigin.Y());

// Off Diagonal Elements of the Mass Matrix
ixyxzyz.X() = -(integral[7] - mass * _inertiaOrigin.X() * _inertiaOrigin.Y());
ixyxzyz.Y() = -(integral[8] - mass * _inertiaOrigin.Y() * _inertiaOrigin.Z());
ixyxzyz.Z() = -(integral[9] - mass * _inertiaOrigin.X() * _inertiaOrigin.Z());

// Set the values in the MassMatrix object
_massMatrix.SetMass(mass);
_massMatrix.SetDiagonalMoments(ixxyyzz * _density);
_massMatrix.SetOffDiagonalMoments(ixyxzyz * _density);
}

//////////////////////////////////////////////////
std::optional<gz::math::Inertiald> MeshInertiaCalculator::operator()
(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;
_errors.push_back({sdf::ErrorCode::FATAL_ERROR,
"Could not calculate mesh inertia as mesh object is"
"std::nullopt"});
return std::nullopt;
}

auto fullPath = asFullPath(sdfMesh->Uri(), sdfMesh->FilePath());

if (fullPath.empty())
{
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;
}

// Load the Mesh
gz::common::MeshManager *meshManager = gz::common::MeshManager::Instance();
mesh = meshManager->Load(fullPath);
std::vector<Triangle> meshTriangles;
gz::math::MassMatrix3d meshMassMatrix;
gz::math::Pose3d centreOfMass;
gz::math::Pose3d inertiaOrigin;

// Create a list of Triangle objects from the mesh vertices and indices
this->GetMeshTriangles(meshTriangles, mesh);

// Calculate the mesh centroid and set is as centre of mass
this->CalculateMeshCentroid(centreOfMass, meshTriangles);

// Calculate mesh mass properties
this->CalculateMassProperties(meshTriangles, density,
meshMassMatrix, inertiaOrigin);

if (inertiaOrigin != centreOfMass)
{
// TODO(jasmeet0915): tranform the calculated inertia matrix
// from inertia origin to centre of mass
gzwarn << "Calculated centroid does not match the mesh origin. "
"Inertia Tensor values won't be correct. Use mesh with origin at "
"geometric center." << std::endl;
}

gz::math::Inertiald meshInertial;

if (!meshInertial.SetMassMatrix(meshMassMatrix))
{
return std::nullopt;
}
else
{
meshInertial.SetPose(centreOfMass);
return meshInertial;
}
}
Loading

0 comments on commit 52167a6

Please sign in to comment.