Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Account for part/link rotation when saving inertia matrix #83

Merged
merged 5 commits into from
Mar 14, 2024
Merged
Show file tree
Hide file tree
Changes from 3 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,6 @@ project(creo2urdf LANGUAGES C CXX
include(GNUInstallDirs)
include(FeatureSummary)

# Needed for https://github.com/robotology/idyntree/issues/1065
find_package(Eigen3 REQUIRED)
find_package(iDynTree REQUIRED)
find_package(yaml-cpp REQUIRED)
Expand Down
1 change: 1 addition & 0 deletions src/creo2urdf/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,7 @@ target_link_libraries(creo2urdf PRIVATE iDynTree::idyntree-modelio
iDynTree::idyntree-high-level
yaml-cpp
LibXml2::LibXml2
Eigen3::Eigen
protk_dllmd_NU
otk_cpp_md
otk_no222_md
Expand Down
57 changes: 46 additions & 11 deletions src/creo2urdf/src/Creo2Urdf.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,11 @@
#include <creo2urdf/Creo2Urdf.h>
#include <creo2urdf/Utils.h>
#include <pfcExceptions.h>

#include <iDynTree/PrismaticJoint.h>
#include <iDynTree/EigenHelpers.h>

#include <Eigen/Core>

void Creo2Urdf::OnCommand() {

Expand Down Expand Up @@ -120,6 +124,7 @@ void Creo2Urdf::OnCommand() {
auto link_name = string(component_handle->GetFullName());

iDynTree::Transform root_H_link = iDynTree::Transform::Identity();
iDynTree::Transform csys_H_link = iDynTree::Transform::Identity();

std::string urdf_link_name = getRenameElementFromConfig(link_name);

Expand All @@ -145,9 +150,15 @@ void Creo2Urdf::OnCommand() {
}

auto mass_prop = pfcSolid::cast(component_handle)->GetMassProperty();

std::tie(ret, csys_H_link) = getTransformFromPart(component_handle, link_frame_name, scale);
if (!ret && warningsAreFatal)
{
return;
}

iDynTree::Link link;
link.setInertia(computeSpatialInertiafromCreo(mass_prop, root_H_link, urdf_link_name));
link.setInertia(computeSpatialInertiafromCreo(mass_prop, csys_H_link, urdf_link_name));

if (!link.getInertia().isPhysicallyConsistent())
{
Expand Down Expand Up @@ -375,31 +386,55 @@ bool Creo2Urdf::exportModelToUrdf(iDynTree::Model mdl, iDynTree::ModelExporterOp
iDynTree::SpatialInertia Creo2Urdf::computeSpatialInertiafromCreo(pfcMassProperty_ptr mass_prop, iDynTree::Transform H, const std::string& link_name) {
auto com = mass_prop->GetGravityCenter();
auto inertia_tensor = mass_prop->GetCenterGravityInertiaTensor();
iDynTree::RotationalInertiaRaw idyn_inertia_tensor = iDynTree::RotationalInertiaRaw::Zero();

iDynTree::RotationalInertiaRaw idyn_inertia_tensor_csys_orientation = iDynTree::RotationalInertiaRaw::Zero();
iDynTree::RotationalInertiaRaw idyn_inertia_tensor_link_orientation = iDynTree::RotationalInertiaRaw::Zero();

bool assigned_inertia_flag = assigned_inertias_map.find(link_name) != assigned_inertias_map.end();
for (int i_row = 0; i_row < idyn_inertia_tensor.rows(); i_row++) {
for (int j_col = 0; j_col < idyn_inertia_tensor.cols(); j_col++) {
if ((assigned_inertia_flag) && (i_row == j_col))
{
idyn_inertia_tensor.setVal(i_row, j_col, assigned_inertias_map.at(link_name)[i_row]);
for (int i_row = 0; i_row < idyn_inertia_tensor_csys_orientation.rows(); i_row++) {
for (int j_col = 0; j_col < idyn_inertia_tensor_csys_orientation.cols(); j_col++) {
if ((assigned_inertia_flag) && (i_row == j_col)) {
// The assigned inertia is already expressed in the link frame
idyn_inertia_tensor_link_orientation.setVal(i_row, j_col, assigned_inertias_map.at(link_name)[i_row]);
}
else {
idyn_inertia_tensor.setVal(i_row, j_col, inertia_tensor->get(i_row, j_col) * scale[i_row] * scale[j_col]);
idyn_inertia_tensor_csys_orientation.setVal(i_row, j_col, inertia_tensor->get(i_row, j_col) * scale[i_row] * scale[j_col]);
}
}
}

iDynTree::Position com_child({ com->get(0) * scale[0] , com->get(1) * scale[1], com->get(2) * scale[2] });
com_child = H.inverse() * com_child; // TODO verify

// Account for root_H_link transformation
// See https://github.com/icub-tech-iit/ergocub-software/issues/224#issuecomment-1985692598 for full contents

// The COM returned by Creo's GetGravityCenter seems to be expressed in the root frame, so we need
// to transform it back to the link frame before passing it to iDynTree's fromRotationalInertiaWrtCenterOfMass
com_child = H.inverse() * com_child;

// The inertia returned by Creo's GetCenterGravityInertiaTensor seems to be expressed with the COM as the
// point in which it is expressed, and with the orientation of the root link, so we rotate it back with
// the orientation of the link frame, unless an assignedInertia is used
Nicogene marked this conversation as resolved.
Show resolved Hide resolved
if (!assigned_inertia_flag) {
// Note, this auto-defined methods are Eigen::Map, so they are reference to data that remains
// stored in the original iDynTree object, see https://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
auto inertia_tensor_root = iDynTree::toEigen(idyn_inertia_tensor_csys_orientation);
auto inertia_tensor_link = iDynTree::toEigen(idyn_inertia_tensor_link_orientation);
auto csys_R_link = iDynTree::toEigen(H.getRotation());

// See Equation 15 of https://ocw.mit.edu/courses/16-07-dynamics-fall-2009/dd277ec654440f4c2b5b07d6c286c3fd_MIT16_07F09_Lec26.pdf
inertia_tensor_link = csys_R_link.transpose()*inertia_tensor_root*csys_R_link;
}

double mass{ 0.0 };
if (config["assignedMasses"][link_name].IsDefined()) {
mass = config["assignedMasses"][link_name].as<double>();
}
else {
mass = mass_prop->GetMass();
}
iDynTree::SpatialInertia sp_inertia(mass, com_child, idyn_inertia_tensor);
sp_inertia.fromRotationalInertiaWrtCenterOfMass(mass, com_child, idyn_inertia_tensor);
iDynTree::SpatialInertia sp_inertia(mass, com_child, idyn_inertia_tensor_link_orientation);
sp_inertia.fromRotationalInertiaWrtCenterOfMass(mass, com_child, idyn_inertia_tensor_link_orientation);

return sp_inertia;
}
Expand Down