Allow setting decimals of numerical values when exporting the URDF

[flferretti]

With this PR, it will be possible to set a number of decimals when exporting the URDF. E.g.:

urdfNumericalPrecision: 10

I have no way to test this in Creo since I'm on Linux. I did a rough test copying the functions to a test file and compiling it:

test_precision.cpp

#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <string>
#include <vector>
#include <cmath>
#include <libxml/parser.h>
#include <libxml/tree.h>
#include <libxml/xpath.h>

// From Utils.h
constexpr double epsilon = 1e-12;

// Copied from Utils.cpp
std::string formatNumericalString(const std::string& input, int precision)
{
    std::istringstream iss(input);
    std::ostringstream oss;
    oss << std::fixed << std::setprecision(precision);

    double value;
    bool first = true;

    while (iss >> value)
    {
        if (!first)
        {
            oss << " ";
        }
        // Round very small numbers to zero
        if (std::abs(value) < epsilon)
        {
            value = 0.0;
        }
        oss << value;
        first = false;
    }

    return oss.str();
}

// Copied from Utils.cpp
bool postProcessUrdfPrecision(const std::string& urdf_path, int precision)
{
    // Load XML document
    xmlDocPtr doc = xmlReadFile(urdf_path.c_str(), NULL, 0);
    if (doc == NULL)
    {
        std::cerr << "Failed to parse URDF file: " << urdf_path << std::endl;
        return false;
    }

    // Create XPath context
    xmlXPathContextPtr xpathCtx = xmlXPathNewContext(doc);
    if (xpathCtx == NULL)
    {
        xmlFreeDoc(doc);
        return false;
    }

    // List of XPath expressions for numerical attributes to reformat
    std::vector<std::string> xpath_expressions = {
        // Link inertial properties
        "//robot/link/inertial/origin/@xyz",
        "//robot/link/inertial/origin/@rpy",
        "//robot/link/inertial/mass/@value",
        "//robot/link/inertial/inertia/@ixx",
        "//robot/link/inertial/inertia/@ixy",
        "//robot/link/inertial/inertia/@ixz",
        "//robot/link/inertial/inertia/@iyy",
        "//robot/link/inertial/inertia/@iyz",
        "//robot/link/inertial/inertia/@izz",

        // Joint properties
        "//robot/joint/origin/@xyz",
        "//robot/joint/origin/@rpy",
        "//robot/joint/axis/@xyz",
        "//robot/joint/limit/@lower",
        "//robot/joint/limit/@upper",
        "//robot/joint/limit/@effort",
        "//robot/joint/limit/@velocity",
        "//robot/joint/dynamics/@damping",
        "//robot/joint/dynamics/@friction",

        // Visual geometry
        "//robot/link/visual/origin/@xyz",
        "//robot/link/visual/origin/@rpy",
        "//robot/link/visual/geometry/box/@size",
        "//robot/link/visual/geometry/cylinder/@radius",
        "//robot/link/visual/geometry/cylinder/@length",
        "//robot/link/visual/geometry/sphere/@radius",
        "//robot/link/visual/geometry/mesh/@scale",

        // Collision geometry
        "//robot/link/collision/origin/@xyz",
        "//robot/link/collision/origin/@rpy",
        "//robot/link/collision/geometry/box/@size",
        "//robot/link/collision/geometry/cylinder/@radius",
        "//robot/link/collision/geometry/cylinder/@length",
        "//robot/link/collision/geometry/sphere/@radius",
        "//robot/link/collision/geometry/mesh/@scale"
    };

    // Process each XPath expression
    for (const auto& xpath : xpath_expressions)
    {
        xmlXPathObjectPtr xpathObj = xmlXPathEvalExpression(
            BAD_CAST xpath.c_str(), xpathCtx);

        if (xpathObj != NULL && xpathObj->nodesetval != NULL)
        {
            for (int i = 0; i < xpathObj->nodesetval->nodeNr; i++)
            {
                xmlNodePtr node = xpathObj->nodesetval->nodeTab[i];
                xmlChar* old_value = xmlNodeGetContent(node);

                if (old_value != NULL)
                {
                    std::string old_str((char*)old_value);
                    std::string new_str = formatNumericalString(old_str, precision);
                    xmlNodeSetContent(node, BAD_CAST new_str.c_str());
                    xmlFree(old_value);
                }
            }
        }

        if (xpathObj != NULL)
        {
            xmlXPathFreeObject(xpathObj);
        }
    }

    // Save the URDF
    int result = xmlSaveFormatFileEnc(urdf_path.c_str(), doc, "UTF-8", 1);

    // Cleanup
    xmlXPathFreeContext(xpathCtx);
    xmlFreeDoc(doc);
    xmlCleanupParser();

    return (result != -1);
}

