def derive_inertia(objects):
"""Derives the inertial information from the specified objects.
Objects should be the list of objects of a specific link (could also contain just a single
object).
Contains these keys:
*mass*: float
*inertia*: list
*pose*: inertiapose containing:
*translation*: center of mass of the objects
*rotation*: [0., 0., 0.]
:param objects: list of objects to derive inertia from
:type objects: list of bpy.types.Object
:return: representation of the inertia or None if errors occur
:rtype: dict
"""
for singleobj in objects:
assert singleobj.phobostype == 'inertial', (
"Not an inertial object: " + singleobj.name + ".")
if objects:
# TODO we do not add the initObjectProperties of the fused object! Are they needed?
# props = initObjectProperties(objects[0], phobostype='inertial')
props = {}
mass, com, inertia = inertiamodel.fuse_inertia_data(objects)
inertia = inertiamodel.inertiaMatrixToList(inertia)
props['mass'] = mass
props['inertia'] = inertia
# TODO do we need to consider a rotation of the "ghost" inertial object?
# old code didn't use it either (unless the link inertial was rotated manually)
props['pose'] = {'translation': com, 'rotation_euler': [0., 0., 0.]}
log(" Success.", 'DEBUG')
return props
log("No inertia objects to derive information from.", 'ERROR')
return None
def validateInertiaData(obj, *args, adjust=False):
"""Validates an inertia dictionary or object.
This checks for the *inertia* and *mass* values in the dictionary (*inertial/inertia* and
*inertial/mass* for an object respectively).
Also, the inertia values are checked to be positive definite (for diagonal, determinant and
eigenvalues).
If adjust is set, values are adjusted/fixed for the returned dict/object. E.g. this sets a
negative mass to 1e-3.
Args:
obj(dict/bpy.types.Object): inertia dictionary or object to validate
*args: other arguments
adjust: if True, bad values will be fixed/complemented (Default value = False)
Returns:
tuple: list of :class:`ValidateMessage`\ s and the fixed dictionary/object
"""
from phobos.model.inertia import inertiaListToMatrix, inertiaMatrixToList
from phobos.utils.io import getExpSettings
import numpy
errors = []
expsetting = 10**(-getExpSettings().decimalPlaces)
# check dictionary parameters (most of the time pre object creation)
if isinstance(obj, dict):
missing = []
if 'inertia' not in obj:
missing.append('inertia')
if 'mass' not in obj:
missing.append('mass')
if missing:
errors.append(
ValidateMessage(
"Inertia dictionary not fully defined!",
'WARNING',
None,
None,
{
'log_info':
"Missing: " +
' '.join(["'{0}'".format(miss) for miss in missing]) +
" Set to default 1e-3."
},
))
if 'inertia' in missing:
obj['inertia'] = (1e-3, 0., 0., 1e-3, 0., 1e-3)
if 'mass' in missing:
obj['mass'] = 1e-3
inertia = obj['inertia']
mass = obj['mass']
# check existing object properties
elif isinstance(obj, bpy.types.Object):
if 'inertial/inertia' not in obj:
errors.append(
ValidateMessage(
"Inertia not defined!",
'WARNING',
obj,
'phobos.generate_inertial_objects',
{'log_info': "Set to default 1e-3."},
))
obj['inertial/inertia'] = (1e-3, 0., 0., 1e-3, 0., 1e-3)
if 'inertial/mass' not in obj:
errors.append(
ValidateMessage(
"Mass is not defined!",
'WARNING',
obj,
'phobos.generate_inertial_objects',
{'log_info': "Set to default 1e-3."},
))
obj['inertial/mass'] = 1e-3
inertia = obj['inertial/inertia']
mass = obj['inertial/mass']
# Check inertia vector for various properties, round to export precision
inertia = numpy.around(numpy.array(inertiaListToMatrix(inertia)),
decimals=getExpSettings().decimalPlaces)
if any(element <= expsetting for element in inertia.diagonal()):
errors.append(
ValidateMessage(
"Negative semidefinite main diagonal in inertia data!",
'WARNING',
None if isinstance(obj, dict) else obj,
None,
{'log_info': "Diagonal: " + str(inertia.diagonal())},
))
# Calculate the determinant if consistent, quick check
if numpy.linalg.det(inertia) <= expsetting:
errors.append(
ValidateMessage(
"Negative semidefinite determinant in inertia data! Checking singular values.",
'WARNING',
None if isinstance(obj, dict) else obj,
None,
{'log_info': "Determinant: " + str(numpy.linalg.det(inertia))},
))
# Calculate the eigenvalues if not consistent
if any(element <= expsetting
for element in numpy.linalg.eigvals(inertia)):
# Apply singular value decomposition and correct the values
S, V = numpy.linalg.eig(inertia)
S[S <= expsetting] = expsetting
inertia = V.dot(numpy.diag(S).dot(V.T))
errors.append(
ValidateMessage(
"Negative semidefinite eigenvalues in inertia data!",
'WARNING',
None if isinstance(obj, dict) else obj,
None,
{
'log_info':
"Eigenvalues: " + str(numpy.linalg.eigvals(inertia))
},
))
if mass <= 0.:
errors.append(
ValidateMessage(
"Mass is {}!".format('zero' if mass == 0. else 'negative'),
'WARNING',
None if isinstance(obj, dict) else obj,
None,
{} if not adjust else {'log_info': "Adjusted to 1e-3."},
))
mass = expsetting
inertia = inertiaMatrixToList(inertia)
if adjust and isinstance(obj, bpy.types.Object):
obj['inertial/inertia'] = inertia
obj['inertial/mass'] = mass
elif adjust:
obj['inertia'] = inertia
obj['mass'] = mass
return errors, obj
def buildModelDictionary(root):
"""Builds a python dictionary representation of a Phobos model.
:param root: bpy.types.objects
:return: dict
"""
# TODO remove this comment
# os.system('clear')
model = {
'links': {},
'joints': {},
'sensors': {},
'motors': {},
'controllers': {},
'materials': {},
'meshes': {},
'lights': {},
'groups': {},
'chains': {}
}
# timestamp of model
model["date"] = datetime.now().strftime("%Y%m%d_%H:%M")
if root.phobostype not in ['link', 'submodel']:
log("Found no 'link' or 'submodel' object as root of the robot model.",
"ERROR")
raise Exception(root.name + " is no valid root link.")
else:
if 'modelname' in root:
model['name'] = root["modelname"]
else:
log("No name for the model defines, setting to 'unnamed_model'",
"WARNING")
model['name'] = 'unnamed_model'
log(
"Creating dictionary for robot " + model['name'] + " from object " +
root.name, "INFO")
# create tuples of objects belonging to model
objectlist = sUtils.getChildren(
root,
selected_only=ioUtils.getExpSettings().selectedOnly,
include_hidden=False)
linklist = [link for link in objectlist if link.phobostype == 'link']
# digest all the links to derive link and joint information
log("Parsing links, joints and motors..." + (str(len(linklist))), "INFO")
for link in linklist:
# parse link and extract joint and motor information
linkdict, jointdict, motordict = deriveKinematics(link)
model['links'][linkdict['name']] = linkdict
# joint will be None if link is a root
if jointdict:
model['joints'][jointdict['name']] = jointdict
# motor will be None if no motor is attached or link is a root
if motordict:
model['motors'][motordict['name']] = motordict
# add inertial information to link
# if this link-inertial object is no present, we ignore the inertia!
try:
inertial = bpy.context.scene.objects['inertial_' +
linkdict['name']]
props = deriveDictEntry(inertial)
if props is not None:
model['links'][linkdict['name']]['inertial'] = props
except KeyError:
log("No inertia for link " + linkdict['name'], "WARNING")
# combine inertia if certain objects are left out, and overwrite it
inertials = (i for i in objectlist
if i.phobostype == 'inertial' and "inertial/inertia" in i)
editlinks = {}
for i in inertials:
if i.parent not in linklist:
realparent = sUtils.getEffectiveParent(i)
if realparent:
parentname = nUtils.getObjectName(realparent)
if parentname in editlinks:
editlinks[parentname].append(i)
else:
editlinks[parentname] = [i]
for linkname in editlinks:
inertials = editlinks[linkname]
try:
inertials.append(bpy.context.scene.objects['inertial_' + linkname])
except KeyError:
pass
mv, cv, iv = inertiamodel.fuseInertiaData(inertials)
iv = inertiamodel.inertiaMatrixToList(iv)
if mv is not None and cv is not None and iv is not None:
model['links'][linkname]['inertial'] = {
'mass': mv,
'inertia': iv,
'pose': {
'translation': list(cv),
'rotation_euler': [0, 0, 0]
}
}
# complete link information by parsing visuals and collision objects
log("Parsing visual and collision (approximation) objects...", "INFO")
for obj in objectlist:
# try:
if obj.phobostype in ['visual', 'collision']:
props = deriveDictEntry(obj)
parentname = nUtils.getObjectName(sUtils.getEffectiveParent(obj))
model['links'][parentname][obj.phobostype][nUtils.getObjectName(
obj)] = props
elif obj.phobostype == 'approxsphere':
props = deriveDictEntry(obj)
parentname = nUtils.getObjectName(sUtils.getEffectiveParent(obj))
model['links'][parentname]['approxcollision'].append(props)
# TODO delete me?
# except KeyError:
# try:
# log(parentname + " not found", "ERROR")
# except TypeError:
# log("No parent found for " + obj.name, "ERROR")
# combine collision information for links
for linkname in model['links']:
link = model['links'][linkname]
bitmask = 0
for collname in link['collision']:
try:
# bitwise OR to add all collision layers
bitmask = bitmask | link['collision'][collname]['bitmask']
except KeyError:
pass
link['collision_bitmask'] = bitmask
# parse sensors and controllers
log("Parsing sensors and controllers...", "INFO")
for obj in objectlist:
if obj.phobostype in ['sensor', 'controller']:
props = deriveDictEntry(obj)
model[obj.phobostype + 's'][nUtils.getObjectName(obj)] = props
# parse materials
log("Parsing materials...", "INFO")
model['materials'] = collectMaterials(objectlist)
for obj in objectlist:
if obj.phobostype == 'visual':
mat = obj.active_material
try:
if mat.name not in model['materials']:
# this should actually never happen
model['materials'][mat.name] = deriveMaterial(mat)
linkname = nUtils.getObjectName(sUtils.getEffectiveParent(obj))
model['links'][linkname]['visual'][nUtils.getObjectName(
obj)]['material'] = mat.name
except AttributeError:
log("Could not parse material for object " + obj.name, "ERROR")
# identify unique meshes
log("Parsing meshes...", "INFO")
for obj in objectlist:
try:
if ((obj.phobostype == 'visual' or obj.phobostype == 'collision')
and (obj['geometry/type'] == 'mesh')
and (obj.data.name not in model['meshes'])):
model['meshes'][obj.data.name] = obj
for lod in obj.lod_levels:
if lod.object.data.name not in model['meshes']:
model['meshes'][lod.object.data.name] = lod.object
except KeyError:
log("Undefined geometry type in object " + obj.name, "ERROR")
# gather information on groups of objects
log("Parsing groups...", "INFO")
# TODO: get rid of the "data" part and check for relation to robot
for group in bpy.data.groups:
# skip empty groups
if not group.objects:
continue
# handle submodel groups separately from other groups
if 'submodeltype' in group.keys():
continue
# TODO create code to derive Submodels
# model['submodels'] = deriveSubmodel(group)
elif nUtils.getObjectName(group, 'group') != "RigidBodyWorld":
model['groups'][nUtils.getObjectName(
group, 'group')] = deriveGroupEntry(group)
# gather information on chains of objects
log("Parsing chains...", "INFO")
chains = []
for obj in objectlist:
if obj.phobostype == 'link' and 'endChain' in obj:
chains.extend(deriveChainEntry(obj))
for chain in chains:
model['chains'][chain['name']] = chain
# gather information on lights
log("Parsing lights...", "INFO")
for obj in objectlist:
if obj.phobostype == 'light':
model['lights'][nUtils.getObjectName(obj)] = deriveLight(obj)
# gather submechanism information from links
log("Parsing submechanisms...", "INFO")
submechanisms = []
for link in linklist:
if 'submechanism/name' in link.keys():
#for key in [key for key in link.keys() if key.startswith('submechanism/')]:
# submechanisms.append({key.replace('submechanism/', ''): value
# for key, value in link.items()})
submech = {
'name':
link['submechanism/category'],
'type':
link['submechanism/type'],
'contextual_name':
link['submechanism/name'],
'jointnames_independent':
[j.name for j in link['submechanism/independent']],
'jointnames_spanningtree':
[j.name for j in link['submechanism/spanningtree']],
'jointnames_active':
[j.name for j in link['submechanism/active']]
}
submechanisms.append(submech)
model['submechanisms'] = submechanisms
# add additional data to model
model.update(deriveTextData(model['name']))
# shorten numbers in dictionary to n decimalPlaces and return it
log("Rounding numbers...", "INFO")
# TODO: implement this separately
epsilon = 10**(-ioUtils.getExpSettings().decimalPlaces)
return epsilonToZero(model, epsilon,
ioUtils.getExpSettings().decimalPlaces)
def buildModelDictionary(root):
"""Builds a python dictionary representation of a Phobos model.
:param root: bpy.types.objects
:return: dict
"""
#os.system('clear')
model = {
'links': {},
'joints': {},
'sensors': {},
'motors': {},
'controllers': {},
'materials': {},
'meshes': {},
'lights': {},
'groups': {},
'chains': {}
}
# timestamp of model
model["date"] = datetime.now().strftime("%Y%m%d_%H:%M")
if root.phobostype != 'link':
log("Found no 'link' object as root of the robot model.", "ERROR",
"buildModelDictionary")
raise Exception(root.name + " is no valid root link.")
else:
if 'modelname' in root:
model['name'] = root["modelname"]
else:
log("No name for the model defines, setting to 'unnamed_model'",
"WARNING", "buildModelDictionary")
model['name'] = 'unnamed_model'
log(
"Creating dictionary for robot " + model['name'] + " from object " +
root.name, "INFO", "buildModelDictionary")
# create tuples of objects belonging to model
objectlist = sUtils.getChildren(root,
selected_only=True,
include_hidden=False)
linklist = [link for link in objectlist if link.phobostype == 'link']
# digest all the links to derive link and joint information
log("Parsing links, joints and motors...", "INFO", "buildModelDictionary")
for link in linklist:
# parse link and extract joint and motor information
linkdict, jointdict, motordict = deriveKinematics(link)
model['links'][linkdict['name']] = linkdict
if jointdict: # joint will be None if link is a root
model['joints'][jointdict['name']] = jointdict
if motordict: # motor will be None if no motor is attached or link is a root
model['motors'][motordict['name']] = motordict
# add inertial information to link
try: # if this link-inertial object is no present, we ignore the inertia!
inertial = bpy.context.scene.objects['inertial_' +
linkdict['name']]
props = deriveDictEntry(inertial)
if props is not None:
model['links'][linkdict['name']]['inertial'] = props
except KeyError:
log("No inertia for link " + linkdict['name'], "WARNING",
"buildModelDictionary")
# combine inertia if certain objects are left out, and overwrite it
inertials = (i for i in objectlist
if i.phobostype == 'inertial' and "inertial/inertia" in i)
editlinks = {}
for i in inertials:
if i.parent not in linklist:
realparent = sUtils.getEffectiveParent(i)
if realparent:
parentname = nUtils.getObjectName(realparent)
if parentname in editlinks:
editlinks[parentname].append(i)
else:
editlinks[parentname] = [i]
for linkname in editlinks:
inertials = editlinks[linkname]
try:
inertials.append(bpy.context.scene.objects['inertial_' + linkname])
except KeyError:
pass
mv, cv, iv = inertiamodel.fuseInertiaData(inertials)
iv = inertiamodel.inertiaMatrixToList(iv)
if mv is not None and cv is not None and iv is not None:
model['links'][linkname]['inertial'] = {
'mass': mv,
'inertia': iv,
'pose': {
'translation': list(cv),
'rotation_euler': [0, 0, 0]
}
}
# complete link information by parsing visuals and collision objects
log("Parsing visual and collision (approximation) objects...", "INFO",
"buildModelDictionary")
for obj in objectlist:
try:
if obj.phobostype in ['visual', 'collision']:
props = deriveDictEntry(obj)
parentname = nUtils.getObjectName(
sUtils.getEffectiveParent(obj))
model['links'][parentname][obj.phobostype][
nUtils.getObjectName(obj)] = props
elif obj.phobostype == 'approxsphere':
props = deriveDictEntry(obj)
parentname = nUtils.getObjectName(
sUtils.getEffectiveParent(obj))
model['links'][parentname]['approxcollision'].append(props)
except KeyError:
try:
log(parentname + " not found", "ERROR")
except TypeError:
log("No parent found for " + obj.name, "ERROR")
# combine collision information for links
for linkname in model['links']:
link = model['links'][linkname]
bitmask = 0
for collname in link['collision']:
try:
bitmask = bitmask | link['collision'][collname]['bitmask']
except KeyError:
pass
link['collision_bitmask'] = bitmask
# parse sensors and controllers
log("Parsing sensors and controllers...", "INFO", "buildModelDictionary")
for obj in objectlist:
if obj.phobostype in ['sensor', 'controller']:
props = deriveDictEntry(obj)
model[obj.phobostype + 's'][nUtils.getObjectName(obj)] = props
# parse materials
log("Parsing materials...", "INFO", "buildModelDictionary")
model['materials'] = collectMaterials(objectlist)
for obj in objectlist:
if obj.phobostype == 'visual' and len(obj.data.materials) > 0:
mat = obj.data.materials[0]
matname = nUtils.getObjectName(mat, 'material')
if matname not in model['materials']:
model['materials'][matname] = deriveMaterial(
mat) # this should actually never happen
linkname = nUtils.getObjectName(sUtils.getEffectiveParent(obj))
model['links'][linkname]['visual'][nUtils.getObjectName(
obj)]['material'] = matname
# identify unique meshes
log("Parsing meshes...", "INFO", "buildModelDictionary")
for obj in objectlist:
try:
if ((obj.phobostype == 'visual' or obj.phobostype == 'collision')
and (obj['geometry/type'] == 'mesh')
and (obj.data.name not in model['meshes'])):
model['meshes'][obj.data.name] = obj
for lod in obj.lod_levels:
if lod.object.data.name not in model['meshes']:
model['meshes'][lod.object.data.name] = lod.object
except KeyError:
log("Undefined geometry type in object " + obj.name, "ERROR",
"buildModelDictionary")
# gather information on groups of objects
log("Parsing groups...", "INFO", "buildModelDictionary")
for group in bpy.data.groups: # TODO: get rid of the "data" part and check for relation to robot
if len(group.objects) > 0 and nUtils.getObjectName(
group, 'group') != "RigidBodyWorld":
model['groups'][nUtils.getObjectName(
group, 'group')] = deriveGroupEntry(group)
# gather information on chains of objects
log("Parsing chains...", "INFO", "buildModelDictionary")
chains = []
for obj in objectlist:
if obj.phobostype == 'link' and 'endChain' in obj:
chains.extend(deriveChainEntry(obj))
for chain in chains:
model['chains'][chain['name']] = chain
# gather information on lights
log("Parsing lights...", "INFO", "buildModelDictionary")
for obj in objectlist:
if obj.phobostype == 'light':
model['lights'][nUtils.getObjectName(obj)] = deriveLight(obj)
# add additional data to model
model.update(deriveTextData(model['name']))
# shorten numbers in dictionary to n decimalPlaces and return it
log("Rounding numbers...", "INFO", "buildModelDictionary")
epsilon = 10**(-bpy.data.worlds[0].phobosexportsettings.decimalPlaces
) # TODO: implement this separately
return epsilonToZero(
model, epsilon,
bpy.data.worlds[0].phobosexportsettings.decimalPlaces), objectlist
def deriveLink(linkobj, objectlist=[], logging=False, errors=None):
"""Derives a dictionary for the link represented by the provided obj.
If objectlist is provided, only objects contained in the list are taken into account
for creating the resulting dictionary.
The dictionary contains (besides any generic object properties) this information:
*parent*: name of parent object or None
*children*: list of names of the links child links
*object*: bpy.types.Object which represents the link
*pose*: deriveObjectPose of the linkobj
*collision*: empty dictionary
*visual*: empty dictionary
*inertial*: derive_inertia of all child inertials of the link
*approxcollision*: empty dictionary
Args:
linkobj(bpy.types.Object): blender object to derive the link from
objectlist: list of bpy.types.Object
.. seealso deriveObjectPose
.. seealso deriveInertial (Default value = [])
logging: (Default value = False)
errors: (Default value = None)
Returns:
"""
# use scene objects if no objects are defined
if not objectlist:
objectlist = list(bpy.context.scene.objects)
if logging:
log("Deriving link from object " + linkobj.name + ".", 'DEBUG')
props = initObjectProperties(
linkobj, phobostype='link', ignoretypes=linkobjignoretypes - {'link'}
)
parent = sUtils.getEffectiveParent(linkobj, objectlist)
props['parent'] = nUtils.getObjectName(parent) if parent else None
props['parentobj'] = parent
props['children'] = [child.name for child in linkobj.children if child.phobostype == 'link']
props['object'] = linkobj
props['pose'] = deriveObjectPose(linkobj)
props['collision'] = {}
props['visual'] = {}
props['inertial'] = {}
props['approxcollision'] = []
# gather all visual/collision objects for the link from the objectlist
for obj in [
item for item in objectlist if item.phobostype in ['visual', 'collision', 'approxsphere']
]:
effectiveparent = sUtils.getEffectiveParent(obj)
if effectiveparent == linkobj:
if logging:
log(
" Adding " + obj.phobostype + " '" + nUtils.getObjectName(obj) + "' to link.",
'DEBUG',
)
if obj.phobostype == 'approxsphere':
props['approxcollision'].append(deriveDictEntry(obj))
else:
props[obj.phobostype][nUtils.getObjectName(obj)] = deriveDictEntry(obj)
# gather the inertials for fusing the link inertia
inertials = inertiamodel.gatherInertialChilds(linkobj, objectlist)
mass = None
com = None
inertia = None
if len(inertials) > 0:
# get inertia data
mass, com, inertia = inertiamodel.fuse_inertia_data(inertials)
if not any([mass, com, inertia]):
if logging:
log("No inertia information for link object " + linkobj.name + ".", 'DEBUG')
else:
# add inertia to link
inertia = inertiamodel.inertiaMatrixToList(inertia)
props['inertial'] = {
'mass': mass,
'inertia': list(inertia),
'pose': {'translation': list(com), 'rotation_euler': [0, 0, 0]},
}
bitmask = 0
for collname in props['collision']:
try:
# bitwise OR to add all collision layers
bitmask = bitmask | props['collision'][collname]['bitmask']
except KeyError:
pass
props['collision_bitmask'] = bitmask
return props
def deriveModelDictionary(root, name='', objectlist=[]):
"""Returns a dictionary representation of a Phobos model.
If name is not specified, it overrides the modelname in the root. If the modelname is not
defined at all, 'unnamed' will be used instead.
Args:
root(bpy_types.Object): root object of the model
name(str): name for the derived model
objectlist(list: bpy_types.Object): objects to derive the model from
"""
if root.phobostype not in ['link', 'submodel']:
log(root.name + " is no valid 'link' or 'submodel' object.", "ERROR")
return None
# define model name
if name:
modelname = name
elif 'modelname' in root:
modelname = root['modelname']
else:
modelname = 'unnamed'
model = {
'links': {},
'joints': {},
'sensors': {},
'motors': {},
'controllers': {},
'materials': {},
'meshes': {},
'lights': {},
'groups': {},
'chains': {},
'date': datetime.now().strftime("%Y%m%d_%H:%M"),
'name': modelname
}
log(
"Creating dictionary for model '" + modelname + "' with root '" +
root.name + "'.", 'INFO')
# create tuples of objects belonging to model
if not objectlist:
objectlist = sUtils.getChildren(
root,
selected_only=ioUtils.getExpSettings().selectedOnly,
include_hidden=False)
linklist = [link for link in objectlist if link.phobostype == 'link']
# digest all the links to derive link and joint information
log(
"Parsing links, joints and motors... " + (str(len(linklist))) +
" total.", "INFO")
for link in linklist:
# parse link information (including inertia)
model['links'][nUtils.getObjectName(link, 'link')] = deriveLink(link)
if sUtils.getEffectiveParent(link):
# joint may be None if link is a root
jointdict = deriveJoint(link)
model['joints'][jointdict['name']] = jointdict
motordict = deriveMotor(link, jointdict)
# motor may be None if no motor is attached
if motordict:
model['motors'][motordict['name']] = motordict
# combine inertia for each link, taking into account inactive links
inertials = (i for i in objectlist
if i.phobostype == 'inertial' and 'inertial/inertia' in i)
editlinks = {}
for i in inertials:
if i.parent not in linklist:
realparent = sUtils.getEffectiveParent(
i, ignore_selection=bool(objectlist))
if realparent:
parentname = nUtils.getObjectName(realparent)
if parentname in editlinks:
editlinks[parentname].append(i)
else:
editlinks[parentname] = [i]
for linkname in editlinks:
inertials = editlinks[linkname]
try:
inertials.append(bpy.context.scene.objects['inertial_' + linkname])
except KeyError:
pass
# get inertia data
mass, com, inertia = inertiamodel.fuse_inertia_data(inertials)
if not any(mass, com, inertia):
continue
# add inertia to model
inertia = inertiamodel.inertiaMatrixToList(inertia)
model['links'][linkname]['inertial'] = {
'mass': mass,
'inertia': inertia,
'pose': {
'translation': list(com),
'rotation_euler': [0, 0, 0]
}
}
# complete link information by parsing visuals and collision objects
log("Parsing visual and collision (approximation) objects...", 'INFO')
for obj in objectlist:
if obj.phobostype in ['visual', 'collision']:
props = deriveDictEntry(obj)
parentname = nUtils.getObjectName(
sUtils.getEffectiveParent(obj,
ignore_selection=bool(objectlist)))
model['links'][parentname][obj.phobostype][nUtils.getObjectName(
obj)] = props
elif obj.phobostype == 'approxsphere':
props = deriveDictEntry(obj)
parentname = nUtils.getObjectName(
sUtils.getEffectiveParent(obj,
ignore_selection=bool(objectlist)))
model['links'][parentname]['approxcollision'].append(props)
# combine collision information for links
for linkname in model['links']:
link = model['links'][linkname]
bitmask = 0
for collname in link['collision']:
try:
# bitwise OR to add all collision layers
bitmask = bitmask | link['collision'][collname]['bitmask']
except KeyError:
pass
link['collision_bitmask'] = bitmask
# parse sensors and controllers
log("Parsing sensors and controllers...", 'INFO')
for obj in objectlist:
if obj.phobostype in ['sensor', 'controller']:
props = deriveDictEntry(obj)
model[obj.phobostype + 's'][nUtils.getObjectName(obj)] = props
# parse materials
log("Parsing materials...", 'INFO')
model['materials'] = collectMaterials(objectlist)
for obj in objectlist:
if obj.phobostype == 'visual':
mat = obj.active_material
if mat:
if mat.name not in model['materials']:
model['materials'][mat.name] = deriveMaterial(mat)
linkname = nUtils.getObjectName(
sUtils.getEffectiveParent(
obj, ignore_selection=bool(objectlist)))
model['links'][linkname]['visual'][nUtils.getObjectName(
obj)]['material'] = mat.name
# identify unique meshes
log("Parsing meshes...", "INFO")
for obj in objectlist:
try:
if ((obj.phobostype == 'visual' or obj.phobostype == 'collision')
and (obj['geometry/type'] == 'mesh')
and (obj.data.name not in model['meshes'])):
model['meshes'][obj.data.name] = obj
for lod in obj.lod_levels:
if lod.object.data.name not in model['meshes']:
model['meshes'][lod.object.data.name] = lod.object
except KeyError:
log("Undefined geometry type in object " + obj.name, "ERROR")
# gather information on groups of objects
log("Parsing groups...", 'INFO')
# TODO: get rid of the "data" part and check for relation to robot
for group in bpy.data.groups:
# skip empty groups
if not group.objects:
continue
# handle submodel groups separately from other groups
if 'submodeltype' in group.keys():
continue
# TODO create code to derive Submodels
# model['submodels'] = deriveSubmodel(group)
elif nUtils.getObjectName(group, 'group') != "RigidBodyWorld":
model['groups'][nUtils.getObjectName(
group, 'group')] = deriveGroupEntry(group)
# gather information on chains of objects
log("Parsing chains...", "INFO")
chains = []
for obj in objectlist:
if obj.phobostype == 'link' and 'endChain' in obj:
chains.extend(deriveChainEntry(obj))
for chain in chains:
model['chains'][chain['name']] = chain
# gather information on lights
log("Parsing lights...", "INFO")
for obj in objectlist:
if obj.phobostype == 'light':
model['lights'][nUtils.getObjectName(obj)] = deriveLight(obj)
# gather submechanism information from links
log("Parsing submechanisms...", "INFO")
def getSubmechanisms(link):
if 'submechanism/name' in link.keys():
submech = {
'type':
link['submechanism/type'],
'contextual_name':
link['submechanism/name'],
'name':
link['submechanism/subtype'] if 'submechanism/subtype' in link
else link['submechanism/type'],
'jointnames_independent': [
nUtils.getObjectName(j, 'joint')
for j in link['submechanism/independent']
],
'jointnames_spanningtree': [
nUtils.getObjectName(j, 'joint')
for j in link['submechanism/spanningtree']
],
'jointnames_active': [
nUtils.getObjectName(j, 'joint')
for j in link['submechanism/active']
],
# TODO: this should work in almost all cases, still a bit of a hack:
'file_path':
'../submechanisms/urdf/' + link['submechanism/name'] + '.urdf'
}
log(' ' + submech['contextual_name'], 'DEBUG')
else:
submech = None
mechanisms = [submech] if submech else []
for c in link.children:
if c.phobostype in ['link', 'interface'] and c in objectlist:
mechanisms.extend(getSubmechanisms(c))
return mechanisms
model['submechanisms'] = getSubmechanisms(root)
# add additional data to model
model.update(deriveTextData(model['name']))
# shorten numbers in dictionary to n decimalPlaces and return it
log("Rounding numbers...", "INFO")
return roundFloatsInDict(model, ioUtils.getExpSettings().decimalPlaces)
def validateInertiaData(obj, *args, adjust=False):
"""Validates an inertia dictionary or object.
This checks for the *inertia* and *mass* values in the dictionary (*inertial/inertia* and
*inertial/mass* for an object respectively).
Also, the inertia values are checked to be positive definite (for diagonal, determinant and
eigenvalues).
If adjust is set, values are adjusted/fixed for the returned dict/object. E.g. this sets a
negative mass to 1e-3.
Args:
obj(dict/bpy.types.Object): inertia dictionary or object to validate
*args: other arguments
adjust: if True, bad values will be fixed/complemented (Default value = False)
Returns:
tuple: list of :class:`ValidateMessage`\ s and the fixed dictionary/object
"""
from phobos.model.inertia import inertiaListToMatrix, inertiaMatrixToList
from phobos.utils.io import getExpSettings
import numpy
errors = []
# check dictionary parameters (most of the time pre object creation)
if isinstance(obj, dict):
missing = []
if 'inertia' not in obj:
missing.append('inertia')
if 'mass' not in obj:
missing.append('mass')
if missing:
errors.append(
ValidateMessage(
"Inertia dictionary not fully defined!",
'WARNING',
None,
None,
{
'log_info': "Missing: "
+ ' '.join(["'{0}'".format(miss) for miss in missing])
+ " Set to default 1e-3."
},
)
)
if 'inertia' in missing:
obj['inertia'] = (1e-3, 0., 0., 1e-3, 0., 1e-3)
if 'mass' in missing:
obj['mass'] = 1e-3
inertia = obj['inertia']
mass = obj['mass']
# check existing object properties
elif isinstance(obj, bpy.types.Object):
if 'inertial/inertia' not in obj:
errors.append(
ValidateMessage(
"Inertia not defined!",
'WARNING',
obj,
'phobos.generate_inertial_objects',
{'log_info': "Set to default 1e-3."},
)
)
obj['inertial/inertia'] = (1e-3, 0., 0., 1e-3, 0., 1e-3)
if 'inertial/mass' not in obj:
errors.append(
ValidateMessage(
"Mass is not defined!",
'WARNING',
obj,
'phobos.generate_inertial_objects',
{'log_info': "Set to default 1e-3."},
)
)
obj['inertial/mass'] = 1e-3
inertia = obj['inertial/inertia']
mass = obj['inertial/mass']
# Check inertia vector for various properties, round to export precision
inertia = numpy.around(
numpy.array(inertiaListToMatrix(inertia)), decimals=getExpSettings().decimalPlaces
)
if any(element <= 0.0 for element in inertia.diagonal()):
errors.append(
ValidateMessage(
"Negative semidefinite main diagonal in inertia data!",
'WARNING',
None if isinstance(obj, dict) else obj,
None,
{'log_info': "Diagonal: " + str(inertia.diagonal())},
)
)
# Calculate the determinant if consistent, quick check
if numpy.linalg.det(inertia) <= 0.0:
errors.append(
ValidateMessage(
"Negative semidefinite determinant in inertia data! Checking singular values.",
'WARNING',
None if isinstance(obj, dict) else obj,
None,
{'log_info': "Determinant: " + str(numpy.linalg.det(inertia))},
)
)
# Calculate the eigenvalues if not consistent
if any(element <= 0.0 for element in numpy.linalg.eigvals(inertia)):
# Apply singular value decomposition and correct the values
S, V = numpy.linalg.eig(inertia)
S[S <= 0.0] = 1e-3
inertia = V.dot(numpy.diag(S).dot(V.T))
errors.append(
ValidateMessage(
"Negative semidefinite eigenvalues in inertia data!",
'WARNING',
None if isinstance(obj, dict) else obj,
None,
{'log_info': "Eigenvalues: " + str(numpy.linalg.eigvals(inertia))},
)
)
if mass <= 0.:
errors.append(
ValidateMessage(
"Mass is {}!".format('zero' if mass == 0. else 'negative'),
'WARNING',
None if isinstance(obj, dict) else obj,
None,
{} if not adjust else {'log_info': "Adjusted to 1e-3."},
)
)
mass = 1e-3
inertia = inertiaMatrixToList(inertia)
if adjust and isinstance(obj, bpy.types.Object):
obj['inertial/inertia'] = inertia
obj['inertial/mass'] = mass
elif adjust:
obj['inertia'] = inertia
obj['mass'] = mass
return errors, obj