Skip to content

Commit

Permalink
Transforms between links correct, axes to be fixed
Browse files Browse the repository at this point in the history
  • Loading branch information
@Nicogene
Nicogene committed Jun 13, 2024
1 parent d369905 commit 1a467c3
Show file tree
Hide file tree
Showing 7 changed files with 135 additions and 85 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ include(GNUInstallDirs)
include(FeatureSummary)

find_package(Eigen3 REQUIRED)
find_package(iDynTree REQUIRED)
find_package(iDynTree 12.3.1 REQUIRED)
find_package(yaml-cpp REQUIRED)
find_package(LibXml2 REQUIRED)

Expand Down
2 changes: 1 addition & 1 deletion src/creo2urdf/include/creo2urdf/Creo2Urdf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,7 +124,7 @@ class Creo2Urdf : public pfcUICommandActionListener {
*/
bool loadYamlConfig(const std::string& filename);

bool processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner, iDynTree::Transform parent_H_csysItem = iDynTree::Transform::Identity());
bool processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner, iDynTree::Transform parentAsm_H_csysAsm = iDynTree::Transform::Identity());

/**
* @brief Get the renamed element from the configuration.
Expand Down
6 changes: 5 additions & 1 deletion src/creo2urdf/include/creo2urdf/ElementTreeManager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ class ElementTreeManager {
* @param[out] joint_info_map A map containing joint information.
* @return True if successful, false otherwise.
*/
bool populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map<std::string, JointInfo>& joint_info_map, bool is_asm=false);
bool populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map<std::string, JointInfo>& joint_info_map);

/**
* @brief Gets the constraint type between two assembled parts.
Expand Down Expand Up @@ -126,4 +126,8 @@ class ElementTreeManager {
* @return A pair representing the limits (min, max).
*/
std::pair<double, double> retrieveLimits(pfcFeature_ptr feat);

// SEEMS to work but we need to investigate further, using this may allow to refactor the code deeply

pfcTransform3D_ptr retrieveTransform(pfcFeature_ptr feat);
};
9 changes: 5 additions & 4 deletions src/creo2urdf/include/creo2urdf/Utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -258,7 +258,8 @@ struct JointInfo {
struct LinkInfo {
std::string name{""}; ///< Name of the link.
pfcModel_ptr modelhdl{nullptr}; ///< Pointer to the Creo model associated with the link.
iDynTree::Transform root_H_link{iDynTree::Transform::Identity()}; ///< 3D Transform from the root to the link's reference frame.
iDynTree::Transform rootAsm_H_linkFrame{iDynTree::Transform::Identity()}; ///< 3D Transform from the root to the link's reference frame.
iDynTree::Transform csysAsm_H_linkFrame{iDynTree::Transform::Identity()}; ///< 3D Transform from the assembly to the link's reference frame.
std::string link_frame_name{""}; ///< Name of the link frame.
};

Expand Down Expand Up @@ -400,7 +401,7 @@ void printRotationMatrix(pfcMatrix3D_ptr m);
void sanitizeSTL(std::string stl);

/**
* @brief Retrieves the transformation from the root to a specified link frame in the context of a component path.
* @brief Retrieves the transformation from the owner assembly to a specified link frame in the context of a component path.
*
* @param comp_path component path that represents the assembly hierarchy.
* @param modelhdl The part model in which the link frame is defined.
Expand All @@ -412,7 +413,7 @@ void sanitizeSTL(std::string stl);
* - The second element is an iDynTree::Transform representing the transformation from the root to the specified link frame.
* If the operation fails, this transformation will be an identity transformation.
*/
std::pair<bool, iDynTree::Transform> getTransformFromRootToChild(pfcComponentPath_ptr comp_path, pfcModel_ptr modelhdl, const std::string& link_frame_name, const array<double, 3>& scale);
std::pair<bool, iDynTree::Transform> getTransformFromOwnerToLinkFrame(pfcComponentPath_ptr comp_path, pfcModel_ptr modelhdl, const std::string& link_frame_name, const array<double, 3>& scale);

/**
* @brief Retrieves the transformation matrix representing the coordinate system of a specified link frame in the given part.
Expand All @@ -438,7 +439,7 @@ std::pair<bool, iDynTree::Transform> getTransformFromPart(pfcModel_ptr modelhdl,
* @param scale The scaling factor for the origin of the child frame
* @return std::pair<bool, iDynTree::Direction> Pair containing a success/failure flag, and the axis direction
*/
std::pair<bool, iDynTree::Direction> getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const std::string& link_frame_name, const array<double, 3>& scale);
std::tuple<bool, iDynTree::Direction, iDynTree::Transform> getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const std::string& link_frame_name, const array<double, 3>& scale);

