galou / freecad.cross

CROSS CAD and ROS, an Open-Source Synergy; ROS workbench for FreeCAD
GNU Lesser General Public License v2.1
95 stars 26 forks source link

Feature: Auto calculating of mass and inertia based on link density #37

Closed drfenixion closed 6 months ago

drfenixion commented 7 months ago

I am sure we need auto calc of mass and inertia for links. Plan: Add option to choose material of link (or a whole robot and it will inherited by links without material). Add tool of calc mass and inertial of all links based on material density. @galou , what do you think? Any suggestion what can help.

drfenixion commented 7 months ago

@galou, I found FreeCAD macro that can calc mass and ineria. We can adapt code and make tool in CROSS for auto calc that for links based on density (optionaly maybe add material option choose from).

FCInfo macro. https://wiki.freecad.org/Macro_FCInfo inertia mass

drfenixion commented 7 months ago

Workpiece

obj_name = 'Body002'
model = FreeCAD.ActiveDocument.getObject(obj_name)

#calc of mass
density = 1040 # ABS plastic
volume = model.Shape.Volume
mass = volume * density

#get inertia
try:  
    matrixOfInertia = model.Shape.MatrixOfInertia
except Exception:
    try:
        matrixOfInertia = model.shape.Solids[0].MatrixOfInertia
    except Exception:
        matrixOfInertia= ""

centerOfMass = model.Shape.CenterOfMass
principalProperties = model.Shape.PrincipalProperties

print('mass: ', mass)
print('matrixOfInertia: ', matrixOfInertia)
print('centerOfMass: ', centerOfMass)
print('principalProperties: ', principalProperties) # principal properties of inertia of current system

Material lib can be taken from FEM->model->materials->material editor

import MaterialEditor
material = MaterialEditor.editMaterial() # open material editor widget (from FEM) and save selected material to var

References for inertia calc: https://classic.gazebosim.org/tutorials?tut=inertia - description of urdf inertia params and test inertia metodology https://habr.com/ru/articles/579364/ - calc inertia tensor based on mesh https://github.com/gstavrinos/calc-inertia/blob/master/calc_inertia_for_urdf.py - calc inertia tensor for primitives (cylinder, box, sphere) https://forum.freecad.org/viewtopic.php?p=555514#p555514 - addressing of FreeCAD matrixOfInertia https://forum.freecad.org/viewtopic.php?p=104153#p104153 - tip that "OCC assumes the solid has a density of 1 (1000 kg/m3, I guess)". It is answer how FC calculates matrixOfInertia without density. Looks it uses default density. https://forum.freecad.org/viewtopic.php?p=104196#p104196 - how to calc matrixOfInertia of compounds. https://forum.freecad.org/viewtopic.php?p=55224#p55224 - description about FC default density = 1 https://forum.freecad.org/viewtopic.php?t=34474 - how to get moment of inertia by some axis https://forum.freecad.org/viewtopic.php?p=14874&sid=4c82cbf72a6a6417f1144e66786f75b2#p14874 - convertion FC value considering the density https://forum.freecad.org/viewtopic.php?p=26524#p26524 - after applied filled obj became compound and need to extract data of Solids of it. https://forum.freecad.org/viewtopic.php?style=10&p=511918#p511918 - inertia matrix convertion from global aligned CS to local aligned CS https://forum.freecad.org/viewtopic.php?p=568118#p568118 - get relative placement

https://wiki.freecad.org/Property - FC properties

Tools: https://www.omnicalculator.com/physics/mass-moment-of-inertia - mass moment of inertia calculator (for checking resulting inertia) https://www.engineeringtoolbox.com/moment-inertia-torque-d_913.html - related formulas and descriptions Formulas of inertia by natural text - https://www.wolframalpha.com/input?i=cone+moment+of+inertia+with+formula&assumption=%7B%22F%22%2C+%22ConeMomentOfInertia%22%2C+%22r%22%7D+-%3E%2230+m%22&assumption=%7B%22F%22%2C+%22ConeMomentOfInertia%22%2C+%22h%22%7D+-%3E%2240+m%22&assumption=%7B%22FS%22%7D+-%3E+%7B%7B%22ConeMomentOfInertia%22%2C+%22Iparallel%22%7D%2C+%7B%22ConeMomentOfInertia%22%2C+%22m%22%7D%2C+%7B%22ConeMomentOfInertia%22%2C+%22r%22%7D%7D&assumption=%7B%22F%22%2C+%22ConeMomentOfInertia%22%2C+%22m%22%7D+-%3E%221+kg%22

Debug: https://forum.freecad.org/viewtopic.php?p=609711#p609711 - VS Code debug config manual. For debug in docker container dont forget to connect VS Code to container (it will not works without it). Also it can be need to choose FreeCAD process to debug if you run FreeCAD outside VS Code.

drfenixion commented 7 months ago

Started implementation.

galou commented 7 months ago

This is missing from the current tool, for sure. I'm not convinced that you can get anything better than an approximation and it's a lot of work to implement this because objects in FreeCAD tend to be inconsistent (mesh vs. shape, placement of links, objects without shape, ...). I'm more concentrated on implementing functions to debug a running system so I won't have time for the modeling part of the workbench. I'll glad if you come with an implementation.

drfenixion commented 7 months ago

@galou Looks like I'm close to the result with mass and inertia auto calculation. Todo: 1) To bug fix - some elements have orientation drifting. looks likes it depend of as body was modeled. Most oriented well. 2) Minor fixes and improvements.

You can see generated inertia blocks. They must be aproximary same size and position/orientation as their links. 2 inertia blocks in model has orientation drift, others are ok. Size and position are ok of all blocks. inertia_blocks

drfenixion commented 7 months ago

I understood reason of 2 not well oriented inertia blocks. It is not a bug. It is because solid of link is not a symmetrical and inertial blocks be built to diagonal oriented with an inclination towards the mass displacement. I tested it by deleted not symmetrical solid of link and it became oriented well.

Deletion of not symmetrical solid fragment lead to horizontal orientation of inertia block (at picture). inertia_block_correct_oriented

Сonclusion - tilt block of inertia for unsymmetrical bodies is normal.

drfenixion commented 6 months ago

Created fork named OVERCROSS with:

  1. Material selection of link or whole robot
  2. Auto calculation of mass and inertia
  3. Launcher for Gazebo
  4. New some icons and icons reordering
  5. Bug fixes
  6. Others

OVERCROSS