vonunwerth / MeshLabInertiaToURDF

Calculating inertial tensor for roboitc links with pymeshlab for Gazebo robots
GNU General Public License v3.0
34 stars 6 forks source link

Scaling operation is unnecessary #2

Open guru-florida opened 8 months ago

guru-florida commented 8 months ago

Nothing wrong with the final result, but fyi, the scaling operation is redundant. The problem with using Meshlab UI is that it doesn't print enough precision nor does it use exponential format, but in your code the tensor is as a ndarray with floats therefor the precision is maintained without scaling. Since you've increased the precision of the printing, it works the same.

So, get rid of the scaling operation, and just change:

        tensor = geom['inertia_tensor'] / pow(scale_factor, 2) * self.mass / volume

to this, where the volume is the original unscaled volume:

        tensor = geom['inertia_tensor'] * self.mass / volume

I've verified the output results are the same numbers. Also, I use this function to export the numbers, it chooses to output non-exponential notation for simple numbers, but switches to exponential notation for small quantities. Feel free to use. Technically, you don't even need the middle 0.001 comparison because the 'g' format will switch between simple and exponential notation based on the precision specifier...this is just my personal preference.

# converts a single float to a string
def export_float(f: float, precision: int = 5):
    if f == 0:
        return "0"
    elif abs(f) < 0.001:
        return f"{f:.{precision}e}"
    else:
        return f"{f:.{precision}g}"

# build a function for converting each element of a numpy array to a string
export_tensor = numpy.vectorize(export_float)

Then later in your processing function, you can use the vectorized exporter:

# convert each float element of the tensor to a string with our desired precision/format
tensor_str = export_tensor(tensor)

# convert to XML   (snipped sections with ... that are unchanged)
# we don't need to format the elements now since they are already strings
inertia_xml = f'<inertia ixx="{tensor_str[0, 0]}" ixy="{tensor[1, 0]}"  ...etc... />'

P.S. Wonderful piece of code you wrote! This was so helpful in my case. If I met you, I'd buy you a buy - or few. :)