calculate distance between two selected parts in freecad for urdf

gistfile1.txt

import math
import numpy as np
# https://www.freecadweb.org/wiki/Code_snippets
sel = FreeCADGui.Selection.getSelection()                                 # select object with getSelection()
boundBox_= sel[0].Shape.BoundBox                                          # BoundBox of the object
boundBox_2 = sel[1].Shape.BoundBox

# https://www.freecadweb.org/wiki/Vector_API
v1 = FreeCAD.Vector(boundBox_.Center)
v2 = FreeCAD.Vector(boundBox_2.Center)

# Select parent -> child (urdf)
print("Distance between " + sel[0].FullName + " and " + sel[1].FullName + ": " + str(-0.001*(v1.x-v2.x)) + " " + str(-0.001*(v1.y-v2.y)) + " " + str(-0.001*(v1.z-v2.z)))
print("-"*100)