Skip to content

Commit

Permalink
New more feature-rich version! 🍺
Browse files Browse the repository at this point in the history 
* Now supporting ROS1 and ROS2 for package lookup
* Producing inertia matrices for all geometries (mesh, box, sphere, cylinder)
* Integrating xacro to support parameters and arguments
* Looking for a geometry in the collision property if a visual one is not found

Signed-off-by: George Stavrinos <gstavrinos@protonmail.com>
  • Loading branch information
@gstavrinos
gstavrinos committed Jun 10, 2022
1 parent b0916ee commit 56bd43e
Show file tree
Hide file tree
Showing 2 changed files with 154 additions and 70 deletions.
66 changes: 53 additions & 13 deletions README.md
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,56 @@
# calc_inertia_for_urdf.py
Based on a provided URDF (or xacro), it reads all geometries that have a `mass` inertial tag, and prints a URDF-ready inertia matrix.

## Dependencies
* xacro (pip install xacro)
* numpy-stl (pip install numpy-stl)
* urdf_paser_py (pip install urdf-parser-py)

## Command line params
* 1: URDF file

### Features added in release 2.0
* Support for ROS1 **AND** ROS2. This functionality basically involves looking for mesh files in `package://` tags. (If your urdf/xacro only includes `file://` tags, there is nothing ROS-related to worry about)
* Support for **ALL** kind of geometries (mesh, box, sphere, cylinder)
* Xacro integration for files that use arguments or parameters in fields of interest
* Support for links that have inertia but not a visual tag (collisions only)

### Example
`python calc_inertia_for_urdf.py /home/gstavrinos/urdfs/model.urdf`
### Output

```
Link name: chassis_link_00
Mass: 48.0
Scale: [0.015, 0.015, 0.015]
Mesh: file:///home/gstavrinos/ros2_ws/install/kart_description/share/kart_description/meshes/kart_chassis.stl
---
Calculating inertia...
---
<inertia ixx="107.96846534714074" ixy="0" ixz="0" iyy="279.5263367708269" iyz="0" izz="346.94143021711494" />
Link name: steering_wheel_link_00
Mass: 7.0
Scale: [0.015, 0.015, 0.015]
Mesh: file:///home/gstavrinos/ros2_ws/install/kart_description/share/kart_description/meshes/kart_steering_wheel.stl
---
Calculating inertia...
---
<inertia ixx="1.7008840358588881" ixy="0" ixz="0" iyy="2.939219711456205" iyz="0" izz="3.1101995734940084" />
.
.
.
```

---


# calc_inertia.py
:warning:**Currently deprecated**:warning:

With an object's STL file and mass, calculate its inertia, based on its bounding box, in mass \* dimension \* scale. The output is URDF-ready.

## Dependencies (Compatible with both Python2 and Python3)
Expand All @@ -7,20 +59,8 @@ With an object's STL file and mass, calculate its inertia, based on its bounding
## Command line params
* 1: STL file
* 2: Mass (in kg)
* 3: Scale (in acese you are scaling your model before use)(= 0.000001 if stl is in mm)
* 3: Scale (in case you are scaling your model before use)(= 0.000001 if stl is in mm)

## Example
`python calc_inertia.py "/home/gstavrinos/meshes/model.stl" 0.025 1`

# calc_inertia_for_urdf.py
Based on the provided URDF (or xacro, by just disregarding xacro tags), it reads all **meshes** (no primitive shapes) that have a `mass` inertial tag, and prints a URDF-ready inertia matrix.

## Dependencies
* numpy-stl (sudo pip install numpy-stl)
* urdf_paser_py (sudo pip install urdf-parser-py)

## Command line params
* 1: URDF file

## Example
`python calc_inertia_for_urdf.py /home/gstavrinos/urdfs/model.urdf`
158 changes: 101 additions & 57 deletions calc_inertia_for_urdf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,79 +2,123 @@

import os
import sys
import stl
import rospkg
import xml.etree.ElementTree
from urdf_parser_py.urdf import URDF
# import stl
import xacro
from stl import mesh
from urdf_parser_py.urdf import URDF, Mesh, Box, Sphere, Cylinder

#(sudo pip install numpy-stl)
#(sudo pip install urdf-parser-py)
#(pip install xacro)
#(pip install numpy-stl)
#(pip install urdf-parser-py)

# Command line params:
# 1: URDF file


def getDimensions(model):
minx = maxx = miny = maxy = minz = maxz = None
for p in model.points:
if minx is None:
minx = p[stl.Dimension.X]
maxx = p[stl.Dimension.X]
miny = p[stl.Dimension.Y]
maxy = p[stl.Dimension.Y]
minz = p[stl.Dimension.Z]
maxz = p[stl.Dimension.Z]
else:
maxx = max(p[stl.Dimension.X], maxx)
minx = min(p[stl.Dimension.X], minx)
maxy = max(p[stl.Dimension.Y], maxy)
miny = min(p[stl.Dimension.Y], miny)
maxz = max(p[stl.Dimension.Z], maxz)
minz = min(p[stl.Dimension.Z], minz)
return maxx - minx, maxy - miny, maxz - minz
return model.x.max() - model.x.min(), model.y.max() - model.y.min(), model.z.max() - model.z.min()

