Skip to content

Commit

Permalink
Completed codecheck
Browse files Browse the repository at this point in the history 
Signed-off-by: Jasmeet Singh <[email protected]>
  • Loading branch information
@jasmeet0915
jasmeet0915 committed Aug 21, 2023
1 parent c79a397 commit 3fc1587
Show file tree
Hide file tree
Showing 3 changed files with 86 additions and 58 deletions.
47 changes: 27 additions & 20 deletions include/gz/sim/MeshInertiaCalculator.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#define GZ_SIM_MESHINERTIACALCULATOR_HH_

#include <optional>
#include <vector>

#include <sdf/sdf.hh>
#include <sdf/CustomInertiaCalcProperties.hh>
Expand Down Expand Up @@ -52,12 +53,13 @@ namespace gz
};

/// \class MeshInertiaCalculator gz/sim/MeshInertiaCalculator.hh
/// \brief Inertial Properties (Mass, Centre of Mass and Moments of Inertia)
/// calculator for 3D meshes.
/// \brief Inertial Properties (Mass, Centre of Mass & Moments of
/// Inertia) calculator for 3D meshes.
///
/// This class overloads the () operator, therefore, an instance of this class
/// registered with libsdformat as a Custom Inertia Calculator. This would used
/// to calculate the inertial properties of 3D mesh using SDFormat.
/// This class overloads the () operator, therefore, an instance
/// of this class registered with libsdformat as a Custom
/// Inertia Calculator. This would used to calculate the
/// inertial properties of 3D mesh using SDFormat.
///
/// The calculation method used in this class is described here:
/// https://www.geometrictools.com/Documentation/PolyhedralMassProperties.pdf
Expand All @@ -72,37 +74,42 @@ namespace gz

/// \brief Function to get the vertices & indices of the given mesh
/// & convert them into instances of the Triangle struct
/// Each triangle represents a triangle in the mesh & is added to a vector
/// \param[out] _triangles A vector to hold all the Triangle instances obtained
/// Each triangle represents a triangle in the mesh & is added
/// to a vector
/// \param[out] _triangles A vector to hold all the Triangle
/// instances obtained
/// from the vertices & indices of the mesh
/// \param[in] _mesh Mesh object
public: void GetMeshTriangles(std::vector<Triangle>& _triangles,
const gz::common::Mesh* _mesh);
public: void GetMeshTriangles(
std::vector<Triangle>& _triangles,
const gz::common::Mesh* _mesh);

/// \brief Function that calculates the mass, mass matrix & centre of
/// \brief Function that calculates the mass, mass matrix & centre of
/// mass of a mesh using a vector of Triangles of the mesh
/// \param[in] _triangles A vector of all the Triangles of the mesh
/// \param[in] _density Density of the mesh
/// \param[out] _massMatrix MassMatrix object to hold mass &
/// moment of inertia of the mesh
/// \param[out] _centreOfMass Pose3d object to hold the centre of
/// \param[out] _centreOfMass Pose3d object to hold the centre of
/// mass of the object
public: void CalculateMassProperties(
std::vector<Triangle>& _triangles,
std::vector<Triangle>& _triangles,
double _density,
gz::math::MassMatrix3d& _massMatrix,
gz::math::Pose3d& _centreOfMass);

/// \brief Overloaded () operator which allows an instance of this class
/// to be registered as a Custom Inertia Calculator with libsdformat
/// \brief Overloaded () operator which allows an instance
/// of this class to be registered as a Custom Inertia
/// Calculator with libsdformat
/// \param[in] _errors A vector of Errors object. Each object
/// would contain an error code and an error message.
/// \param _calculatorParams An instance of CustomInertiaCalcProperties.
/// This instance can be used to access the data like density, properties
/// of the mesh, etc.
public: std::optional<gz::math::Inertiald> operator()
(sdf::Errors& _errors,
const sdf::CustomInertiaCalcProperties& _calculatorParams);
/// \param _calculatorParams An instance of
/// CustomInertiaCalcProperties. This instance can be used
/// to access the data like density,
/// properties of the mesh, etc.
public: std::optional<gz::math::Inertiald> operator()(
sdf::Errors& _errors,
const sdf::CustomInertiaCalcProperties& _calculatorParams);
};
}
}
Expand Down
81 changes: 49 additions & 32 deletions src/MeshInertiaCalculator.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
*/

#include <optional>
#include <vector>

