Skip to content

Commit

Permalink
Cleanup of some var names
Browse files Browse the repository at this point in the history
  • Loading branch information
Nicogene committed Mar 14, 2024
1 parent 695faf1 commit 7783834
Show file tree
Hide file tree
Showing 2 changed files with 17 additions and 17 deletions.
20 changes: 10 additions & 10 deletions src/creo2urdf/src/Creo2Urdf.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -124,7 +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();
iDynTree::Transform csysPart_H_link = iDynTree::Transform::Identity();

std::string urdf_link_name = getRenameElementFromConfig(link_name);

Expand All @@ -151,14 +151,14 @@ void Creo2Urdf::OnCommand() {

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

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

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

if (!link.getInertia().isPhysicallyConsistent())
{
Expand Down Expand Up @@ -387,18 +387,18 @@ iDynTree::SpatialInertia Creo2Urdf::computeSpatialInertiafromCreo(pfcMassPropert
auto com = mass_prop->GetGravityCenter();
auto inertia_tensor = mass_prop->GetCenterGravityInertiaTensor();

iDynTree::RotationalInertiaRaw idyn_inertia_tensor_csys_orientation = iDynTree::RotationalInertiaRaw::Zero();
iDynTree::RotationalInertiaRaw idyn_inertia_tensor_csysPart_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_csys_orientation.rows(); i_row++) {
for (int j_col = 0; j_col < idyn_inertia_tensor_csys_orientation.cols(); j_col++) {
for (int i_row = 0; i_row < idyn_inertia_tensor_csysPart_orientation.rows(); i_row++) {
for (int j_col = 0; j_col < idyn_inertia_tensor_csysPart_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_csys_orientation.setVal(i_row, j_col, inertia_tensor->get(i_row, j_col) * scale[i_row] * scale[j_col]);
idyn_inertia_tensor_csysPart_orientation.setVal(i_row, j_col, inertia_tensor->get(i_row, j_col) * scale[i_row] * scale[j_col]);
}
}
}
Expand All @@ -418,12 +418,12 @@ iDynTree::SpatialInertia Creo2Urdf::computeSpatialInertiafromCreo(pfcMassPropert
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_root = iDynTree::toEigen(idyn_inertia_tensor_csysPart_orientation);
auto inertia_tensor_link = iDynTree::toEigen(idyn_inertia_tensor_link_orientation);
auto csys_R_link = iDynTree::toEigen(H.getRotation());
auto csysPart_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;
inertia_tensor_link = csysPart_R_link.transpose()*inertia_tensor_root*csysPart_R_link;
}

double mass{ 0.0 };
Expand Down
14 changes: 7 additions & 7 deletions src/creo2urdf/src/Utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -103,23 +103,23 @@ void sanitizeSTL(std::string stl)

std::pair<bool, iDynTree::Transform> getTransformFromRootToChild(pfcComponentPath_ptr comp_path, pfcModel_ptr modelhdl, const std::string& link_frame_name, const array<double, 3>& scale) {

iDynTree::Transform H_child = iDynTree::Transform::Identity();
iDynTree::Transform csysAsm_H_link = iDynTree::Transform::Identity();

auto asm_csys_H_csys = fromCreo(comp_path->GetTransform(xtrue), scale);
iDynTree::Transform csys_H_child;
auto csysAsm_H_csysPart = fromCreo(comp_path->GetTransform(xtrue), scale);
iDynTree::Transform csysPart_H_link;

bool ret = false;
std::tie(ret, csys_H_child) = getTransformFromPart(modelhdl, link_frame_name, scale);
std::tie(ret, csysPart_H_link) = getTransformFromPart(modelhdl, link_frame_name, scale);

if (!ret)
{
printToMessageWindow("Unable to get the transform from to the root for " + string(modelhdl->GetFullName()), c2uLogLevel::WARN);
return make_pair(false, H_child);
return make_pair(false, csysAsm_H_link);
}

H_child = asm_csys_H_csys * csys_H_child;
csysAsm_H_link = csysAsm_H_csysPart * csysPart_H_link;

return make_pair(true, H_child);
return make_pair(true, csysAsm_H_link);

}

Expand Down

0 comments on commit 7783834

Please sign in to comment.