# for p in model.points:
# if minx is None:
# minx = p[stl.Dimension.X]
# maxx = p[.Dimension.X]
# miny = p[stl.Dimension.Y]
# maxy = p[stl.Dimension.Y]
# minz = p[stl.Dimension.Z]
# maxz = p[stl.Dimension.Z]
# else:
# maxx = max(p[stl.Dimension.X], maxx)
# minx = min(p[stl.Dimension.X], minx)
# maxy = max(p[stl.Dimension.Y], maxy)
# miny = min(p[stl.Dimension.Y], miny)
# maxz = max(p[stl.Dimension.Z], maxz)
# minz = min(p[stl.Dimension.Z], minz)
# return maxx - minx, maxy - miny, maxz - minz

# Based on https://en.wikipedia.org/wiki/List_of_moments_of_inertia#List_of_3D_inertia_tensors
def getInertia(x, y, z, m, s):
def getInertia(geometry, m, s):
print("\033[97m Link name: \033[0m" + link_name)
print("\033[93m Mass: \033[0m" + str(mass))
print("\033[95m Scale: \033[0m" + str(scale))
xx = yy = zz = 0.0
if type(geometry) == Mesh:
print("\033[94m Mesh: \033[0m" + geometry.filename)
print("---\nCalculating inertia...\n---")
# TODO handle filename
ROS_VERSION = os.getenv("ROS_VERSION")
get_pkg_fn = None
if not ROS_VERSION:
print("Could not find the ROS_VERSION environment variable, thus, can't determine your ros version. Assuming ROS2!")
ROS_VERSION = "2"
if ROS_VERSION == "1":
import rospkg
get_pkg_fn = rospkg.RosPack().get_path
else:
import ament_index_python
get_pkg_fn = ament_index_python.get_package_share_path
pkg_tag = "package://"
file_tag = "file://"
mesh_file = ""
if geometry.filename.startswith(pkg_tag):
package, mesh_file = geometry.filename.split(pkg_tag)[1].split(os.sep, 1)
mesh_file = get_pkg_fn(package)+os.sep+mesh
elif geometry.filename.startswith(file_tag):
mesh_file = geometry.filename.replace(file_tag, "")
model = mesh.Mesh.from_file(mesh_file)
x,y,z = getDimensions(model)
xx,yy,zz = getBoxInertia(x, y, z, m, geometry.scale)
elif type(geometry) == Box:
print("\033[94m Box: \033[0m" + geometry.size)
print("---\nCalculating inertia...\n---")
x,y,z = geometry.size
xx,yy,zz = getBoxInertia(x, y, z, m, s)
elif type(geometry) == Sphere:
print("\033[94m Box: \033[0m" + geometry.size)
print("---\nCalculating inertia...\n---")
x,y,z = geometry.size
xx,yy,zz = getBoxInertia(x, y, z, m, s)

print("\033[92m")
print("<inertia ixx=\"%s\" ixy=\"0\" ixz=\"0\" iyy=\"%s\" iyz=\"0\" izz=\"%s\" />" % (xx,yy,zz))
print("\033[0m")

def getBoxInertia(x, y, z, m, s):
xx = 1./12 * m * (y**2 + z**2) * s[0]
yy = 1./12 * m * (x**2 + z**2) * s[1]
zz = 1./12 * m * (x**2 + y**2) * s[2]
return xx, yy, zz

def getSphereInertia(r, m):
i = 2./5 * m * r**2
return i, i, i

def getCylinderInertia(r, h, m):
xx = yy = 1./12 * (3 * r**2 + h**2)
zz = 1./2 * m * r**2
return xx, yy, zz

if __name__ == '__main__':
rospack = rospkg.RosPack()
for (ev, el) in xml.etree.ElementTree.iterparse(sys.argv[1]):
link_name = mass = mesh = None
# robot = URDF.from_xml_string(xacro.process_file(sys.argv[1]).toprettyxml(indent=" "))
robot = URDF.from_xml_string(xacro.process_file(sys.argv[1]).toprettyxml())
for link in robot.links:
link_name = mass = geometry = None
x = y = z = 0.0
scale = [1.0, 1.0, 1.0]
if el.tag == "link":
link_name = el.get("name")

for i in el:
if i.tag == "inertial":
for j in i:
if j.tag == "mass":
mass = float(j.get("value"))
if i.tag == "visual":
for j in i:
if j.tag == "geometry":
for k in j:
if k.tag == "mesh":
mesh = k.get("filename")
package, mesh = mesh.split("package://")[1].split("/", 1)
mesh = rospack.get_path(package)+os.sep+mesh

scale = k.get("scale", default=[1.0, 1.0, 1.0]).split()
scale = [float(n) for n in scale]
link_name = link.name
inertial = link.inertial
if inertial:
mass = inertial.mass
if mass:
visual = link.visual
if visual:
geometry = visual.geometry
# If we don't find a visual geometry, look for a collision one
else:
collision = link.collision
if collision:
geometry = collision.geometry

if mass != None and mesh != None:
print("\033[97m Link name: \033[0m" + link_name)
print("\033[93m Mass: \033[0m" + str(mass))
print("\033[94m Mesh: \033[0m" + mesh)
print("\033[95m Scale: \033[0m" + str(scale))
print("---\nCalculating inertia...\n---")
model = stl.mesh.Mesh.from_file(mesh)
x, y, z = getDimensions(model)
print("\033[92m")
print("<inertia ixx=\"%s\" ixy=\"0\" ixz=\"0\" iyy=\"%s\" iyz=\"0\" izz=\"%s\" />" % (getInertia(x, y, z, mass, scale)))
print("\033[0m")
if mass != None and geometry != None:
if geometry.scale:
scale = geometry.scale
getInertia(geometry, mass, scale)

0 comments on commit 56bd43e

Please sign in to comment.