#include <sdf/sdf.hh>
#include <sdf/CustomInertiaCalcProperties.hh>
Expand Down Expand Up @@ -50,7 +51,7 @@ void MeshInertiaCalculator::GetMeshTriangles(
double* vertArray = NULL;
int* indArray = NULL;
_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)
{
Expand All @@ -75,24 +76,32 @@ void MeshInertiaCalculator::CalculateMassProperties(
gz::math::MassMatrix3d& _massMatrix,
gz::math::Pose3d& _centreOfMass)
{
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. Would probably add this as a data member of the MeshAdapterClass
// 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
int numTriangles = _triangles.size();
// Calculate cross products of triangles

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

for (int i = 0; i < numTriangles; ++i) {
crosses[i] =
(_triangles[i].v1 - _triangles[i].v0).Cross(_triangles[i].v2 - _triangles[i].v0);

// Caculating cross products of 2 vectors emerging from a common vertex
// This basically gives a vector normal to the plane of triangle
for (int 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 (int i = 0; i < numTriangles; ++i) {
std::vector<gz::math::Vector3d> f1(numTriangles), f2(numTriangles),
f3(numTriangles), g0(numTriangles), g1(numTriangles), g2(numTriangles);

for (int 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 +
Expand All @@ -110,7 +119,8 @@ void MeshInertiaCalculator::CalculateMassProperties(

// Calculate integral terms
std::vector<double> integral(10);
for (int i = 0; i < numTriangles; ++i) {
for (int i = 0; i < numTriangles; ++i)
{
double x0 = _triangles[i].v0.X();
double y0 = _triangles[i].v0.Y();
double z0 = _triangles[i].v0.Z();
Expand All @@ -127,15 +137,19 @@ void MeshInertiaCalculator::CalculateMassProperties(
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());
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) {
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;
Expand All @@ -144,24 +158,27 @@ void MeshInertiaCalculator::CalculateMassProperties(
_centreOfMass.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 * (_centreOfMass.Y() * _centreOfMass.Y() + _centreOfMass.Z() * _centreOfMass.Z()));
ixxyyzz.Y() = (integral[4] + integral[6] - mass * (_centreOfMass.Z() * _centreOfMass.Z() + _centreOfMass.X() * _centreOfMass.X()));
ixxyyzz.Z() = integral[4] + integral[5] - mass * (_centreOfMass.X() * _centreOfMass.X() + _centreOfMass.Y() * _centreOfMass.Y());

ixxyyzz.X() = (integral[5] + integral[6] - mass *
(_centreOfMass.Y() * _centreOfMass.Y() +
_centreOfMass.Z() * _centreOfMass.Z()));
ixxyyzz.Y() = (integral[4] + integral[6] - mass *
(_centreOfMass.Z() * _centreOfMass.Z() +
_centreOfMass.X() * _centreOfMass.X()));
ixxyyzz.Z() = integral[4] + integral[5] - mass *
(_centreOfMass.X() * _centreOfMass.X() +
_centreOfMass.Y() * _centreOfMass.Y());

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

// Set the values in the MassMatrix object
_massMatrix.SetMass(mass);
_massMatrix.SetDiagonalMoments(ixxyyzz * _density);
_massMatrix.SetOffDiagonalMoments(ixyxzyz * _density);
std::cout << "Mass of the mesh is: " << _massMatrix.Mass() << std::endl;
std::cout << "Diagonal Elements of the Inertia Matrix are: " << _massMatrix.DiagonalMoments() << std::endl;
std::cout << "Off Diagonal Elements of the Inertia Matrix are: " << _massMatrix.OffDiagonalMoments() << std::endl;
}

//////////////////////////////////////////////////
Expand Down Expand Up @@ -203,4 +220,4 @@ std::optional<gz::math::Inertiald> MeshInertiaCalculator::operator()
meshInertial.SetPose(centreOfMass);
return meshInertial;
}
}
}
16 changes: 10 additions & 6 deletions src/Server.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,9 +97,11 @@ Server::Server(const ServerConfig &_config)
}
gzmsg << msg;
sdf::ParserConfig sdfParserConfig;
errors = this->dataPtr->sdfRoot.LoadSdfString(_config.SdfString(), sdfParserConfig);
sdf::Errors inertialErr = this->dataPtr->sdfRoot.CalculateInertials(sdfParserConfig);
errors.insert(errors.end(), inertialErr.begin(), inertialErr.end());
errors = this->dataPtr->sdfRoot.LoadSdfString(
_config.SdfString(), sdfParserConfig);
sdf::Errors inertialErr =
this->dataPtr->sdfRoot.CalculateInertials(sdfParserConfig);
errors.insert(errors.end(), inertialErr.begin(), inertialErr.end());
break;
}

Expand Down Expand Up @@ -136,7 +138,8 @@ Server::Server(const ServerConfig &_config)
{
// If the specified file only contains a model, load the default
// world and add the model to it.
errors = this->dataPtr->sdfRoot.LoadSdfString(DefaultWorld::World(), sdfParserConfig);
errors = this->dataPtr->sdfRoot.LoadSdfString(
DefaultWorld::World(), sdfParserConfig);
sdf::World *world = this->dataPtr->sdfRoot.WorldByIndex(0);
if (world == nullptr) {
return;
Expand All @@ -147,8 +150,9 @@ Server::Server(const ServerConfig &_config)
}
}
}

sdf::Errors inertialErr = this->dataPtr->sdfRoot.CalculateInertials(sdfParserConfig);

sdf::Errors inertialErr =
this->dataPtr->sdfRoot.CalculateInertials(sdfParserConfig);
errors.insert(errors.end(), inertialErr.begin(), inertialErr.end());
break;
}
Expand Down

0 comments on commit 3fc1587

Please sign in to comment.