// Sample URDF for testing
void createSampleUrdf(const std::string& filename)
{
    std::ofstream urdf(filename);
    urdf << R"(<?xml version="1.0"?>
<robot name="test_robot">
  <link name="base_link">
    <inertial>
      <origin xyz="0.123456789012345 0.234567890123456 0.345678901234567" rpy="0.111111111111111 0.222222222222222 0.333333333333333"/>
      <mass value="1.234567890123456"/>
      <inertia ixx="0.001234567890123" ixy="0.000123456789012" ixz="0.000012345678901"
               iyy="0.002345678901234" iyz="0.000234567890123" izz="0.003456789012345"/>
    </inertial>
    <visual>
      <origin xyz="0.111111111111111 0.222222222222222 0.333333333333333" rpy="0.0 0.0 0.0"/>
      <geometry>
        <box size="0.123456789012345 0.234567890123456 0.345678901234567"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
      <geometry>
        <cylinder radius="0.123456789012345" length="0.987654321098765"/>
      </geometry>
    </collision>
  </link>

  <link name="link1">
    <inertial>
      <origin xyz="0.0 0.0 0.5" rpy="0.0 0.0 0.0"/>
      <mass value="2.5"/>
      <inertia ixx="0.008333333333333" ixy="0.0" ixz="0.0"
               iyy="0.008333333333333" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>

  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0.123456789012345 1e-15 0.345678901234567" rpy="0.0 0.0 0.0"/>
    <axis xyz="1e-13 0.0 1.0"/>
    <limit lower="-1.570796326794897" upper="1.570796326794897" effort="10.123456789012345" velocity="2.345678901234567"/>
    <dynamics damping="1e-14" friction="0.234567890123456"/>
  </joint>
</robot>
)";
    urdf.close();
}

int main(int argc, char* argv[])
{
    // Test with different precisions
    std::vector<int> precisions = {3, 6, 10};

    for (int precision : precisions)
    {
        std::string test_file = "test_urdf_precision_" + std::to_string(precision) + ".urdf";

        std::cout << "\nPrecision: " << precision << std::endl;
        std::cout << "Output file: " << test_file << std::endl;

        // Create sample URDF
        createSampleUrdf(test_file);

        bool success = postProcessUrdfPrecision(test_file, precision);

        if (success)
        {
            std::cout << "PASSED" << std::endl;

            std::ifstream urdf(test_file);
            std::string line;
            bool show_lines = false;
            int line_count = 0;

            std::cout << "\nfirst inertial section: " << std::endl;
            while (std::getline(urdf, line) && line_count < 10)
            {
                if (line.find("<inertial>") != std::string::npos)
                {
                    show_lines = true;
                }

                if (show_lines)
                {
                    std::cout << "  " << line << std::endl;
                    line_count++;

                    if (line.find("</inertial>") != std::string::npos)
                    {
                        break;
                    }
                }
            }
            urdf.close();
        }
        else
        {
            std::cerr << "FAILED" << std::endl;
            return 1;
        }
    }

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.16)
project(test_precision)

find_package(LibXml2 REQUIRED)
add_executable(test_precision test_precision.cpp)

target_include_directories(test_precision PRIVATE
    ${LIBXML2_INCLUDE_DIR}
)

target_link_libraries(test_precision PRIVATE
    ${LIBXML2_LIBRARIES}
)

target_compile_features(test_precision PRIVATE cxx_std_14)

C.C. @FabioBergonti

[gemini-code-assist]

Summary of Changes

Hello @flferretti, I'm Gemini Code Assist¹! I'm currently reviewing this pull request and will post my feedback shortly. In the meantime, here's a summary to help you and other reviewers quickly get up to speed!

