def deriveSensor(obj, names=False, objectlist=[], logging=False):
"""This function derives a sensor from a given blender object
Args:
obj(bpy_types.Object): The blender object to derive the sensor from.
names(bool, optional): return the link object name instead of an object link. (Default value = False)
objectlist(list(bpy.types.Object, optional): objectlist to which possible parents are restricted (Default value = [])
logging(bool, optional): whether to write log messages or not (Default value = False)
Returns:
: dict -- phobos representation of the sensor
"""
from phobos.model.models import initObjectProperties
if logging:
log(
"Deriving sensor from object " + nUtils.getObjectName(obj, phobostype='sensor') + ".",
'DEBUG',
)
try:
props = initObjectProperties(obj, phobostype='sensor', ignoretypes=('pose'))
if names:
props['link'] = nUtils.getObjectName(
sUtils.getEffectiveParent(obj, objectlist=objectlist), phobostype='link'
)
else:
props['link'] = sUtils.getEffectiveParent(obj, objectlist=objectlist)
except KeyError:
if logging:
log("Missing data in sensor " + obj.name, "ERROR")
return None
return props
def deriveObjectPose(obj):
"""Derives a pose of link, visual or collision object.
The transformations of the object are calculated according to
phobos.utils.edititing.getCombinedTransform.
The returned dictionary contains this information:
*rawmatrix*: mathutils.Matrix
*matrix*: list representation (list of lists) of mathutils.Matrix
*translation*: list (according to mathutils.Matrix.to_translation)
*rotation_euler*: list (according to mathutils.Matrix.to_euler)
*rotation_quaternion*: list (according to mathutils.Matrix.to_quaternion)
:param obj: blender object to derive the pose from
:type obj: bpy.types.Object
:return: pose information of the object
:rtype: dict
.. seealso phobos.utils.editing.getCombinedTransform
"""
effectiveparent = sUtils.getEffectiveParent(obj)
matrix = eUtils.getCombinedTransform(obj, effectiveparent)
pose = {
'rawmatrix': matrix,
'matrix': [list(vector) for vector in list(matrix)],
'translation': list(matrix.to_translation()),
'rotation_euler': list(matrix.to_euler()),
'rotation_quaternion': list(matrix.to_quaternion())
}
return pose
def derive_link(linkobj, inertialobjs=None):
"""Derives a link from a blender object and creates its initial phobos data structure.
If inertialobjs are provided, the inertia will be calculated from the specified list instead
of the getInertiaChildren.
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
:param linkobj: blender object to derive the link from.
:type linkobj: bpy.types.Object
:param inertialobjs: override the link inertial objects
:type inertialobjs: list of bpy.types.Object
:return: representation of the link
:rtype: dict
.. seealso deriveObjectPose
.. seealso deriveInertiaChildren
.. seealso derive_inertia
"""
assert linkobj.phobostype == 'link', ("Wrong phobostype: " +
linkobj.phobostype +
" instead of link.")
log("Deriving link from object " + linkobj.name + ".", 'DEBUG')
props = initObjectProperties(linkobj,
phobostype='link',
ignoretypes=linkobjignoretypes - {'link'})
parent = sUtils.getEffectiveParent(linkobj)
props['parent'] = nUtils.getObjectName(parent) if parent else None
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'] = []
if not inertialobjs:
inertialobjs = inertiamodel.getInertiaChildren(linkobj)
log(" Deriving inertial...", 'DEBUG')
# add inertial information to link
if inertialobjs:
props['inertial'] = derive_inertia(inertialobjs)
else:
log(
"No valid inertial data for link " + props['name'] + ". " +
str(len(inertialobjs)) + " inertial objects selected.", 'WARNING')
return props
def deriveLight(obj):
"""This function derives a light from a given blender object
:param obj: The blender object to derive the light from.
:type obj: bpy_types.Object
:return: tuple
"""
light = initObjectProperties(obj, phobostype='light')
light_data = obj.data
if light_data.use_diffuse:
light['color_diffuse'] = list(light_data.color)
if light_data.use_specular:
light['color_specular'] = copy.copy(light['color_diffuse'])
light['type'] = light_data.type.lower()
if light['type'] == 'SPOT':
light['size'] = light_data.size
pose = deriveObjectPose(obj)
light['position'] = pose['translation']
light['rotation'] = pose['rotation_euler']
try:
light['attenuation_linear'] = float(light_data.linear_attenuation)
except AttributeError:
pass
try:
light['attenuation_quadratic'] = float(light_data.quadratic_attenuation)
except AttributeError:
pass
if light_data.energy:
light['attenuation_constant'] = float(light_data.energy)
light['parent'] = nUtils.getObjectName(sUtils.getEffectiveParent(obj))
return light
def deriveLight(obj):
"""This function derives a light from a given blender object
:param obj: The blender object to derive the light from.
:type obj: bpy_types.Object
:return: tuple
"""
light = initObjectProperties(obj, phobostype='light')
light_data = obj.data
if light_data.use_diffuse:
light['color_diffuse'] = list(light_data.color)
if light_data.use_specular:
light['color_specular'] = copy.copy(light['color_diffuse'])
light['type'] = light_data.type.lower()
if light['type'] == 'SPOT':
light['size'] = light_data.size
pose = deriveObjectPose(obj)
light['position'] = pose['translation']
light['rotation'] = pose['rotation_euler']
try:
light['attenuation_linear'] = float(light_data.linear_attenuation)
except AttributeError:
# TODO handle this somehow
pass
try:
light['attenuation_quadratic'] = float(
light_data.quadratic_attenuation)
except AttributeError:
pass
if light_data.energy:
light['attenuation_constant'] = float(light_data.energy)
light['parent'] = nUtils.getObjectName(sUtils.getEffectiveParent(obj))
return light
def get_link_information(linkobj):
"""Returns the full link information including joint and motor data from a blender object.
The link information is derived according to :func:`derive_link`.
Args:
linkobj(bpy.types.Object): blender object to derive the link from
Returns:
dict: link representation of the object
"""
props = initObjectProperties(linkobj,
phobostype='link',
ignoretypes=['joint', 'motor', 'entity'])
parent = sUtils.getEffectiveParent(linkobj)
props['parent'] = parent.name if parent else None
props['pose'] = deriveObjectPose(linkobj)
props['joint'] = deriveJoint(linkobj, logging=False, adjust=False)
del props['joint']['parent']
# collect collision objs for link
collisionobjs = sUtils.getImmediateChildren(linkobj,
phobostypes=('collision'),
include_hidden=True)
collisiondict = {}
for colobj in collisionobjs:
collisiondict[colobj.name] = colobj
props['collision'] = collisiondict
# collect visual objs for link
visualobjects = sUtils.getImmediateChildren(linkobj,
phobostypes=('visual'),
include_hidden=True)
visualdict = {}
for visualobj in visualobjects:
visualdict[visualobj.name] = visualobj
props["visual"] = visualdict
# collect inertial objects
inertialdict = {
nUtils.getObjectName(obj): obj
for obj in linkobj.children if obj.phobostype == 'inertial'
}
props["inertial"] = inertialdict
# collect sensor objects
sensorobjects = sUtils.getImmediateChildren(linkobj,
phobostypes=('sensor'),
include_hidden=True)
sensordict = {}
for sensorobj in sensorobjects:
sensordict[sensorobj.name] = sensorobj
if sensordict:
props["sensor"] = sensordict
props['approxcollision'] = []
return props
def createInertial(inertialdict,
obj,
size=0.03,
errors=None,
adjust=False,
logging=False):
"""Creates the Blender representation of a given inertial provided a dictionary.
Args:
inertialdict(dict): intertial data
obj:
size: (Default value = 0.03)
errors: (Default value = None)
adjust: (Default value = False)
logging: (Default value = False)
Returns:
: bpy_types.Object -- newly created blender inertial object
"""
if errors and not adjust:
log('Can not create inertial object.', 'ERROR')
try:
origin = mathutils.Vector(inertialdict['pose']['translation'])
except KeyError:
origin = mathutils.Vector()
# create new inertial object
name = nUtils.getUniqueName('inertial_' + nUtils.getObjectName(obj),
bpy.data.objects)
inertialobject = bUtils.createPrimitive(
name,
'box',
(size, ) * 3,
defs.layerTypes["inertial"],
pmaterial='phobos_inertial',
phobostype='inertial',
)
sUtils.selectObjects((inertialobject, ), clear=True, active=0)
bpy.ops.object.transform_apply(scale=True)
# set position according to the parent link
inertialobject.matrix_world = obj.matrix_world
parent = obj
if parent.phobostype != 'link':
parent = sUtils.getEffectiveParent(obj, ignore_selection=True)
eUtils.parentObjectsTo(inertialobject, parent)
# position and parent the inertial object relative to the link
# inertialobject.matrix_local = mathutils.Matrix.Translation(origin)
sUtils.selectObjects((inertialobject, ), clear=True, active=0)
# bpy.ops.object.transform_apply(scale=True)
# add properties to the object
for prop in ('mass', 'inertia'):
inertialobject['inertial/' + prop] = inertialdict[prop]
return inertialobject
def get_link_information(linkobj):
"""Returns the full link information including joint and motor data from a blender object.
:param linkobj: blender object to derive the link from
:type linkobj: bpy.types.Object
:return: representation of the link including motor and joint data
:rtype: dict
.. seealso:: derive_link
"""
assert linkobj.phobostype == 'link', ("Wrong phobostype: " +
linkobj.phobostype +
" instead of link.")
props = initObjectProperties(linkobj,
phobostype='link',
ignoretypes=['joint', 'motor', 'entity'])
parent = sUtils.getEffectiveParent(linkobj)
props['parent'] = parent.name if parent else None
props['pose'] = deriveObjectPose(linkobj)
props['joint'] = deriveJoint(linkobj, adjust=False)
del props['joint']['parent']
# derive Motor
if any(item.startswith('motor') for item in props):
props['motor'] = deriveMotor(linkobj, props['joint'])
# collect collision objs for link
collisionobjs = sUtils.getImmediateChildren(linkobj,
phobostypes=('collision'),
include_hidden=True)
collisiondict = {}
for colobj in collisionobjs:
collisiondict[colobj.name] = colobj
props['collision'] = collisiondict
# collect visual objs for link
visualobjects = sUtils.getImmediateChildren(linkobj,
phobostypes=('visual'),
include_hidden=True)
visualdict = {}
for visualobj in visualobjects:
visualdict[visualobj.name] = visualobj
props["visual"] = visualdict
# collect inertial objects
inertialobjects = inertiamodel.getInertiaChildren(linkobj)
inertialdict = {}
for inertialobj in inertialobjects:
inertialdict[inertialobj.name] = inertialobj
props["inertial"] = inertialdict
props['approxcollision'] = []
return props
def createInertial(inertialdict, obj, size=0.03, errors=None, adjust=False, logging=False):
"""Creates the Blender representation of a given inertial provided a dictionary.
Args:
inertialdict(dict): intertial data
obj:
size: (Default value = 0.03)
errors: (Default value = None)
adjust: (Default value = False)
logging: (Default value = False)
Returns:
: bpy_types.Object -- newly created blender inertial object
"""
if errors and not adjust:
log('Can not create inertial object.', 'ERROR')
try:
origin = mathutils.Vector(inertialdict['pose']['translation'])
except KeyError:
origin = mathutils.Vector()
# create new inertial object
name = nUtils.getUniqueName('inertial_' + nUtils.getObjectName(obj), bpy.data.objects)
inertialobject = bUtils.createPrimitive(
name,
'box',
(size,) * 3,
defs.layerTypes["inertial"],
pmaterial='phobos_inertial',
phobostype='inertial',
)
sUtils.selectObjects((inertialobject,), clear=True, active=0)
bpy.ops.object.transform_apply(scale=True)
# set position according to the parent link
inertialobject.matrix_world = obj.matrix_world
parent = obj
if parent.phobostype != 'link':
parent = sUtils.getEffectiveParent(obj, ignore_selection=True)
eUtils.parentObjectsTo(inertialobject, parent)
# position and parent the inertial object relative to the link
# inertialobject.matrix_local = mathutils.Matrix.Translation(origin)
sUtils.selectObjects((inertialobject,), clear=True, active=0)
# bpy.ops.object.transform_apply(scale=True)
# add properties to the object
for prop in ('mass', 'inertia'):
inertialobject['inertial/' + prop] = inertialdict[prop]
return inertialobject
def deriveSensor(obj):
"""This function derives a sensor from a given blender object
:param obj: The blender object to derive the sensor from.
:type obj: bpy_types.Object
:return: dict
"""
try:
props = initObjectProperties(obj, phobostype='sensor')
props['link'] = nUtils.getObjectName(sUtils.getEffectiveParent(obj))
except KeyError:
log("Missing data in sensor " + obj.name, "ERROR", "deriveSensor")
return None
return props
def deriveSensor(obj):
"""This function derives a sensor from a given blender object
:param obj: The blender object to derive the sensor from.
:type obj: bpy_types.Object
:return: dict
"""
try:
props = initObjectProperties(obj, phobostype='sensor')
props['link'] = nUtils.getObjectName(sUtils.getEffectiveParent(obj))
except KeyError:
log("Missing data in sensor " + obj.name, "ERROR")
return None
return props
def deriveLink(linkobj, objectlist=[]):
"""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
"""
if not linkobj.phobostype == 'link':
log(
"Could not parse link from {0}. No valid link object.".format(
linkobj.name), 'ERROR')
return None
if not objectlist:
objectlist = list(bpy.data.objects)
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'] = []
return props
def deriveSensor(obj, names=False, objectlist=[], logging=False):
"""This function derives a sensor from a given blender object
Args:
obj(bpy_types.Object): The blender object to derive the sensor from.
names(bool, optional): return the l object name instead of an object link. (Default value = False)
objectlist(list(bpy.types.Object, optional): objectlist to which possible parents are restricted (Default value = [])
logging(bool, optional): whether to write log messages or not (Default value = False)
Returns:
: dict -- phobos representation of the sensor
"""
from phobos.model.models import initObjectProperties
from phobos.model.poses import deriveObjectPose
if logging:
log(
"Deriving sensor from object " + nUtils.getObjectName(obj, phobostype='sensor') + ".",
'DEBUG',
)
try:
props = initObjectProperties(obj, phobostype='sensor', ignoretypes=('pose'))
if names:
props['link'] = nUtils.getObjectName(
sUtils.getEffectiveParent(obj, objectlist=objectlist), phobostype='link'
)
else:
props['link'] = sUtils.getEffectiveParent(obj, objectlist=objectlist)
except KeyError:
if logging:
log("Missing data in sensor " + obj.name, "ERROR")
return None
props['pose'] = deriveObjectPose(obj)
return props
def deriveJoint(obj, logging=False, adjust=False, errors=None):
"""Derives a joint from a blender object and creates its initial phobos data structure.
Args:
obj(bpy.types.Object): object to derive the joint from
adjust(bool, optional): TODO (Default value = False)
logging: (Default value = False)
errors: (Default value = None)
Returns:
: dict
"""
joint_type, crot = jointmodel.deriveJointType(obj,
adjust=adjust,
logging=logging)
props = initObjectProperties(obj,
phobostype='joint',
ignoretypes=linkobjignoretypes - {'joint'})
parent = sUtils.getEffectiveParent(obj)
props['parent'] = nUtils.getObjectName(parent)
props['child'] = nUtils.getObjectName(obj)
axis, minmax = jointmodel.getJointConstraints(obj)
if axis:
props['axis'] = list(axis)
limits = {}
if minmax is not None:
# prismatic or revolute joint, TODO: planar etc.
if len(minmax) == 2:
limits['lower'] = minmax[0]
limits['upper'] = minmax[1]
if 'maxvelocity' in props:
limits['velocity'] = props['maxvelocity']
del props['maxvelocity']
if 'maxeffort' in props:
limits['effort'] = props['maxeffort']
del props['maxeffort']
if limits != {}:
props['limits'] = limits
props['pose'] = deriveObjectPose(obj)
# TODO: what about these?
# - calibration
# - dynamics
# - mimic
# - safety_controller
return props
def deriveObjectPose(obj):
"""Derives a pose of link, visual or collision object.
:param obj: The blender object to derive the pose from.
:type obj: bpy_types.Object
:return: dict
"""
effectiveparent = sUtils.getEffectiveParent(obj)
matrix = eUtils.getCombinedTransform(obj, effectiveparent)
pose = {
'rawmatrix': matrix,
'matrix': [list(vector) for vector in list(matrix)],
'translation': list(matrix.to_translation()),
'rotation_euler': list(matrix.to_euler()),
'rotation_quaternion': list(matrix.to_quaternion())
}
return pose
def deriveLink(obj):
"""This function derives a link from a blender object and creates its initial phobos data structure.
:param obj: The blender object to derive the link from.
:type obj: bpy_types.Object
:return: dict
"""
props = initObjectProperties(obj, phobostype='link', ignoretypes=['joint', 'motor', 'entity'])
parent = sUtils.getEffectiveParent(obj)
props['parent'] = parent.name if parent else None
props["pose"] = deriveObjectPose(obj)
props["collision"] = {}
props["visual"] = {}
props["inertial"] = {}
props['approxcollision'] = []
return props
def deriveLink(obj):
"""This function derives a link from a blender object and creates its initial phobos data structure.
:param obj: The blender object to derive the link from.
:type obj: bpy_types.Object
:return: dict
"""
props = initObjectProperties(obj,
phobostype='link',
ignoretypes=['joint', 'motor', 'entity'])
parent = sUtils.getEffectiveParent(obj)
props['parent'] = parent.name if parent else None
props["pose"] = deriveObjectPose(obj)
props["collision"] = {}
props["visual"] = {}
props["inertial"] = {}
props['approxcollision'] = []
return props
def deriveJoint(obj, adjust=True):
"""This function derives a joint from a blender object and creates its initial phobos data structure.
Args:
obj: The blender object to derive the joint from.
adjust: (Default value = True)
Returns:
dict
"""
if 'joint/type' not in obj.keys():
jt, crot = jointmodel.deriveJointType(obj, adjust=adjust)
props = initObjectProperties(obj,
phobostype='joint',
ignoretypes=linkobjignoretypes - {'joint'})
parent = sUtils.getEffectiveParent(obj)
props['parent'] = nUtils.getObjectName(parent)
props['child'] = nUtils.getObjectName(obj)
axis, minmax = jointmodel.getJointConstraints(obj)
if axis:
props['axis'] = list(axis)
limits = {}
if minmax is not None:
# prismatic or revolute joint, TODO: planar etc.
if len(minmax) == 2:
limits['lower'] = minmax[0]
limits['upper'] = minmax[1]
if 'maxvelocity' in props:
limits['velocity'] = props['maxvelocity']
del props['maxvelocity']
if 'maxeffort' in props:
limits['effort'] = props['maxeffort']
del props['maxeffort']
if limits != {}:
props['limits'] = limits
# TODO: what about these?
# - calibration
# - dynamics
# - mimic
# - safety_controller
return props
def deriveObjectPose(obj, logging=False, adjust=False, errors=None):
"""Derives a pose of link, visual or collision object.
The transformations of the object are calculated according to
phobos.utils.edititing.getCombinedTransform.
The returned dictionary contains this information:
*rawmatrix*: mathutils.Matrix
*matrix*: list representation (list of lists) of mathutils.Matrix
*translation*: list (according to mathutils.Matrix.to_translation)
*rotation_euler*: list (according to mathutils.Matrix.to_euler)
*rotation_quaternion*: list (according to mathutils.Matrix.to_quaternion)
Args:
obj(bpy.types.Object): blender object to derive the pose from
logging: (Default value = False)
errors: (Default value = None)
adjust: (Default value = False)
Returns:
: dict
.. seealso phobos.utils.editing.getCombinedTransform: pose information of the object
"""
effectiveparent = sUtils.getEffectiveParent(obj)
matrix = eUtils.getCombinedTransform(obj, effectiveparent)
pose = {
'rawmatrix': matrix,
'matrix': [list(vector) for vector in list(matrix)],
'translation': list(matrix.to_translation()),
'rotation_euler': list(matrix.to_euler()),
'rotation_quaternion': list(matrix.to_quaternion()),
}
if logging:
log(
"Location: " + str(pose['translation']) + " Rotation: " +
str(pose['rotation_euler']),
'DEBUG',
)
return pose
def deriveFullLinkInformation(obj):
"""This function derives the full link information (including joint and
motor data) from a blender object and creates its initial phobos data
structure.
:param obj: The blender object to derive the link from.
:type obj: bpy_types.Object
:return: dict
"""
props = initObjectProperties(obj,
phobostype='link',
ignoretypes=['joint', 'motor', 'entity'])
parent = sUtils.getEffectiveParent(obj)
props['parent'] = parent.name if parent else None
props["pose"] = deriveObjectPose(obj)
props["joint"] = deriveJoint(obj, adjust=False)
del props["joint"]["parent"]
if any(item.startswith('motor') for item in props.keys()):
props["motor"] = deriveMotor(obj, props['joint'])
collisionObjects = sUtils.getImmediateChildren(obj,
phobostypes=('collision'),
include_hidden=True)
collisionDict = {}
for colobj in collisionObjects:
collisionDict[colobj.name] = colobj
props["collision"] = collisionDict
visualObjects = sUtils.getImmediateChildren(obj,
phobostypes=('visual'),
include_hidden=True)
visualDict = {}
for visualobj in visualObjects:
visualDict[visualobj.name] = visualobj
props["visual"] = visualDict
inertialObjects = sUtils.getImmediateChildren(obj,
phobostypes=('inertial'),
include_hidden=True)
inertialDict = {}
for inertialobj in inertialObjects:
inertialDict[inertialobj.name] = inertialobj
props["inertial"] = inertialDict
props['approxcollision'] = []
return props
def deriveObjectPose(obj, logging=False, adjust=False, errors=None):
"""Derives a pose of link, visual or collision object.
The transformations of the object are calculated according to
phobos.utils.edititing.getCombinedTransform.
The returned dictionary contains this information:
*rawmatrix*: mathutils.Matrix
*matrix*: list representation (list of lists) of mathutils.Matrix
*translation*: list (according to mathutils.Matrix.to_translation)
*rotation_euler*: list (according to mathutils.Matrix.to_euler)
*rotation_quaternion*: list (according to mathutils.Matrix.to_quaternion)
Args:
obj(bpy.types.Object): blender object to derive the pose from
logging: (Default value = False)
errors: (Default value = None)
adjust: (Default value = False)
Returns:
: dict
.. seealso phobos.utils.editing.getCombinedTransform: pose information of the object
"""
effectiveparent = sUtils.getEffectiveParent(obj)
matrix = eUtils.getCombinedTransform(obj, effectiveparent)
pose = {
'rawmatrix': matrix,
'matrix': [list(vector) for vector in list(matrix)],
'translation': list(matrix.to_translation()),
'rotation_euler': list(matrix.to_euler()),
'rotation_quaternion': list(matrix.to_quaternion()),
}
if logging:
log(
"Location: " + str(pose['translation']) + " Rotation: " + str(pose['rotation_euler']),
'DEBUG',
)
return pose
def deriveObjectPose(obj):
"""This function derives a pose of link, visual or collision object.
:param obj: The blender object to derive the pose from.
:type obj: bpy_types.Object
:return: dict
"""
matrix = obj.matrix_local
effectiveparent = sUtils.getEffectiveParent(obj)
parent = obj.parent
while parent != effectiveparent and parent is not None:
matrix = parent.matrix_local * matrix
parent = parent.parent
pose = {'rawmatrix': matrix,
'matrix': [list(vector) for vector in list(matrix)],
'translation': list(matrix.to_translation()),
'rotation_euler': list(matrix.to_euler()),
'rotation_quaternion': list(matrix.to_quaternion())
}
return pose
def deriveKinematics(obj):
"""This function takes an object and derives a link, joint and motor from it, if possible.
:param obj: The object to derive its kinematics from.
:type obj: bpy_types.Object
:return: tuple
"""
link = deriveLink(obj)
joint = None
motor = None
# joints and motors of root elements are only relevant for scenes, not within models
if sUtils.getEffectiveParent(obj):
# TODO: here we have to identify root joints and write their properties to SMURF!
# --> namespacing parent = "blub::blublink1"
# --> how to mark separate smurfs in phobos (simply modelname?)
# -> cut models in pieces but adding modelnames
# -> automatic namespacing
joint = deriveJoint(obj)
motor = deriveMotor(obj, joint)
return link, joint, motor
def deriveKinematics(obj):
"""This function takes an object and derives a link, joint and motor from it, if possible.
:param obj: The object to derive its kinematics from.
:type obj: bpy_types.Object
:return: tuple
"""
link = deriveLink(obj)
joint = None
motor = None
# joints and motors of root elements are only relevant for scenes, not within models
if sUtils.getEffectiveParent(obj):
# TODO: here we have to identify root joints and write their properties to SMURF!
# --> namespacing parent = "blub::blublink1"
# --> how to mark separate smurfs in phobos (simply modelname?)
# -> cut models in pieces but adding modelnames
# -> automatic namespacing
joint = deriveJoint(obj)
motor = deriveMotor(obj, joint)
return link, joint, motor
def deriveObjectPose(obj):
"""This function derives a pose of link, visual or collision object.
:param obj: The blender object to derive the pose from.
:type obj: bpy_types.Object
:return: dict
"""
matrix = obj.matrix_local
effectiveparent = sUtils.getEffectiveParent(obj)
parent = obj.parent
while parent != effectiveparent and parent is not None:
matrix = parent.matrix_local * matrix
parent = parent.parent
pose = {
'rawmatrix': matrix,
'matrix': [list(vector) for vector in list(matrix)],
'translation': list(matrix.to_translation()),
'rotation_euler': list(matrix.to_euler()),
'rotation_quaternion': list(matrix.to_quaternion())
}
return pose
def deriveJoint(obj):
"""This function derives a joint from a blender object and creates its initial phobos data structure.
:param obj: The blender object to derive the joint from.
:return: dict
"""
if 'joint/type' not in obj.keys():
jt, crot = joints.deriveJointType(obj, adjust=True)
props = initObjectProperties(obj, phobostype='joint', ignoretypes=['link', 'motor', 'entity'])
parent = sUtils.getEffectiveParent(obj)
props['parent'] = nUtils.getObjectName(parent)
props['child'] = nUtils.getObjectName(obj)
axis, minmax = joints.getJointConstraints(obj)
if axis:
props['axis'] = list(axis)
limits = {}
if minmax is not None:
if len(minmax) == 2: # prismatic or revolute joint, TODO: planar etc.
limits['lower'] = minmax[0]
limits['upper'] = minmax[1]
if 'maxvelocity' in props:
limits['velocity'] = props['maxvelocity']
del props['maxvelocity']
if 'maxeffort' in props:
limits['effort'] = props['maxeffort']
del props['maxeffort']
if limits != {}:
props['limits'] = limits
# TODO:
# - calibration
# - dynamics
# - mimic
# - safety_controller
return props
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 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 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 buildModelDictionary(root):
"""Builds a python dictionary representation of a SMURF model for export and inspection.
:param root: bpy.types.objects
:return: dict
"""
#os.system('clear')
robot = {'links': {},
'joints': {},
'sensors': {},
'motors': {},
'controllers': {},
'materials': {},
'lights': {},
'groups': {},
'chains': {}
}
# timestamp of model
robot["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:
robot['modelname'] = root["modelname"]
else:
log("No name for the model defines, setting to 'unnamed_model'", "WARNING", "buildModelDictionary")
robot['modelname'] = 'unnamed_model'
log("Creating dictionary for robot " + robot['modelname'] + " 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)
robot['links'][linkdict['name']] = linkdict
if jointdict: # joint will be None if link is a root
robot['joints'][jointdict['name']] = jointdict
if motordict: # motor will be None if no motor is attached or link is a root
robot['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:
robot['links'][linkdict['name']]['inertial'] = props
except KeyError:
log("No inertia for link " + linkdict['name'], "WARNING", "buildModelDictionary")
# we need to 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 = inertia.fuseInertiaData(inertials)
iv = inertia.inertiaMatrixToList(iv)
if mv is not None and cv is not None and iv is not None:
robot['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))
robot['links'][parentname][obj.phobostype][nUtils.getObjectName(obj)] = props
elif obj.phobostype == 'approxsphere':
props = deriveDictEntry(obj)
parentname = nUtils.getObjectName(sUtils.getEffectiveParent(obj))
robot['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 robot['links']:
link = robot['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)
robot[obj.phobostype+'s'][nUtils.getObjectName(obj)] = props
# parse materials
log("Parsing materials...", "INFO", "buildModelDictionary")
robot['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 robot['materials']:
robot['materials'][matname] = deriveMaterial(mat) # this should actually never happen
linkname = nUtils.getObjectName(sUtils.getEffectiveParent(obj))
robot['links'][linkname]['visual'][nUtils.getObjectName(obj)]['material'] = matname
# 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":
robot['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:
robot['chains'][chain['name']] = chain
# gather information on lights
log("Parsing lights...", "INFO", "buildModelDictionary")
for obj in objectlist:
if obj.phobostype == 'light':
robot['lights'][nUtils.getObjectName(obj)] = deriveLight(obj)
# add additional data to model
robot.update(deriveTextData(robot['modelname']))
# shorten numbers in dictionary to n decimalPlaces and return it
log("Rounding numbers...", "INFO", "buildModelDictionary")
epsilon = 10**(-bpy.data.worlds[0].decimalPlaces) # TODO: implement this separately
return epsilonToZero(robot, epsilon, bpy.data.worlds[0].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 buildModelDictionary(root):
"""Builds a python dictionary representation of a SMURF model for export and inspection.
:param root: bpy.types.objects
:return: dict
"""
#os.system('clear')
robot = {
'links': {},
'joints': {},
'sensors': {},
'motors': {},
'controllers': {},
'materials': {},
'lights': {},
'groups': {},
'chains': {}
}
# timestamp of model
robot["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:
robot['modelname'] = root["modelname"]
else:
log("No name for the model defines, setting to 'unnamed_model'",
"WARNING", "buildModelDictionary")
robot['modelname'] = 'unnamed_model'
log(
"Creating dictionary for robot " + robot['modelname'] +
" 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)
robot['links'][linkdict['name']] = linkdict
if jointdict: # joint will be None if link is a root
robot['joints'][jointdict['name']] = jointdict
if motordict: # motor will be None if no motor is attached or link is a root
robot['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:
robot['links'][linkdict['name']]['inertial'] = props
except KeyError:
log("No inertia for link " + linkdict['name'], "WARNING",
"buildModelDictionary")
# we need to 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 = inertia.fuseInertiaData(inertials)
iv = inertia.inertiaMatrixToList(iv)
if mv is not None and cv is not None and iv is not None:
robot['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))
robot['links'][parentname][obj.phobostype][
nUtils.getObjectName(obj)] = props
elif obj.phobostype == 'approxsphere':
props = deriveDictEntry(obj)
parentname = nUtils.getObjectName(
sUtils.getEffectiveParent(obj))
robot['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 robot['links']:
link = robot['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)
robot[obj.phobostype + 's'][nUtils.getObjectName(obj)] = props
# parse materials
log("Parsing materials...", "INFO", "buildModelDictionary")
robot['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 robot['materials']:
robot['materials'][matname] = deriveMaterial(
mat) # this should actually never happen
linkname = nUtils.getObjectName(sUtils.getEffectiveParent(obj))
robot['links'][linkname]['visual'][nUtils.getObjectName(
obj)]['material'] = matname
# 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":
robot['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:
robot['chains'][chain['name']] = chain
# gather information on lights
log("Parsing lights...", "INFO", "buildModelDictionary")
for obj in objectlist:
if obj.phobostype == 'light':
robot['lights'][nUtils.getObjectName(obj)] = deriveLight(obj)
# add additional data to model
robot.update(deriveTextData(robot['modelname']))
# shorten numbers in dictionary to n decimalPlaces and return it
log("Rounding numbers...", "INFO", "buildModelDictionary")
epsilon = 10**(-bpy.data.worlds[0].decimalPlaces
) # TODO: implement this separately
return epsilonToZero(robot, epsilon,
bpy.data.worlds[0].decimalPlaces), objectlist
