def exportMesh(obj, path, meshtype):
# DOCU add some docstring
objname = nUtils.getObjectName(obj)
tmpobjname = obj.name
# OPT: surely no one will ever name an object like so, better solution?
obj.name = 'tmp_export_666'
tmpobject = bUtils.createPrimitive(objname, 'box', (1.0, 1.0, 1.0))
# copy the mesh here
tmpobject.data = obj.data
outpath = os.path.join(path, obj.data.name + "." + meshtype)
if meshtype == 'obj':
axis_forward = ioUtils.getExpSettings().obj_axis_forward
axis_up = ioUtils.getExpSettings().obj_axis_up
bpy.ops.export_scene.obj(filepath=outpath,
use_selection=True,
use_normals=True,
use_materials=False,
use_mesh_modifiers=True,
axis_forward=axis_forward,
axis_up=axis_up)
elif meshtype == 'stl':
bpy.ops.export_mesh.stl(filepath=outpath,
use_selection=True,
use_mesh_modifiers=True)
elif meshtype == 'dae':
bpy.ops.wm.collada_export(filepath=outpath, selected=True)
bpy.ops.object.select_all(action='DESELECT')
tmpobject.select = True
bpy.ops.object.delete()
obj.name = tmpobjname
def storePose(root, posename):
"""Stores the current pose of all of a model's selected joints.
Existing poses of the same name will be overwritten.
Args:
root(bpy_types.Object): root of the model the pose belongs to
posename(str): name the pose will be stored under
Returns:
: Nothing.
"""
if root:
filename = nUtils.getModelName(root) + '::poses'
posedict = json.loads(bUtils.readTextFile(filename))
if not posedict:
posedict = {posename: {'name': posename, 'joints': {}}}
else:
posedict[posename] = {'name': posename, 'joints': {}}
links = sUtils.getChildren(root, ('link', ), True, False)
sUtils.selectObjects([root] + links, clear=True, active=0)
bpy.ops.object.mode_set(mode='POSE')
for link in (link for link in links if 'joint/type' in link
and link['joint/type'] not in ['fixed', 'floating']):
link.pose.bones['Bone'].rotation_mode = 'XYZ'
posedict[posename]['joints'][nUtils.getObjectName(
link, 'joint')] = link.pose.bones['Bone'].rotation_euler.y
bpy.ops.object.mode_set(mode='OBJECT')
posedict = gUtils.roundFloatsInDict(
posedict,
ioUtils.getExpSettings().decimalPlaces)
bUtils.updateTextFile(filename, json.dumps(posedict))
else:
log("No model root provided to store the pose for", "ERROR")
def exportSMURFScene(entities, path):
"""Exports an arranged scene into SMURFS. It will export only entities
with a valid entity/name, and entity/type property.
Args:
selected_only(bool): If True only selected entities get exported.
subfolder(bool): If True the models are exported into separate subfolders
entities:
path:
Returns:
"""
log("Exporting scene to " + path + '.smurfs', "INFO")
with open(path + '.smurfs', 'w') as outputfile:
sceneinfo = (
"# SMURF scene created at "
+ path
+ " "
+ datetime.now().strftime("%Y%m%d_%H:%M")
+ "\n"
)
log(sceneinfo, "INFO")
sceneinfo += "# created with Phobos " + version + " - " + repository + "\n\n"
ioUtils.securepath(path)
outputfile.write(sceneinfo)
entitiesdict = roundFloatsInDict(
{'entities': entities}, ioUtils.getExpSettings().decimalPlaces
)
outputfile.write(yaml.dump(entitiesdict))
def exportSMURFScene(entities, path):
"""Exports an arranged scene into SMURFS. It will export only entities
with a valid entity/name, and entity/type property.
Args:
selected_only(bool): If True only selected entities get exported.
subfolder(bool): If True the models are exported into separate subfolders
entities:
path:
Returns:
"""
log("Exporting scene to " + path + '.smurfs', "INFO")
with open(path + '.smurfs', 'w') as outputfile:
sceneinfo = ("# SMURF scene created at " + path + " " +
datetime.now().strftime("%Y%m%d_%H:%M") + "\n")
log(sceneinfo, "INFO")
sceneinfo += "# created with Phobos " + version + " - " + repository + "\n\n"
ioUtils.securepath(path)
outputfile.write(sceneinfo)
entitiesdict = roundFloatsInDict(
{'entities': entities},
ioUtils.getExpSettings().decimalPlaces)
outputfile.write(yaml.dump(entitiesdict))
def deriveEntity(root, outpath):
"""Derives the dictionary for a SMURF entity from the phobos model dictionary.
:param root: The smurf root object.
:type root: bpy.types.Object
:param outpath: The path to export the smurf to.
:type outpath: str
:param savetosubfolder: If True the export path has a subfolder for this smurf entity.
:type savetosubfolder: bool
:return: dict - An entry for the scenes entitiesList
"""
entitypose = models.deriveObjectPose(root)
entity = models.initObjectProperties(root, 'entity',
['link', 'joint', 'motor'])
if 'parent' not in entity and 'joint/type' in root and root[
'joint/type'] == 'fixed':
entity['parent'] = 'world'
entity["position"] = entitypose["translation"]
entity["rotation"] = entitypose["rotation_quaternion"]
# check model data if entity is a reference
# FIXME: this part is broken but not used at the moment anyways
if "isReference" in entity:
entity.pop("isReference")
bpy.ops.scene.reload_models_and_poses_operator()
modelsPosesColl = bpy.context.user_preferences.addons[
"phobos"].preferences.models_poses
for robot_model in modelsPosesColl:
if (root["modelname"]
== robot_model.robot_name) and (root["entity/pose"]
== robot_model.label):
pass
entity['file'] = os.path.join(
os.path.relpath(robot_model.path, outpath),
root["name"] + ".smurf")
'''
with open(os.path.join(os.path.dirname(defs.__file__), "RobotLib.yml"), "r") as f:
robots = yaml.load(f.read())
sourcepath = robots[smurf["modelname"]]
for filename in os.listdir(sourcepath):
fullpath = os.path.join(sourcepath, filename)
if os.path.isfile(fullpath):
shutil.copy2(fullpath, os.path.join(smurf_outpath, filename))
else:
# remove old folders to prevent errors in copytree
shutil.rmtree(os.path.join(smurf_outpath, filename), True)
shutil.copytree(fullpath, os.path.join(smurf_outpath, filename))
'''
else:
modelpath = os.path.join(outpath, root['modelname'])
if ioUtils.getExpSettings().structureExport:
modelpath = os.path.join(modelpath, 'smurf')
log("Scene paths: " + outpath + ' ' + modelpath, "DEBUG")
entity['file'] = os.path.join(
os.path.relpath(modelpath, os.path.dirname(outpath)),
root['modelname'] + ".smurf")
return entity
def execute(self, context):
exportlist = []
# TODO variable not used
exportsettings = ioUtils.getExpSettings()
# identify all entities' roots in the scene
entities = ioUtils.getExportEntities()
if not entities:
log("There are no entities to export!", "WARNING")
return {'CANCELLED'}
# derive entities and export if necessary
models = set()
for root in entities:
log("Adding entity '" + str(root["entity/name"]) + "' to scene.",
"INFO")
if root["entity/type"] in entity_types:
# TODO delete me?
# try:
if (self.exportModels
and 'export' in entity_types[root['entity/type']]
and root['modelname'] not in models):
modelpath = os.path.join(ioUtils.getExportPath(),
self.sceneName, root['modelname'])
# FIXME: the following is a hack, the problem is that
# robots are always smurf entities
if root['entity/type'] == 'smurf':
formatlist = ['smurf', 'urdf']
else:
formatlist = [root['entity/type']]
exportModel(root, modelpath, formatlist)
models.add(root['modelname'])
# known entity export
entity = entity_types[root["entity/type"]]['derive'](
root, os.path.join(ioUtils.getExportPath(),
self.sceneName))
# TODO delete me?
# except KeyError:
# log("Required method ""deriveEntity"" not implemented for type " + entity["entity/type"], "ERROR")
# continue
# generic entity export
else:
entity = deriveGenericEntity(root)
exportlist.append(entity)
for scenetype in scene_types:
typename = "export_scene_" + scenetype
# check if format exists and should be exported
if getattr(bpy.data.worlds[0], typename):
scene_types[scenetype]['export'](exportlist,
os.path.join(
ioUtils.getExportPath(),
self.sceneName))
return {'FINISHED'}
def execute(self, context):
roots = ioUtils.getExportModels()
if not roots:
log("No properly defined models selected or present in scene.",
'ERROR')
return {'CANCELLED'}
elif not self.exportall:
roots = [
root for root in roots if root['modelname'] == self.modelname
]
if len(roots) > 1:
log(
"Ambiguous model definitions: " + self.modelname +
" exists " + str(len(roots)) + " times.", "ERROR")
return {'CANCELLED'}
for root in roots:
# setup paths
exportpath = ioUtils.getExportPath()
if not securepath(exportpath):
log("Could not secure path to export to.", "ERROR")
continue
log("Export path: " + exportpath, "DEBUG")
ioUtils.exportModel(models.deriveModelDictionary(root), exportpath)
# select all exported models after export is done
if ioUtils.getExpSettings().selectedOnly:
for root in roots:
objectlist = sUtils.getChildren(root,
selected_only=True,
include_hidden=False)
sUtils.selectObjects(objectlist, clear=False)
else:
bpy.ops.object.select_all(action='DESELECT')
for root in roots:
sUtils.selectObjects(list([root]), False)
bpy.ops.phobos.select_model()
# report success to user
log("Export successful.", "INFO")
return {'FINISHED'}
def storePose(root, posename):
"""Stores the current pose of all of a model's selected joints.
Existing poses of the same name will be overwritten.
Args:
root(bpy_types.Object): root of the model the pose belongs to
posename(str): name the pose will be stored under
Returns:
: Nothing.
"""
if root:
filename = nUtils.getModelName(root) + '::poses'
posedict = yaml.load(bUtils.readTextFile(filename))
if not posedict:
posedict = {posename: {'name': posename, 'joints': {}}}
else:
posedict[posename] = {'name': posename, 'joints': {}}
links = sUtils.getChildren(root, ('link',), True, False)
sUtils.selectObjects([root] + links, clear=True, active=0)
bpy.ops.object.mode_set(mode='POSE')
for link in (
link
for link in links
if 'joint/type' in link and link['joint/type'] not in ['fixed', 'floating']
):
link.pose.bones['Bone'].rotation_mode = 'XYZ'
posedict[posename]['joints'][nUtils.getObjectName(link, 'joint')] = link.pose.bones[
'Bone'
].rotation_euler.y
bpy.ops.object.mode_set(mode='OBJECT')
posedict = gUtils.roundFloatsInDict(posedict, ioUtils.getExpSettings().decimalPlaces)
bUtils.updateTextFile(filename, yaml.dump(posedict, default_flow_style=False))
else:
log("No model root provided to store the pose for", "ERROR")
def execute(self, context):
"""
Args:
context:
Returns:
"""
roots = ioUtils.getExportModels()
if not roots:
log("No properly defined models selected or present in scene.", 'ERROR')
return {'CANCELLED'}
elif not self.exportall:
roots = [root for root in roots if nUtils.getModelName(root) == self.modelname]
if len(roots) > 1:
log(
"Ambiguous model definitions: "
+ self.modelname
+ " exists "
+ str(len(roots))
+ " times.",
"ERROR",
)
return {'CANCELLED'}
for root in roots:
# setup paths
exportpath = ioUtils.getExportPath()
if not securepath(exportpath):
log("Could not secure path to export to.", "ERROR")
continue
log("Export path: " + exportpath, "DEBUG")
ioUtils.exportModel(models.deriveModelDictionary(root), exportpath)
# select all exported models after export is done
if ioUtils.getExpSettings().selectedOnly:
for root in roots:
objectlist = sUtils.getChildren(root, selected_only=True, include_hidden=False)
sUtils.selectObjects(objectlist, clear=False)
else:
bpy.ops.object.select_all(action='DESELECT')
for root in roots:
sUtils.selectObjects(list([root]), False)
bpy.ops.phobos.select_model()
# TODO: Move mesh export to individual formats? This is practically SMURF
# export meshes in selected formats
# for meshtype in meshes.mesh_types:
# mesh_path = ioUtils.getOutputMeshpath(meshtype)
# try:
# typename = "export_mesh_" + meshtype
# if getattr(bpy.data.worlds[0], typename):
# securepath(mesh_path)
# for meshname in model['meshes']:
# meshes.mesh_types[meshtype]['export'](model['meshes'][meshname], mesh_path)
# except KeyError:
# log("No export function available for selected mesh function: " + meshtype,
# "ERROR", "ExportModelOperator")
# print(sys.exc_info()[0])
# TODO: Move texture export to individual formats? This is practically SMURF
# export textures
# if ioUtils.textureExportEnabled():
# texture_path = ''
# for materialname in model['materials']:
# mat = model['materials'][materialname]
# for texturetype in ['diffuseTexture', 'normalTexture', 'displacementTexture']:
# if texturetype in mat:
# texpath = os.path.join(os.path.expanduser(bpy.path.abspath('//')), mat[texturetype])
# if os.path.isfile(texpath):
# if texture_path == '':
# texture_path = securepath(os.path.join(export_path, 'textures'))
# log("Exporting textures to " + texture_path, "INFO", "ExportModelOperator")
# try:
# shutil.copy(texpath, os.path.join(texture_path, os.path.basename(mat[texturetype])))
# except shutil.SameFileError:
# log("{} already in place".format(texturetype), "INFO", "ExportModelOperator")
# report success to user
log("Export successful.", "INFO", end="\n\n")
return {'FINISHED'}
def exportModel(root, export_path, entitytypes=None, model=None):
# derive model
model = models.buildModelDictionary(root)
if not model:
model = models.buildModelDictionary(root)
# export model in selected formats
if entitytypes is None:
entitytypes = entities.entity_types
for entitytype in entitytypes:
typename = "export_entity_" + entitytype
# check if format exists and should be exported
if not getattr(bpy.data.worlds[0], typename, False):
continue
# format exists and is exported:
if ioUtils.getExpSettings().structureExport:
model_path = os.path.join(export_path, entitytype)
else:
model_path = export_path
securepath(model_path)
try:
entities.entity_types[entitytype]['export'](model, model_path)
log(
"Export model: " + model['name'] + ' as ' + entitytype +
" to " + model_path, "DEBUG")
except KeyError:
log(
"No export function available for selected model type: " +
entitytype, "ERROR")
continue
# TODO: Move mesh export to individual formats? This is practically SMURF
# export meshes in selected formats
i = 1
mt = len([
m for m in meshes.mesh_types
if getattr(bpy.data.worlds[0], "export_mesh_" + m)
])
mc = len(model['meshes'])
n = mt * mc
for meshtype in meshes.mesh_types:
mesh_path = ioUtils.getOutputMeshpath(export_path, meshtype)
try:
typename = "export_mesh_" + meshtype
if getattr(bpy.data.worlds[0], typename):
securepath(mesh_path)
for meshname in model['meshes']:
meshes.mesh_types[meshtype]['export'](
model['meshes'][meshname], mesh_path)
display.setProgress(
i / n,
'Exporting ' + meshname + '.' + meshtype + '...')
i += 1
except KeyError:
log(
"No export function available for selected mesh function: " +
meshtype, "ERROR")
print(sys.exc_info()[0])
display.setProgress(0)
# TODO: Move texture export to individual formats? This is practically SMURF
# TODO: Also, this does not properly take care of textures embedded in a .blend file
# export textures
if ioUtils.getExpSettings().exportTextures:
for materialname in model['materials']:
mat = model['materials'][materialname]
for texturetype in [
'diffuseTexture', 'normalTexture', 'displacementTexture'
]:
if texturetype in mat:
sourcepath = os.path.join(
os.path.expanduser(bpy.path.abspath('//')),
mat[texturetype])
if os.path.isfile(sourcepath):
texture_path = securepath(
os.path.join(export_path, 'textures'))
log("Exporting textures to " + texture_path, "INFO")
try:
shutil.copy(
sourcepath,
os.path.join(
texture_path,
os.path.basename(mat[texturetype])))
except shutil.SameFileError:
log("{} already in place".format(texturetype),
"INFO")
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 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
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 deriveModelDictionary(root, name='', objectlist=[]):
"""Derives 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.
:param root: root object of the model
:type root: bpy.types.Object
:param name: name for the derived model
:type name: str
:param objectlist: objects to derive the model from
:type objectlist: list of bpy.types.Object
:return: representation of the model based on the root object
:rtype: dict
"""
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')] = derive_link(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
# TODO what was this supposed to do?
# as it is only ever used by deriveSubmechanism we might want to move it...?
# combine inertia if certain objects are left out, and overwrite it
# inertials = (i for i in objectlist if i.phobostype == 'inertial' and '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
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, ignore_selection=bool(objectlist)))
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:
indep = [
nUtils.getObjectName(j, 'joint')
for j in link['submechanism/independent']
]
spann = [
nUtils.getObjectName(j, 'joint')
for j in link['submechanism/spanningtree']
]
active = [
nUtils.getObjectName(j, 'joint')
for j in link['submechanism/active']
]
submech = {
'type': link['submechanism/type'],
'contextual_name': link['submechanism/name'],
'jointnames_independent': indep,
'jointnames_spanningtree': spann,
'jointnames_active': 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
return roundFloatsInDict(model, ioUtils.getExpSettings().decimalPlaces)
def draw(self, context):
expsets = bpy.context.scene.phobosexportsettings
layout = self.layout
# export robot model options
layout.prop(expsets, "path")
ginlayout = self.layout.split()
g1 = ginlayout.column(align=True)
# FIXME remove this?
# g1.prop(expsets, "relativePaths")
g1.prop(expsets, "exportTextures")
g1.prop(expsets, "selectedOnly")
g2 = ginlayout.column(align=True)
g2.prop(expsets, "decimalPlaces")
layout.separator()
# Settings for mesh and entity export
inlayout = self.layout.split()
cmodel = inlayout.column(align=True)
cmodel.label(text="Models")
for entitytype in ioUtils.getEntityTypesForExport():
typename = "export_entity_" + entitytype
cmodel.prop(bpy.context.scene, typename)
cmesh = inlayout.column(align=True)
cmesh.label(text="Meshes")
for meshtype in sorted(meshes.mesh_types):
if 'export' in meshes.mesh_types[meshtype]:
typename = "export_mesh_" + meshtype
cmesh.prop(bpy.context.scene, typename)
cmesh.prop(bpy.context.scene.phobosexportsettings, 'outputMeshtype')
cscene = inlayout.column(align=True)
cscene.label(text="Scenes")
for scenetype in ioUtils.getSceneTypesForExport():
typename = "export_scene_" + scenetype
cscene.prop(bpy.context.scene, typename)
# additional obj parameters
if getattr(bpy.context.scene, 'export_mesh_obj', False):
layout.separator()
box = layout.box()
box.label('OBJ axis')
box.prop(ioUtils.getExpSettings(), 'obj_axis_forward')
box.prop(ioUtils.getExpSettings(), 'obj_axis_up')
# TODO delete me?
# c2.prop(expsets, "exportCustomData", text="Export custom data")
# TODO delete me?
# layout.separator()
# layout.label(text="Baking")
# layout.operator("phobos.export_bake", text="Bake Robot Model", icon="OUTLINER_OB_ARMATURE")
# layout.operator("phobos.create_robot_instance", text="Create Robot Lib Instance", icon="RENDERLAYERS")
# self.layout.prop(expsets, "heightmapMesh", text="export heightmap as mesh")
layout.separator()
layout.operator("phobos.export_model", icon="EXPORT")
layout.operator("phobos.export_scene", icon="WORLD_DATA")
def execute(self, context):
"""
Args:
context:
Returns:
"""
roots = ioUtils.getExportModels()
if not roots:
log("No properly defined models selected or present in scene.",
'ERROR')
return {'CANCELLED'}
elif not self.exportall:
roots = [
root for root in roots
if nUtils.getModelName(root) == self.modelname
]
if len(roots) > 1:
log(
"Ambiguous model definitions: " + self.modelname +
" exists " + str(len(roots)) + " times.",
"ERROR",
)
return {'CANCELLED'}
for root in roots:
# setup paths
exportpath = ioUtils.getExportPath()
if not securepath(exportpath):
log("Could not secure path to export to.", "ERROR")
continue
log("Export path: " + exportpath, "DEBUG")
ioUtils.exportModel(models.deriveModelDictionary(root), exportpath)
# select all exported models after export is done
if ioUtils.getExpSettings().selectedOnly:
for root in roots:
objectlist = sUtils.getChildren(root,
selected_only=True,
include_hidden=False)
sUtils.selectObjects(objectlist, clear=False)
else:
bpy.ops.object.select_all(action='DESELECT')
for root in roots:
sUtils.selectObjects(list([root]), False)
bpy.ops.phobos.select_model()
# TODO: Move mesh export to individual formats? This is practically SMURF
# export meshes in selected formats
# for meshtype in meshes.mesh_types:
# mesh_path = ioUtils.getOutputMeshpath(meshtype)
# try:
# typename = "export_mesh_" + meshtype
# if getattr(bpy.data.worlds[0], typename):
# securepath(mesh_path)
# for meshname in model['meshes']:
# meshes.mesh_types[meshtype]['export'](model['meshes'][meshname], mesh_path)
# except KeyError:
# log("No export function available for selected mesh function: " + meshtype,
# "ERROR", "ExportModelOperator")
# print(sys.exc_info()[0])
# TODO: Move texture export to individual formats? This is practically SMURF
# export textures
# if ioUtils.textureExportEnabled():
# texture_path = ''
# for materialname in model['materials']:
# mat = model['materials'][materialname]
# for texturetype in ['diffuseTexture', 'normalTexture', 'displacementTexture']:
# if texturetype in mat:
# texpath = os.path.join(os.path.expanduser(bpy.path.abspath('//')), mat[texturetype])
# if os.path.isfile(texpath):
# if texture_path == '':
# texture_path = securepath(os.path.join(export_path, 'textures'))
# log("Exporting textures to " + texture_path, "INFO", "ExportModelOperator")
# try:
# shutil.copy(texpath, os.path.join(texture_path, os.path.basename(mat[texturetype])))
# except shutil.SameFileError:
# log("{} already in place".format(texturetype), "INFO", "ExportModelOperator")
# report success to user
log("Export successful.", "INFO", end="\n\n")
return {'FINISHED'}
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, optional): name for the derived model (Default value = '')
objectlist(list: bpy_types.Object): objects to derive the model from
objectlist: (Default value = [])
Returns:
"""
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 'model/name' in root:
modelname = root['model/name']
else:
modelname = 'unnamed'
# define model version
if 'model/version' in root:
modelversion = root['model/version']
else:
modelversion = 'undefined'
modeldescription = bUtils.readTextFile('README.md')
model = {
'links': {},
'joints': {},
'sensors': {},
'motors': {},
'controllers': {},
'materials': {},
'meshes': {},
'lights': {},
'groups': {},
'chains': {},
'date': datetime.now().strftime("%Y%m%d_%H:%M"),
'name': modelname,
'version': modelversion,
'description': modeldescription,
}
log(
"Creating dictionary for model '" + modelname + "' with root '" + root.name + "'.",
'INFO',
prefix="\n",
)
# 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, logging=True, objectlist=objectlist
)
# parse joint and motor information
if sUtils.getEffectiveParent(link):
# joint may be None if link is a root
# to prevent confusion links are always defining also joints
jointdict = deriveJoint(link, logging=True, adjust=True)
log(" Setting joint type '{}' for link.".format(jointdict['type']), 'DEBUG')
# first check if we have motor information in the joint properties
# if so they can be extended/overwritten by motor objects later on
if '$motor' in jointdict:
motordict = jointdict['$motor']
# at least we need a type property
if 'type' in motordict:
# if no name is given derive it from the joint
if not 'name' in motordict:
motordict["name"] = jointdict['name']
model['motors'][motordict['name']] = motordict
# link the joint by name:
motordict['joint'] = jointdict['name']
del jointdict['$motor']
model['joints'][jointdict['name']] = jointdict
for mot in [child for child in link.children if child.phobostype == 'motor']:
motordict = motormodel.deriveMotor(mot, jointdict)
# motor may be None if no motor is attached
if motordict:
log(" Added motor {} to link.".format(motordict['name']), 'DEBUG')
if motordict['name'] in model["motors"]:
model['motors'][motordict['name']].update(motordict)
else:
model['motors'][motordict['name']] = motordict
# parse sensors and controllers
sencons = [obj for obj in objectlist if obj.phobostype in ['sensor', 'controller']]
log("Parsing sensors and controllers... {} total.".format(len(sencons)), 'INFO')
for obj in sencons:
props = deriveDictEntry(obj, names=True, objectlist=objectlist)
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
#todo2.9: 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')
#todo2.9: 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):
"""
Args:
link:
Returns:
"""
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 to {} digits.".format(ioUtils.getExpSettings().decimalPlaces), 'INFO')
model = roundFloatsInDict(model, ioUtils.getExpSettings().decimalPlaces)
log("Sorting objects.", 'DEBUG')
model = sortListsInDict(model)
return model
def fuse_inertia_data(inertials):
"""Computes combined mass, center of mass and inertia given a list of inertial objects.
Computation based on Modern Robotics, Lynch & Park, p. 287 .
If no inertials are found (None, None, None) is returned.
If successful, the tuple contains this information:
*mass*: float
*com*: mathutils.Vector(3)
*inertia*: mathutils.Matrix(3)
Args:
inertials(list): the alist of objects relevant for the inertia of a link
Returns:
3: tuple of mass, COM and inertia or None(3) if no inertials are found
"""
from phobos.utils.io import getExpSettings
expsetting = 10**(-getExpSettings().decimalPlaces)
# Find objects who have some inertial data
for obj in inertials:
if not any([True
for key in obj.keys() if key.startswith('inertial/')]):
inertials.remove(obj)
# Check for an empty list -> No inertials to fuse
if not inertials:
return 1e-3, [0.0, 0.0, 0.0], numpy.diag([1e-3, 1e-3, 1e-3])
fused_inertia = numpy.zeros((3, 3))
fused_com = numpy.zeros((1, 3))
fused_mass = 0.0
# Calculate the fused mass and center of mass
fused_mass, fused_com = combine_com_3x3(inertials)
# Check for conformity
if fused_mass <= expsetting:
log(" Correcting fused mass : negative semidefinite value.", 'WARNING')
fused_mass = expsetting if fused_mass < expsetting else fused_mass
# TODO Maybe we can reuse the functions defined here.
# Calculate the fused inertias
for obj in inertials:
# Get the rotation of the inertia
current_Rotation = numpy.array(obj.matrix_local.to_3x3())
current_Inertia = numpy.array(
inertiaListToMatrix(obj['inertial/inertia']))
# Rotate the inertia into the current frame
current_Inertia = numpy.dot(
numpy.dot(current_Rotation.T, current_Inertia), current_Rotation)
# Move the inertia to the center of mass
# Get the current relative position of the center of mass
relative_position = numpy.array(
obj.matrix_local.translation) - fused_com
# Calculate the translational influence
current_Inertia += obj['inertial/mass'] * (
relative_position.T * relative_position * numpy.eye(3) -
numpy.outer(relative_position, relative_position))
fused_inertia += numpy.dot(
numpy.dot(current_Rotation.T, current_Inertia), current_Rotation)
# Check the inertia
if any(element <= expsetting for element in fused_inertia.diagonal()):
log(
" Correting fused inertia : negative semidefinite diagonal entries.",
'WARNING')
for i in range(3):
fused_inertia[i, i] = expsetting if fused_inertia[
i, i] <= expsetting else fused_inertia[i, i]
if any(element <= expsetting
for element in numpy.linalg.eigvals(fused_inertia)):
log(" Correcting fused inertia : negative semidefinite eigenvalues",
'WARNING')
S, V = numpy.linalg.eig(fused_inertia)
S[S <= expsetting] = expsetting
fused_inertia = V.dot(numpy.diag(S).dot(V.T))
# Add minimum value for the inertia
for i in range(3):
fused_inertia[i, i] += expsetting if fused_inertia[
i, i] <= expsetting else fused_inertia[i, i]
return fused_mass, fused_com, fused_inertia