This pull request enhances the URDF export functionality by introducing a new configuration option that allows users to define the numerical precision of values within the generated URDF file. This feature addresses the need for consistent and controlled decimal representation in robot models, which can be critical for simulation accuracy or compatibility with other systems. The implementation involves reading the precision setting from the configuration and applying it through a post-processing step that meticulously reformats numerical attributes across various URDF elements using XML parsing.

Highlights

• New Configuration Option: Introduced a new configuration parameter, urdfNumericalPrecision, allowing users to specify the number of decimal places for numerical values in the exported URDF.
• Numerical Precision Control: Implemented a post-processing step that uses libxml2 to parse the exported URDF and reformat all numerical attributes (e.g., positions, masses, joint limits) to the desired precision.
• Utility Functions: Added formatNumericalString and postProcessUrdfPrecision utility functions to handle the numerical formatting and XML manipulation, respectively.
• Documentation Update: Updated the README.md to document the new urdfNumericalPrecision option, including its valid range (1-15) and recommended value.

Changelog

• README.md
  • Added documentation for the new urdfNumericalPrecision configuration parameter under a 'Numerical Formatting Parameters' section.
  • Included details on the parameter's type, default value (Not set), description, and valid range (1-15).
• src/creo2urdf/include/creo2urdf/Creo2Urdf.h
  • Added a new member variable urdfNumericalPrecision of type int to the Creo2Urdf class, initialized to -1.
• src/creo2urdf/include/creo2urdf/Utils.h
  • Declared two new utility functions: postProcessUrdfPrecision and formatNumericalString.
  • Added Doxygen comments explaining the purpose and parameters of these new functions.
• src/creo2urdf/src/Creo2Urdf.cpp
  • Implemented logic to read the urdfNumericalPrecision value from the configuration YAML.
  • Added validation to ensure urdfNumericalPrecision is within the range of 1 to 15, defaulting to 8 and issuing a warning if an invalid value is provided.
  • Integrated a call to postProcessUrdfPrecision after the initial URDF export, conditionally executing it if urdfNumericalPrecision is set to a positive value.
• src/creo2urdf/src/Utils.cpp
  • Included necessary headers: <sstream>, <iomanip>, <libxml/parser.h>, <libxml/tree.h>, and <libxml/xpath.h>.
  • Implemented formatNumericalString to take a space-separated string of numbers and format each number to a specified decimal precision, also rounding very small numbers to zero.
  • Implemented postProcessUrdfPrecision which loads the URDF XML file, uses XPath expressions to identify various numerical attributes (e.g., inertial properties, joint limits, geometry dimensions), and applies the specified precision using formatNumericalString before saving the modified URDF.

Activity

• The author performed local testing of the new precision formatting functions using a standalone C++ test file (test_precision.cpp) and a custom CMakeLists.txt due to environment limitations (Linux vs. Creo).
• The author requested a review or attention from @FabioBergonti.

Using Gemini Code Assist

The full guide for Gemini Code Assist can be found on our documentation page, here are some quick tips.

Invoking Gemini

You can request assistance from Gemini at any point by creating a comment using either /gemini <command> or @gemini-code-assist <command>. Below is a summary of the supported commands on the current page.

Feature	Command	Description
Code Review	/gemini review	Performs a code review for the current pull request in its current state.
Pull Request Summary	/gemini summary	Provides a summary of the current pull request in its current state.
Comment	@gemini-code-assist	Responds in comments when explicitly tagged, both in pull request comments and review comments.
Help	/gemini help	Displays a list of available commands.

Customization

To customize Gemini Code Assist for GitHub experience, repository maintainers can create a configuration file and/or provide a custom code review style guide (such as PEP-8 for Python) by creating and adding files to a .gemini/ folder in the base of the repository. Detailed instructions can be found here.

Limitations & Feedback

Gemini Code Assist may make mistakes. Please leave feedback on any instances where its feedback is incorrect or counter productive. You can react with 👍 and 👎 on @gemini-code-assist comments. If you're interested in giving your feedback about your experience with Gemini Code Assist for Github and other Google products, sign up here.

You can also get AI-powered code generation, chat, as well as code reviews directly in the IDE at no cost with the Gemini Code Assist IDE Extension.

Footnotes

1. Review the Privacy Notices, Generative AI Prohibited Use Policy, Terms of Service, and learn how to configure Gemini Code Assist in GitHub here. Gemini can make mistakes, so double check it and use code with caution. ↩

[gemini-code-assist]

Code Review