/**
* @brief Extracts the folder path from a file path.
Expand Down
140 changes: 73 additions & 67 deletions src/creo2urdf/src/Creo2Urdf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
#include <Eigen/Core>


bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner, iDynTree::Transform parent_H_csysItem) {
bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner, iDynTree::Transform parentAsm_H_csysAsm) {

for (int i = 0; i < asmListItems->getarraysize(); i++)
{
Expand All @@ -33,38 +33,13 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod
if (!component_handle) {
return false;
}

auto type = component_handle->GetType();

ElementTreeManager element_tree_manager;

//printToMessageWindow("Processing " + string(component_handle->GetFullName()) + " Owner " + string(model_owner->GetFullName()));

auto type = component_handle->GetType();
if (type == pfcMDL_ASSEMBLY) {
auto sub_asm_component_list = component_handle->ListItems(pfcModelItemType::pfcITEM_FEATURE);
auto ok = element_tree_manager.populateJointInfoFromElementTree(asmItemAsFeat, joint_info_map);
if (!ok) {
// WE NEED TO FIX the failure, the paths are not cleared
//continue;
}
iDynTree::Transform parentAsmCsys_H_asmCsys = iDynTree::Transform::Identity();
bool ret{ false };
std::tie(ret, parentAsmCsys_H_asmCsys) = getTransformFromPart(component_handle, "ASM_CSYS", scale);
if(!ret) {
printToMessageWindow("Failed to get the transform from " + string(model_owner->GetFullName()) + " (the parent assembly) to " + string(component_handle->GetFullName()), c2uLogLevel::WARN);
//return false;
}
else {
//printToMessageWindow("Got the transform from " + string(model_owner->GetFullName()) + " (the parent assembly) to " + string(component_handle->GetFullName()), c2uLogLevel::INFO);
//printToMessageWindow(parentAsmCsys_H_asmCsys.toString());
}
ok = processAsmItems(sub_asm_component_list, component_handle, parentAsmCsys_H_asmCsys);
if (!ok) {
return false;
}
else {
continue;
}
}

xintsequence_ptr seq = xintsequence::create();
seq->append(asmItemAsFeat->GetId());

Expand All @@ -73,33 +48,50 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod

pfcComponentPath_ptr comp_path = pfcCreateComponentPath(pfcAssembly::cast(model_owner), seq);

iDynTree::Transform csysAsm_H_linkFrame = iDynTree::Transform::Identity();
iDynTree::Transform csysPart_H_link_frame = iDynTree::Transform::Identity();
iDynTree::Transform parentAsm_H_linkFrame = iDynTree::Transform::Identity();

std::string link_frame_name{ "" };
auto link_name = string(component_handle->GetFullName());
std::string urdf_link_name { "" };

if (type == pfcMDL_ASSEMBLY) {
link_frame_name = "ASM_CSYS";
}
else {
link_frame_name = "CSYS";
urdf_link_name = getRenameElementFromConfig(link_name);
for (const auto& lf : config["linkFrames"]) {
if (lf["linkName"].Scalar() != urdf_link_name)
{
continue;
}
link_frame_name = lf["frameName"].Scalar();
}

iDynTree::Transform csysAsm_H_link = iDynTree::Transform::Identity();
iDynTree::Transform csysPart_H_link = iDynTree::Transform::Identity();
iDynTree::Transform parent_H_link = iDynTree::Transform::Identity();
if (link_frame_name.empty()) {
printToMessageWindow(link_name + " misses the frame in the linkFrames section, CSYS will be used instead", c2uLogLevel::WARN);
link_frame_name = "CSYS";
}
}
std::tie(ret, csysAsm_H_linkFrame) = getTransformFromOwnerToLinkFrame(comp_path, component_handle, link_frame_name , scale);

std::string urdf_link_name = getRenameElementFromConfig(link_name);
parentAsm_H_linkFrame = parentAsm_H_csysAsm * csysAsm_H_linkFrame;

std::string link_frame_name = "";
if (type == pfcMDL_ASSEMBLY) {
auto sub_asm_component_list = component_handle->ListItems(pfcModelItemType::pfcITEM_FEATURE);

for (const auto& lf : config["linkFrames"]) {
if (lf["linkName"].Scalar() != urdf_link_name)
{
bool ok = processAsmItems(sub_asm_component_list, component_handle, parentAsm_H_linkFrame);
if (!ok) {
return false;
}
else {
continue;
}
link_frame_name = lf["frameName"].Scalar();
}

if (link_frame_name.empty()) {
printToMessageWindow(link_name + " misses the frame in the linkFrames section, CSYS will be used instead", c2uLogLevel::WARN);
link_frame_name = "CSYS";
}
std::tie(ret, csysAsm_H_link) = getTransformFromRootToChild(comp_path, component_handle, link_frame_name, scale);

parent_H_link = parent_H_csysItem * csysAsm_H_link;

//printToMessageWindow(csysAsm_H_link.toString());
//printToMessageWindow(csysAsm_H_linkFrame.toString());

if (!ret && warningsAreFatal)
{
Expand All @@ -108,14 +100,14 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod

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

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

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

if (!link.getInertia().isPhysicallyConsistent())
{
Expand All @@ -125,7 +117,7 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod
}
}

LinkInfo l_info{ urdf_link_name, component_handle, parent_H_link, link_frame_name };
LinkInfo l_info{ urdf_link_name, component_handle, parentAsm_H_linkFrame, csysAsm_H_linkFrame, link_frame_name };
link_info_map.insert(std::make_pair(link_name, l_info));
populateExportedFrameInfoMap(component_handle);

Expand All @@ -141,6 +133,14 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod

void Creo2Urdf::OnCommand() {

// Let's clear the map in case of multiple click
if (joint_info_map.size() > 0) {
joint_info_map.clear();
link_info_map.clear();
exported_frame_info_map.clear();
assigned_inertias_map.clear();
assigned_collision_geometry_map.clear();
}
m_session_ptr = pfcGetProESession();
if (!m_session_ptr) {
printToMessageWindow("Failed to get the session", c2uLogLevel::WARN);
Expand Down Expand Up @@ -200,15 +200,6 @@ void Creo2Urdf::OnCommand() {
return;
}

// Let's clear the map in case of multiple click
if (joint_info_map.size() > 0) {
joint_info_map.clear();
link_info_map.clear();
exported_frame_info_map.clear();
assigned_inertias_map.clear();
assigned_collision_geometry_map.clear();
}

if (config["warningsAreFatal"].IsDefined()) {
warningsAreFatal = config["warningsAreFatal"].as<bool>();
}
Expand Down Expand Up @@ -260,22 +251,23 @@ void Creo2Urdf::OnCommand() {
}


printToMessageWindow("Adding joint " + joint_name);
auto root_H_parent_link = link_info_map.at(parent_link_name).root_H_link;
auto root_H_child_link = link_info_map.at(child_link_name).root_H_link;

auto asm_owner_H_parent_link = link_info_map.at(parent_link_name).rootAsm_H_linkFrame;
auto asm_owner_H_child_link = link_info_map.at(child_link_name).rootAsm_H_linkFrame;
auto child_model = link_info_map.at(child_link_name).modelhdl;
auto parent_model = link_info_map.at(parent_link_name).modelhdl;
auto parent_link_frame = link_info_map.at(parent_link_name).link_frame_name;

//printToMessageWindow("Parent link H " + root_H_parent_link.toString());
//printToMessageWindow("Child link H " + root_H_child_link.toString());
//printToMessageWindow("Parent link H " + asm_owner_H_parent_link.toString());
//printToMessageWindow("Child link H " + asm_owner_H_child_link.toString());
iDynTree::Transform parent_H_child = iDynTree::Transform::Identity();
parent_H_child = root_H_parent_link.inverse() * root_H_child_link;
parent_H_child = asm_owner_H_parent_link.inverse() * asm_owner_H_child_link;

if (joint_info.second.type == JointType::Revolute || joint_info.second.type == JointType::Linear) {

iDynTree::Direction axis;
std::tie(ret, axis) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale);
iDynTree::Transform oldChild_H_newChild;
std::tie(ret, axis, oldChild_H_newChild) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale);

if (!ret && warningsAreFatal)
{
Expand All @@ -288,6 +280,20 @@ void Creo2Urdf::OnCommand() {
axis = axis.reverse();
}

auto urdf_child_link_name = getRenameElementFromConfig(child_link_name);


// FIXME
// It does something but the transform is somehow wrong, in case of proper definition of the csys oldChild_H_newChild is the identity
// Once we found the proper transform we should updatea also the spatial inertia

parent_H_child = parent_H_child * oldChild_H_newChild;
auto link_H_collision_solid_shape = idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->getLink_H_geometry();
auto link_H_visual_solid_shape = idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->getLink_H_geometry();

idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->setLink_H_geometry(oldChild_H_newChild.inverse() * link_H_collision_solid_shape);
idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->setLink_H_geometry(oldChild_H_newChild.inverse() * link_H_visual_solid_shape);

std::shared_ptr<iDynTree::IJoint> joint_sh_ptr;
if (joint_info.second.type == JointType::Revolute) {
joint_sh_ptr = std::make_shared<iDynTree::RevoluteJoint>();
Expand Down Expand Up @@ -480,7 +486,7 @@ iDynTree::SpatialInertia Creo2Urdf::computeSpatialInertiafromCreo(pfcMassPropert

iDynTree::Position com_child({ com->get(0) * scale[0] , com->get(1) * scale[1], com->get(2) * scale[2] });

// Account for csysPart_H_link transformation
// Account for csysPart_H_link_frame 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
Expand Down
34 changes: 28 additions & 6 deletions src/creo2urdf/src/ElementTreeManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,16 +24,13 @@ ElementTreeManager::ElementTreeManager(pfcFeature_ptr feat, std::map<std::string

ElementTreeManager::~ElementTreeManager() {}

bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map<std::string, JointInfo>& joint_info_map, bool is_asm)
bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map<std::string, JointInfo>& joint_info_map)
{
wfeat = wfcWFeature::cast(feat);

try
{
tree = wfeat->GetElementTree(nullptr, wfcFEAT_EXTRACT_NO_OPTS);
std::string joint_name = std::to_string(feat->GetId()) + ".xml";
//printToMessageWindow("Element tree extracted for feature " + std::to_string(feat->GetId()), c2uLogLevel::INFO);
tree->WriteElementTreeToFile(wfcELEMTREE_XML, joint_name.c_str());
}
xcatchbegin
xcatchcip(pfcXToolkitInvalidType)
Expand All @@ -49,11 +46,9 @@ bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, s
printToMessageWindow("Could not retrieve solid references!", c2uLogLevel::WARN);
return false;
}

joint.child_link_name = getChildName();
joint.parent_link_name = getParentName();
std::string joint_name = joint.parent_link_name + "--" + joint.child_link_name;
printToMessageWindow("JOINT !! " + joint_name, c2uLogLevel::INFO);
joint.type = proAsmCompSetType_to_JointType.at(static_cast<ProAsmcompSetType>(getConstraintType()));

if (joint.type == JointType::Revolute)
Expand Down Expand Up @@ -235,6 +230,33 @@ std::string ElementTreeManager::retrievePartName()
xcatchend
}


pfcTransform3D_ptr ElementTreeManager::retrieveTransform(pfcFeature_ptr feat) {
pfcTransform3D_ptr parentCsys_H_childCsys = nullptr;

wfcElemPathItems_ptr elemItems = wfcElemPathItems::create();
wfcElemPathItem_ptr Item;
wfcElement_ptr element;

Item = wfcElemPathItem::Create(wfcELEM_PATH_ITEM_TYPE_ID, wfcPRO_E_COMPONENT_INIT_POS);
elemItems->append(Item);

wfcElementPath_ptr transform_path = wfcElementPath::Create(elemItems);
element = tree->GetElement(transform_path);

auto value_ptr = element->GetValue();
if(!value_ptr)
printToMessageWindow("wfcPRO_E_COMPONENT_INIT_POS value is null");
else {
parentCsys_H_childCsys = value_ptr->GetTransformValue();
// Because the transform is from child to parent, we need to invert it
parentCsys_H_childCsys->Invert();
}

return parentCsys_H_childCsys;
}


std::pair<double, double> ElementTreeManager::retrieveLimits(pfcFeature_ptr feat)
{
wfcElemPathItems_ptr elemItems = wfcElemPathItems::create();
Expand Down
Loading

0 comments on commit 1a467c3

Please sign in to comment.