diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp index 6a8561a..b08a1ab 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/src/creo2urdf/src/Creo2Urdf.cpp @@ -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); @@ -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()) { @@ -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]); } } } @@ -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 }; diff --git a/src/creo2urdf/src/Utils.cpp b/src/creo2urdf/src/Utils.cpp index dfdeab1..3858e80 100644 --- a/src/creo2urdf/src/Utils.cpp +++ b/src/creo2urdf/src/Utils.cpp @@ -103,23 +103,23 @@ void sanitizeSTL(std::string stl) std::pair getTransformFromRootToChild(pfcComponentPath_ptr comp_path, pfcModel_ptr modelhdl, const std::string& link_frame_name, const array& 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); }