The pull request introduces functionality to control the numerical precision of values when exporting URDF files. This is a valuable addition for users who need fine-grained control over the output format. The implementation includes parsing a new configuration parameter, validating its range, and applying the precision using XPath to modify the generated XML. Documentation in README.md has been updated to reflect this new feature. Overall, the changes are well-structured and address the stated objective.

[Nicogene]

@flferretti thank you for the contribution!

If I understood correctly you are formatting the numerical precision at the level of XML (URDF), isn't there an exporter option in iDyntree that allows to do this? (cc @traversaro)

If not it is ok what you did

[flferretti]

If I understood correctly you are formatting the numerical precision at the level of XML (URDF), isn't there an exporter option in iDyntree that allows to do this? (cc @traversaro)

If not it is ok what you did

I was not aware of this, thank you! I'll check on idyntree if I can find that option

EDIT: I couldn't find anything useful, maybe Silvio can give us more info

[traversaro]

If I understood correctly you are formatting the numerical precision at the level of XML (URDF), isn't there an exporter option in iDyntree that allows to do this? (cc @traversaro)

No, the double ---> string conversion in URDF exporter is handled using a shortest string exact conversion (i.e. the shortest string that represent exactly the double value) in https://github.com/gbionics/idyntree/blob/ab04c74ed32e8460a6a07f8a0166b470085b9236/src/model_io/codecs/include/private/URDFParsingUtils.h#L45-L60, since gbionics/idyntree#554 .

We can easily add a ModelExporter::numericalPrecision in https://github.com/gbionics/idyntree/blob/ab04c74ed32e8460a6a07f8a0166b470085b9236/src/model_io/codecs/include/iDynTree/ModelExporter.h#L90 . The current design of the URDF exporter using free functions means that a few private function signature needs to be modified to propagate the parameter correctly, but that is something trivial for any LLM.

The main advantages of doing this in iDynTree and then just expose the parameter are that any logic added to creo2urdf is hard to test and debug due to the licensing and OS requirement of Creo, while implementing the same logic somewhere else (like iDynTree, or any open source libraries) make it much easier to test the algorithm and fix it for anyone.

GitHub

idyntree/src/model_io/codecs/include/private/URDFParsingUtils.h at ab04c74ed32e8460a6a07f8a0166b470085b9236 · gbionics/idyntree

Multibody Dynamics Library designed for Free Floating Robots - gbionics/idyntree

GitHub

idyntree/src/model_io/codecs/include/iDynTree/ModelExporter.h at ab04c74ed32e8460a6a07f8a0166b470085b9236 · gbionics/idyntree

Multibody Dynamics Library designed for Free Floating Robots - gbionics/idyntree

[traversaro]

See comment.

[Nicogene]

I don't know if @traversaro @flferretti are you planning to add this functionality to iDyntree, I agree it would be much clean and easy to maintain, otherwise if @flferretti you have tested that it works, it is ok for me to merge this PR as it is

[flferretti]

I don't know if @traversaro @flferretti are you planning to add this functionality to iDyntree, I agree it would be much clean and easy to maintain, otherwise if @flferretti you have tested that it works, it is ok for me to merge this PR as it is

I can work on the iDynTree side next week and update this PR

[Nicogene]

I don't know if @traversaro @flferretti are you planning to add this functionality to iDyntree, I agree it would be much clean and easy to maintain, otherwise if @flferretti you have tested that it works, it is ok for me to merge this PR as it is

I can work on the iDynTree side next week and update this PR

Great! The only drawback I see is that we should need also un update of idyntree vcpkg version that includes this new feature (@traversaro)

[flferretti]

I've opened gbionics/idyntree#1293

[Nicogene]

Great @flferretti ! Remember to update:

• https://github.com/icub-tech-iit/creo2urdf/blob/0003097791e170779cac34cdd75282f39f6daef3/vcpkg.json#L12

To 15.0.0 as soon as we have the vcpkg port updated

[flferretti]

@Nicogene I can drop the outdated commits with a force push or if you prefer to squash, for me it's fine

[Nicogene]

@Nicogene I can drop the outdated commits with a force push or if you prefer to squash, for me it's fine

Great, let's await that:

• [idyntree] Update to 15.0.0 microsoft/vcpkg#49962

is merged and we can merge as well

[traversaro]

The vcpkg pr was merged.