def execute(self, context):
"""
Args:
context:
Returns:
"""
objects = context.selected_objects
annotation = defs.definitions[self.annotationtype][self.devicetype]
# add annotation (objects) to the selected objects
annot_objects = []
for obj in objects:
if self.asObject:
annot_objects.append(
eUtils.addAnnotationObject(
obj,
annotation,
name=obj.name + '_annotation',
namespace=self.annotationtype.rstrip('s'),
)
)
else:
eUtils.addAnnotation(obj, annotation, namespace=self.annotationtype.rstrip('s'))
# reselect the original objects and additional annotation objects
sUtils.selectObjects(objects + annot_objects, clear=True)
bUtils.toggleLayer('annotation', value=True)
return {'FINISHED'}
def createLink(scale, position=None, orientation=None, name=''):
"""Creates an empty link (bone) at the current 3D cursor position.
:param scale: This is the scale you want to apply to the new link.
:type scale: Float array with 3 elements.
:param position: This specifies the position of the newly created link. When not given its (0.0,0.0,0.0)
:type position: Float array with 3 elements.
:param orientation: This specifies the rotation of the newly created link. When not given its (0.0,0.0,0.0)
:type orientation:Float array with 3 elements.
:param name: This sets the name for the new link. When not given the link is nameless.
:type name: string.
:return: blender object
"""
blenderUtils.toggleLayer(defs.layerTypes['link'], True)
if position is None and orientation is None:
bpy.ops.object.armature_add(layers=blenderUtils.defLayers([0]))
elif position is None:
bpy.ops.object.armature_add(rotation=orientation, layers=blenderUtils.defLayers([0]))
elif orientation is None:
bpy.ops.object.armature_add(location=position, layers=blenderUtils.defLayers([0]))
else:
bpy.ops.object.armature_add(location=position, rotation=orientation, layers=blenderUtils.defLayers([0]))
link = bpy.context.active_object
link.scale = [scale, scale, scale]
bpy.ops.object.transform_apply(scale=True)
if name:
link.name = name
link.phobostype = 'link'
return link
def createLink(scale, position=None, orientation=None, name=''):
"""Creates an empty link (bone) at the current 3D cursor position.
:param scale: This is the scale you want to apply to the new link.
:type scale: Float array with 3 elements.
:param position: This specifies the position of the newly created link. When not given its (0.0,0.0,0.0)
:type position: Float array with 3 elements.
:param orientation: This specifies the rotation of the newly created link. When not given its (0.0,0.0,0.0)
:type orientation:Float array with 3 elements.
:param name: This sets the name for the new link. When not given the link is nameless.
:type name: str
:return: bpy_types.Object
"""
blenderUtils.toggleLayer(defs.layerTypes['link'], True)
if position is None and orientation is None:
bpy.ops.object.armature_add(layers=blenderUtils.defLayers([0]))
elif position is None:
bpy.ops.object.armature_add(rotation=orientation,
layers=blenderUtils.defLayers([0]))
elif orientation is None:
bpy.ops.object.armature_add(location=position,
layers=blenderUtils.defLayers([0]))
else:
bpy.ops.object.armature_add(location=position,
rotation=orientation,
layers=blenderUtils.defLayers([0]))
link = bpy.context.active_object
link.scale = [scale, scale, scale]
bpy.ops.object.transform_apply(scale=True)
if name:
link.name = name
link.phobostype = 'link'
return link
def createLink(link):
"""Creates the blender representation of a given link and its parent joint.
Args:
link(dict): The link you want to create a representation of.
Returns:
bpy_types.Object -- the newly created blender link object.
"""
# create armature/bone
bUtils.toggleLayer(defs.layerTypes['link'], True)
bpy.ops.object.select_all(action='DESELECT')
bpy.ops.object.armature_add(
layers=bUtils.defLayers([defs.layerTypes['link']]))
newlink = bpy.context.active_object
# Move bone when adding at selected objects location
if 'matrix' in link:
newlink.matrix_world = link['matrix']
newlink.phobostype = 'link'
if link['name'] in bpy.data.objects.keys():
log('Object with name of new link already exists: ' + link['name'],
'WARNING')
nUtils.safelyName(newlink, link['name'])
# set the size of the link
visuals, collisions = getGeometricElements(link)
if visuals or collisions:
scale = max((geometrymodel.getLargestDimension(e['geometry'])
for e in visuals + collisions))
else:
scale = 0.2
# use scaling factor provided by user
if 'scale' in link:
scale *= link['scale']
newlink.scale = (scale, scale, scale)
bpy.ops.object.transform_apply(scale=True)
# add custom properties
for prop in link:
if prop.startswith('$'):
for tag in link[prop]:
newlink['link/' + prop[1:] + '/' + tag] = link[prop][tag]
# create inertial
if 'inertial' in link:
inertia.createInertial(link['name'], link['inertial'], newlink)
# create geometric elements
log(
"Creating visual and collision objects for link '{0}': {1}".format(
link['name'],
', '.join([elem['name'] for elem in visuals + collisions])),
'DEBUG')
for v in visuals:
geometrymodel.createGeometry(v, 'visual', newlink)
for c in collisions:
geometrymodel.createGeometry(c, 'collision', newlink)
return newlink
def execute(self, context):
"""
Args:
context:
Returns:
"""
objects = context.selected_objects
annotation = defs.definitions[self.annotationtype][self.devicetype]
# add annotation (objects) to the selected objects
annot_objects = []
for obj in objects:
if self.asObject:
annot_objects.append(
eUtils.addAnnotationObject(
obj,
annotation,
name=obj.name + '_annotation',
namespace=self.annotationtype.rstrip('s'),
)
)
else:
eUtils.addAnnotation(obj, annotation, namespace=self.annotationtype.rstrip('s'))
# reselect the original objects and additional annotation objects
sUtils.selectObjects(objects + annot_objects, clear=True)
bUtils.toggleLayer(defs.layerTypes['annotation'], value=True)
return {'FINISHED'}
def execute(self, context):
"""
Args:
context:
Returns:
"""
filepath = self.filepath
# infer entitytpe from the filepath to avoid dropdown selection errors
extension = filepath.split('.')[-1].lower()
entitytype = self.entitytype
if extension in ['sdf', 'urdf']:
log(
"Inferring entitytpe {entitytype} from file extension.".format(
entitytype=entitytype), "INFO")
entitytype = extension
self.entitytype = extension
log(
"Importing {filepath} as {entitytype}".format(
filepath=filepath, entitytype=entitytype), "INFO")
model = entity_io.entity_types[entitytype]['import'](filepath)
# bUtils.cleanScene()
models.buildModelFromDictionary(model)
for layer in ['link', 'inertial', 'visual', 'collision', 'sensor']:
bUtils.toggleLayer(defs.layerTypes[layer], True)
return {'FINISHED'}
def createLink(link):
"""Creates the blender representation of a given link and its parent joint.
:param link: The link you want to create a representation of.
:type link: dict
:return: bpy_types.Object -- the newly created blender link object.
"""
# create armature/bone
bUtils.toggleLayer(defs.layerTypes['link'], True)
bpy.ops.object.select_all(action='DESELECT')
bpy.ops.object.armature_add(
layers=bUtils.defLayers([defs.layerTypes['link']]))
newlink = bpy.context.active_object
# Move bone when adding at selected objects location
if 'matrix' in link:
newlink.matrix_world = link['matrix']
newlink.phobostype = 'link'
newlink.name = link['name']
# FIXME: This is a hack and should be checked before creation!
# this is a backup in case an object with the link's name already exists
newlink["link/name"] = link['name']
# FIXME geometric dimensions are not intiallized properly, thus scale always 0.2!
# set the size of the link
elements = getGeometricElements(link)
scale = max((geometrymodel.getLargestDimension(element['geometry'])
for element in elements)) if elements else 0.2
# use scaling factor provided by user
#FIXME where would this *scale* come from?
if 'scale' in link:
scale *= link['scale']
newlink.scale = (scale, scale, scale)
bpy.ops.object.transform_apply(scale=True)
# add custom properties
for prop in link:
if prop.startswith('$'):
for tag in link[prop]:
newlink['link/' + prop[1:] + '/' + tag] = link[prop][tag]
# create inertial
if 'inertial' in link:
inertiamodel.createInertialFromDictionary(link['name'],
link['inertial'])
# create visual elements
if 'visual' in link:
for v in link['visual']:
visual = link['visual'][v]
geometrymodel.createGeometry(visual, 'visual')
# create collision elements
if 'collision' in link:
for c in link['collision']:
collision = link['collision'][c]
geometrymodel.createGeometry(collision, 'collision')
return newlink
def execute(self, context):
log("Importing " + self.filepath + ' as ' + self.entitytype, "INFO")
model = entity_io.entity_types[self.entitytype]['import'](self.filepath)
# bUtils.cleanScene()
models.buildModelFromDictionary(model)
for layer in ['link', 'inertial', 'visual', 'collision', 'sensor']:
bUtils.toggleLayer(defs.layerTypes[layer], True)
return {'FINISHED'}
def createJoint(joint, linkobj=None):
"""Adds joint data to 'link' object.
Args:
joint (dict): dictionary containing the joint definition
linkobj (bpy.types.Object): the obj of phobostype 'link' that receives the joint
Returns:
"""
# add joint information
if not linkobj:
linkobj = sUtils.getObjectByName(joint['child'])
if isinstance(linkobj, list):
log(
"Could not identify object to define joint '{0}'.".format(
joint['name']), 'ERROR')
return
if joint['name'] != linkobj.name:
linkobj['joint/name'] = joint['name']
# get hold of object
bUtils.toggleLayer(list(linkobj.layers).index(True),
True) # any layer containing the object
sUtils.selectObjects([linkobj], clear=True, active=0)
# set axis
if 'axis' in joint:
if mathutils.Vector(tuple(joint['axis'])).length == 0.:
log('Axis of joint {0} is of zero length: '.format(joint['name']),
'ERROR')
else:
bpy.ops.object.mode_set(mode='EDIT')
editbone = linkobj.data.edit_bones[0]
length = editbone.length
axis = mathutils.Vector(tuple(joint['axis']))
editbone.tail = editbone.head + axis.normalized() * length
# add constraints
for param in ['effort', 'velocity']:
try:
if 'limits' in joint:
linkobj['joint/max' + param] = joint['limits'][param]
except KeyError:
log("Joint limits incomplete for joint {0}".format(joint['name']),
'ERROR')
try:
lower = joint['limits']['lower']
upper = joint['limits']['upper']
except KeyError:
lower = 0.0
upper = 0.0
setJointConstraints(linkobj, joint['type'], lower, upper)
for prop in joint:
if prop.startswith('$'):
for tag in joint[prop]:
linkobj['joint/' + prop[1:] + '/' + tag] = joint[prop][tag]
def createSensor(sensor, reference, origin=mathutils.Matrix()):
blenderUtils.toggleLayer(defs.layerTypes['sensor'], value=True)
# create sensor object
if 'Camera' in sensor['type']:
bpy.context.scene.layers[defs.layerTypes['sensor']] = True
bpy.ops.object.add(type='CAMERA', location=origin.to_translation(),
rotation=origin.to_euler(),
layers=blenderUtils.defLayers([defs.layerTypes['sensor']]))
newsensor = bpy.context.active_object
if reference is not None:
selectionUtils.selectObjects([newsensor, bpy.data.objects[reference]], clear=True, active=1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
elif sensor['type'] in ['RaySensor', 'RotatingRaySensor', 'ScanningSonar', 'MultiLevelLaserRangeFinder']:
# TODO: create a proper ray sensor scanning layer disc here
newsensor = blenderUtils.createPrimitive(sensor['name'], 'disc', (0.5, 36),
defs.layerTypes['sensor'], 'phobos_laserscanner',
origin.to_translation(), protation=origin.to_euler())
if reference is not None and reference != []:
if type(reference) == str:
key = reference
else:
key = reference[0]
selectionUtils.selectObjects([newsensor, bpy.data.objects[key]], clear=True, active=1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
else: # contact, force and torque sensors (or unknown sensors)
newsensor = blenderUtils.createPrimitive(sensor['name'], 'sphere', 0.05,
defs.layerTypes['sensor'], 'phobos_sensor',
origin.to_translation(), protation=origin.to_euler())
if 'Node' in sensor['type']:
newsensor['sensor/nodes'] = sorted(reference)
elif 'Joint' in sensor['type'] or 'Motor' in sensor['type']:
newsensor['sensor/joints'] = sorted(reference)
if reference is not None and reference != []:
selectionUtils.selectObjects([newsensor, selectionUtils.getRoot(bpy.data.objects[0])], clear=True, active=1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
# set sensor properties
newsensor.phobostype = 'sensor'
newsensor.name = sensor['name']
newsensor['sensor/type'] = sensor['type']
for prop in sensor['props']:
newsensor['sensor/'+prop] = sensor['props'][prop]
# add custom properties
#for prop in sensor:
# if prop.startswith('$'):
# for tag in sensor[prop]:
# newsensor[prop[1:]+'/'+tag] = sensor[prop][tag]
# throw warning if type is not known
if sensor['type'] not in defs.sensortypes:
print("### Warning: sensor", sensor['name'], "is of unknown/custom type.")
selectionUtils.selectObjects([newsensor], clear=False, active=0)
return newsensor
def execute(self, context):
try:
log("Importing " + self.filepath + ' as ' + self.entitytype,
"INFO")
model = entities.entity_types[self.entitytype]['import'](
self.filepath)
# bUtils.cleanScene()
models.buildModelFromDictionary(model)
for layer in ['link', 'inertial', 'visual', 'collision', 'sensor']:
bUtils.toggleLayer(defs.layerTypes[layer], True)
except KeyError:
log(
"No import function available for selected model type: " +
self.entitytype, "ERROR")
return {'FINISHED'}
def execute(self, context):
"""
Args:
context:
Returns:
"""
log("Importing " + self.filepath + ' as ' + self.entitytype, "INFO")
model = entity_io.entity_types[self.entitytype]['import'](self.filepath)
# bUtils.cleanScene()
models.buildModelFromDictionary(model)
for layer in ['link', 'inertial', 'visual', 'collision', 'sensor']:
bUtils.toggleLayer(defs.layerTypes[layer], True)
return {'FINISHED'}
def createJoint(joint, linkobj=None):
# TODO add some docstring
# add joint information
if not linkobj:
linkobj = bpy.data.objects[joint['child']] # TODO: Make this generic?
if joint['name'] != linkobj.name:
linkobj['joint/name'] = joint['name']
# get hold of object
bUtils.toggleLayer(defs.layerTypes['link'], True)
sUtils.selectObjects([linkobj], clear=True, active=0)
# set axis
if 'axis' in joint:
# Providing a zero axis joint will size the editbone to zero scale
if mathutils.Vector(tuple(joint['axis'])).length == 0.:
log(
'Faulty joint definition ({0}): Axis is of zero length.'.
format(joint['name']), 'ERROR')
else:
bpy.ops.object.mode_set(mode='EDIT')
editbone = linkobj.data.edit_bones[0]
#oldaxis = editbone.vector
length = editbone.length
axis = mathutils.Vector(tuple(joint['axis']))
#oldaxis.cross(axis) # rotation axis
editbone.tail = editbone.head + axis.normalized() * length
# add constraints
for param in ['effort', 'velocity']:
try:
if 'limits' in joint:
linkobj['joint/max' + param] = joint['limits'][param]
except KeyError:
# TODO more details
log("Key Error in adding joint constraints for joint",
joint['name'])
try:
lower = joint['limits']['lower']
upper = joint['limits']['upper']
except KeyError:
lower = 0.0
upper = 0.0
setJointConstraints(linkobj, joint['type'], lower, upper)
for prop in joint:
if prop.startswith('$'):
for tag in joint[prop]:
linkobj['joint/' + prop[1:] + '/' + tag] = joint[prop][tag]
def execute(self, context):
"""
Args:
context:
Returns:
"""
suffix = self.filepath.split(".")[-1]
if suffix in entity_io.entity_types:
log("Importing " + self.filepath + ' as ' + suffix, "INFO")
model = entity_io.entity_types[suffix]['import'](self.filepath)
# bUtils.cleanScene()
models.buildModelFromDictionary(model)
for layer in ['link', 'inertial', 'visual', 'collision', 'sensor']:
bUtils.toggleLayer(layer, True)
else:
log("No module found to import " + suffix, "ERROR")
return {'FINISHED'}
def execute(self, context):
"""
Args:
context:
Returns:
"""
phobos_dict = ioUtils.getDictFromYamlDefs(self.phobostype,
self.preset_name,
self.obj_name)
selected_objs = context.selected_objects
# store collected object data properties in the props dictionary
for i in range(len(self.phobos_data)):
if self.phobos_data[i].name[0] == 'b':
store = '$' + str(bool(
self.phobos_data[i]['boolProp'])).lower()
elif self.phobos_data[i].name[0] == 'i':
store = self.phobos_data[i]['intProp']
elif self.phobos_data[i].name[0] == 's':
store = self.phobos_data[i]['stringProp']
elif self.phobos_data[i].name[0] == 'f':
store = self.phobos_data[i]['floatProp']
phobos_dict['props'][self.phobos_data[i].name[2:]] = store
annotations = {}
# add annotation objects for other categories
for custom_anno in self.annotation_checks:
if custom_anno.boolProp:
# parse object dictionaries if "$selected_objects:..." syntax is found
annot = defs.definitions[self.phobostype + 's'][
self.preset_name][custom_anno.name[2:]]
annotations[custom_anno.name[2:]] = linkObjectLists(
annot, selected_objs)
# let the exectute function handle the object creation
new_objs, annot_objs, otherobjs = execute_func(
phobos_dict, annotations, selected_objs, context.active_object,
*args)
# select the newly added objects
sUtils.selectObjects(new_objs + annot_objs + otherobjs,
clear=True,
active=0)
bUtils.toggleLayer(defs.layerTypes[self.phobostype], value=True)
if annot_objs:
bUtils.toggleLayer(defs.layerTypes['annotation'], value=True)
# toggle layers for generic objects
if otherobjs:
for obj in otherobjs:
bUtils.toggleLayer(defs.layerTypes[obj.phobostype],
value=True)
return {'FINISHED'}
def execute(self, context):
"""
Args:
context:
Returns:
"""
phobos_dict = ioUtils.getDictFromYamlDefs(
self.phobostype, self.preset_name, self.obj_name
)
selected_objs = context.selected_objects
# store collected object data properties in the props dictionary
for i in range(len(self.phobos_data)):
if self.phobos_data[i].name[0] == 'b':
store = '$' + str(bool(self.phobos_data[i]['boolProp'])).lower()
elif self.phobos_data[i].name[0] == 'i':
store = self.phobos_data[i]['intProp']
elif self.phobos_data[i].name[0] == 's':
store = self.phobos_data[i]['stringProp']
elif self.phobos_data[i].name[0] == 'f':
store = self.phobos_data[i]['floatProp']
phobos_dict['props'][self.phobos_data[i].name[2:]] = store
annotations = {}
# add annotation objects for other categories
for custom_anno in self.annotation_checks:
if custom_anno.boolProp:
# parse object dictionaries if "$selected_objects:..." syntax is found
annot = defs.definitions[self.phobostype + 's'][self.preset_name][
custom_anno.name[2:]
]
annotations[custom_anno.name[2:]] = linkObjectLists(annot, selected_objs)
# let the exectute function handle the object creation
new_objs, annot_objs, otherobjs = execute_func(
phobos_dict, annotations, selected_objs, context.active_object, *args
)
# select the newly added objects
sUtils.selectObjects(new_objs + annot_objs + otherobjs, clear=True, active=0)
bUtils.toggleLayer(defs.layerTypes[self.phobostype], value=True)
if annot_objs:
bUtils.toggleLayer(defs.layerTypes['annotation'], value=True)
# toggle layers for generic objects
if otherobjs:
for obj in otherobjs:
bUtils.toggleLayer(defs.layerTypes[obj.phobostype], value=True)
return {'FINISHED'}
def createSensor(sensor, reference, origin=mathutils.Matrix()):
"""This function creates a new sensor specified by its parameters.
The sensor dictionary has to contain these keys:
*name*: name of the new sensor
*type*: type specifier of the sensor
*shape*: a shape specifier for the sensor
*props*: custom properties to be written to the sensor object
Args:
sensor(dict): phobos representation of the new sensor
reference(bpy_types.Object): object to add a parent relationship to
origin(mathutils.Matrix, optional): new sensors origin (Default value = mathutils.Matrix())
Returns:
: The newly created sensor object
"""
layers = defs.layerTypes['sensor']
bUtils.toggleLayer(layers, value=True)
# create sensor object
if sensor['shape'].startswith('resource'):
newsensor = bUtils.createPrimitive(
sensor['name'],
'box',
[1, 1, 1],
layers,
plocation=origin.to_translation(),
protation=origin.to_euler(),
pmaterial=sensor['material'],
phobostype='sensor',
)
# use resource name provided as: "resource:whatever_name"
resource_obj = ioUtils.getResource(
['sensor'] + sensor['shape'].split('://')[1].split('_'))
if resource_obj:
log("Assigned resource mesh and materials to new sensor object.",
'DEBUG')
newsensor.data = resource_obj.data
newsensor.scale = (sensor['size'], ) * 3
else:
log(
"Could not use resource mesh for sensor. Default cube used instead.",
'WARNING')
else:
newsensor = bUtils.createPrimitive(
sensor['name'],
sensor['shape'],
sensor['size'],
layers,
plocation=origin.to_translation(),
protation=origin.to_euler(),
pmaterial=sensor['material'],
phobostype='sensor',
)
# assign the parent if available
if reference is not None:
eUtils.parentObjectsTo(newsensor, reference)
# TODO we need to deal with other types of parameters for sensors
# TODO cameraRotLock() use or dispose?
# contact, force and torque sensors (or unknown sensors)
# else:
# newsensor = bUtils.createPrimitive(
# sensor['name'], 'ico', 0.05, layers, 'phobos_sensor',
# origin.to_translation(), protation=origin.to_euler())
# if sensor['name'] == 'Joint6DOF':
# # TODO delete me? handle this
# #newsensor['sensor/nodes'] = nUtils.getObjectName(reference)
# pass
# elif 'Node' in sensor['type']:
# newsensor['sensor/nodes'] = sorted([nUtils.getObjectName(ref) for ref in reference])
# elif 'Joint' in sensor['type'] or 'Motor' in sensor['type']:
# newsensor['sensor/joints'] = sorted([nUtils.getObjectName(ref) for ref in reference])
# elif sensor['type'] in ['Joint6DOF']:
# for obj in context.selected_objects:
# if obj.phobostype == 'link':
# sensor['name'] = "sensor_joint6dof_" + nUtils.getObjectName(obj, phobostype="joint")
# sensors.createSensor(sensor, obj, obj.matrix_world)
# elif 'Node' in sensor['type']:
# sensors.createSensor(sensor, [obj for obj in context.selected_objects if obj.phobostype == 'collision'],
# mathutils.Matrix.Translation(context.scene.cursor_location))
# elif 'Motor' in sensor['type'] or 'Joint' in sensor['type']:
# sensors.createSensor(sensor, [obj for obj in context.selected_objects if obj.phobostype == 'link'],
# mathutils.Matrix.Translation(context.scene.cursor_location))
# set sensor properties
newsensor.phobostype = 'sensor'
newsensor.name = sensor['name']
newsensor['sensor/name'] = sensor['name']
newsensor['sensor/type'] = sensor['type']
# write the custom properties to the sensor
eUtils.addAnnotation(newsensor, sensor['props'], namespace='sensor')
# throw warning if type is not known
# TODO we need to link this error to the sensor type specifications
if sensor['type'] not in [
key.lower() for key in defs.def_settings['sensors']
]:
log(
"Sensor " + sensor['name'] + " is of unknown/custom type: " +
sensor['type'] + ".",
'WARNING',
)
# select the new sensor
sUtils.selectObjects([newsensor], clear=True, active=0)
return newsensor
def createLink(link):
"""Creates the blender representation of a given link and its parent joint.
The link is added to the link layer.
These entries in the dictionary are mandatory:
*name*: name for the link
The specified dictionary may contain these entries:
*matrix*: world matrix for the new link transformation
*scale*: scale for the new link (single float)
*visual*: list of visual dictionaries
*collision*: list of collision dictionaries
*inertial*: inertial dictionary (an inertial object will be created on the fly)
Furthermore any generic properties, prepended by a `$` will be added as custom properties to the
link. E.g. $test/etc would be put to link/test/etc. However, these properties are extracted
only in the first layer of hierarchy.
Args:
link(dict): The link you want to create a representation of.
Returns:
: bpy_types.Object -- the newly created blender link object.
"""
log("Creating link object '{}'...".format(link['name']),
'DEBUG',
prefix='\n')
# create armature/bone
bUtils.toggleLayer('link', True)
bpy.ops.object.select_all(action='DESELECT')
bpy.ops.object.armature_add()
newlink = bpy.context.active_object
# Move bone when adding at selected objects location
if 'matrix' in link:
newlink.matrix_world = link['matrix']
# give it a proper name
newlink.phobostype = 'link'
if link['name'] in bpy.data.objects.keys():
log('Object with name of new link already exists: ' + link['name'],
'WARNING')
nUtils.safelyName(newlink, link['name'])
# set the size of the link
visuals, collisions = getGeometricElements(link)
if visuals or collisions:
scale = max((geometrymodel.getLargestDimension(e['geometry'])
for e in visuals + collisions))
else:
scale = 0.2
# use scaling factor provided by user
if 'scale' in link:
scale *= link['scale']
newlink.scale = (scale, scale, scale)
bpy.ops.object.transform_apply(location=False,
rotation=False,
scale=True,
properties=False)
# add custom properties
for prop in link:
if prop.startswith('$'):
for tag in link[prop]:
newlink['link/' + prop[1:] + '/' + tag] = link[prop][tag]
# create inertial
if 'inertial' in link:
inertia.createInertial(link['inertial'], newlink)
# create geometric elements
log(
"Creating visual and collision objects for link '{0}':\n{1}".format(
link['name'],
' \n'.join([elem['name'] for elem in visuals + collisions])),
'DEBUG',
)
for vis in visuals:
geometrymodel.createGeometry(vis, 'visual', newlink)
for col in collisions:
geometrymodel.createGeometry(col, 'collision', newlink)
bUtils.sortObjectToCollection(newlink, 'link')
return newlink
def createController(controller, reference, origin=mathutils.Matrix(), annotations=None):
"""This function creates a new controller specified by its parameters.
If an annotation category or the keyword 'all' is specified, the respective annotations for the
controller will be added as objects.
Args:
controller(dict): phobos representation of the new controller
reference(bpy_types.Object): object to add a parent relationship to
origin(mathutils.Matrix, optional): new controllers origin (Default value = mathutils.Matrix())
annotations(list(str, optional): list of annotation keys or 'all' to add to as annotation
objects (Default value = None)
Returns:
: bpy.types.Object -- new created controller object
"""
layers = defs.layerTypes['controller']
bUtils.toggleLayer(layers, value=True)
# create controller object
if controller['shape'].startswith('resource'):
newcontroller = bUtils.createPrimitive(
controller['name'],
'box',
[1, 1, 1],
layers,
plocation=origin.to_translation(),
protation=origin.to_euler(),
pmaterial=controller['material'],
phobostype='controller',
)
# use resource name provided as: "resource:whatever_name"
resource_obj = ioUtils.getResource(
['controller'] + controller['shape'].split('://')[1].split('_')
)
if resource_obj:
log("Assigned resource mesh and materials to new controller object.", 'DEBUG')
newcontroller.data = resource_obj.data
newcontroller.scale = (controller['size'],) * 3
else:
log("Could not use resource mesh for controller. Default cube used instead.", 'WARNING')
else:
newcontroller = bUtils.createPrimitive(
controller['name'],
controller['shape'],
controller['size'],
layers,
plocation=origin.to_translation(),
protation=origin.to_euler(),
pmaterial=controller['material'],
phobostype='controller',
)
newcontroller.name = controller['name']
newcontroller['controller/type'] = controller['type']
# write the custom properties to the controller
eUtils.addAnnotation(newcontroller, controller['props'], namespace='controller')
if controller['annotations'] and annotations:
if annotations == 'all':
keys = controller['annotations'].keys()
elif isinstance(annotations, list):
keys = [key for key in annotations if key in controller['annotations']]
else:
keys = []
for key in keys:
eUtils.addAnnotationObject(
newcontroller, controller['annotations'][key], namespace='controller/' + key
)
# assign the parent if available
if reference is not None:
sUtils.selectObjects([newcontroller, reference], clear=True, active=1)
if reference.phobostype == 'link':
bpy.ops.object.parent_set(type='BONE_RELATIVE')
else:
bpy.ops.object.parent_set(type='OBJECT')
return newcontroller
def createSensor(sensor, reference, origin=mathutils.Matrix()):
"""This function creates a new sensor specified by its parameters.
Args:
sensor(dict): The phobos representation of the new sensor.
reference(bpy_types.Object): This is an object to add a parent relationship to.
origin(mathutils.Matrix, optional): The new sensors origin. (Default value = mathutils.Matrix()
Returns:
The newly created sensor object
"""
bUtils.toggleLayer(defs.layerTypes['sensor'], value=True)
# create sensor object
if 'Camera' in sensor['type']:
bpy.ops.object.add(type='CAMERA',
location=origin.to_translation(),
rotation=origin.to_euler(),
layers=bUtils.defLayers([defs.layerTypes['sensor']
]))
newsensor = bpy.context.active_object
if reference is not None:
sUtils.selectObjects([newsensor, reference], clear=True, active=1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
elif sensor['type'] in [
'RaySensor', 'RotatingRaySensor', 'ScanningSonar',
'MultiLevelLaserRangeFinder'
]:
# TODO: create a proper ray sensor scanning layer disc here
newsensor = bUtils.createPrimitive(sensor['name'],
'disc', (0.5, 36),
defs.layerTypes['sensor'],
'phobos_laserscanner',
origin.to_translation(),
protation=origin.to_euler())
if reference is not None:
sUtils.selectObjects([newsensor, reference], clear=True, active=1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
# contact, force and torque sensors (or unknown sensors)
else:
newsensor = bUtils.createPrimitive(sensor['name'],
'sphere',
0.05,
defs.layerTypes['sensor'],
'phobos_sensor',
origin.to_translation(),
protation=origin.to_euler())
if sensor['type'] == 'Joint6DOF':
# TODO delete me? handle this
#newsensor['sensor/nodes'] = nUtils.getObjectName(reference)
pass
elif 'Node' in sensor['type']:
newsensor['sensor/nodes'] = sorted(
[nUtils.getObjectName(ref) for ref in reference])
elif 'Joint' in sensor['type'] or 'Motor' in sensor['type']:
newsensor['sensor/joints'] = sorted(
[nUtils.getObjectName(ref) for ref in reference])
if reference is not None:
sUtils.selectObjects([newsensor, reference], clear=True, active=1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
# set sensor properties
newsensor.phobostype = 'sensor'
newsensor.name = sensor['name']
newsensor['sensor/type'] = sensor['type']
# TODO delete me?
#for prop in ['link', 'joint', 'links', 'joints', 'motors']:
# if prop in sensor:
# newsensor['sensor/'+prop] = sensor[prop]
# add custom properties
#for prop in sensor:
# if prop.startswith('$'):
# for tag in sensor[prop]:
# newsensor[prop[1:]+'/'+tag] = sensor[prop][tag]
# throw warning if type is not known
if sensor['type'] not in defs.sensortypes:
print("### Warning: sensor", sensor['name'],
"is of unknown/custom type.")
sUtils.selectObjects([newsensor], clear=False, active=0)
return newsensor
def createSensor(sensor, reference, origin=mathutils.Matrix()):
"""This function creates a new sensor specified by its parameters.
:param sensor: The phobos representation of the new sensor.
:type sensor: dict
:param reference: This is an object to add a parent relationship to.
:type reference: bpy_types.Object
:param origin: The new sensors origin.
:type origin: mathutils.Matrix
:return: The newly created sensor object
"""
blenderUtils.toggleLayer(defs.layerTypes['sensor'], value=True)
# create sensor object
if 'Camera' in sensor['type']:
bpy.ops.object.add(type='CAMERA', location=origin.to_translation(),
rotation=origin.to_euler(),
layers=blenderUtils.defLayers([defs.layerTypes['sensor']]))
newsensor = bpy.context.active_object
if reference is not None:
selectionUtils.selectObjects([newsensor, reference], clear=True, active=1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
elif sensor['type'] in ['RaySensor', 'RotatingRaySensor', 'ScanningSonar', 'MultiLevelLaserRangeFinder']:
# TODO: create a proper ray sensor scanning layer disc here
newsensor = blenderUtils.createPrimitive(sensor['name'], 'disc', (0.5, 36),
defs.layerTypes['sensor'], 'phobos_laserscanner',
origin.to_translation(), protation=origin.to_euler())
if reference is not None:
selectionUtils.selectObjects([newsensor, reference], clear=True, active=1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
else: # contact, force and torque sensors (or unknown sensors)
newsensor = blenderUtils.createPrimitive(sensor['name'], 'sphere', 0.05,
defs.layerTypes['sensor'], 'phobos_sensor',
origin.to_translation(), protation=origin.to_euler())
if sensor['type'] == 'Joint6DOF':
pass #newsensor['sensor/nodes'] = nameUtils.getObjectName(reference)
elif 'Node' in sensor['type']:
newsensor['sensor/nodes'] = sorted([nameUtils.getObjectName(ref) for ref in reference])
elif 'Joint' in sensor['type'] or 'Motor' in sensor['type']:
newsensor['sensor/joints'] = sorted([nameUtils.getObjectName(ref) for ref in reference])
if reference is not None:
selectionUtils.selectObjects([newsensor, reference], clear=True, active=1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
# set sensor properties
newsensor.phobostype = 'sensor'
newsensor.name = sensor['name']
newsensor['sensor/type'] = sensor['type']
#for prop in ['link', 'joint', 'links', 'joints', 'motors']:
# if prop in sensor:
# newsensor['sensor/'+prop] = sensor[prop]
# add custom properties
#for prop in sensor:
# if prop.startswith('$'):
# for tag in sensor[prop]:
# newsensor[prop[1:]+'/'+tag] = sensor[prop][tag]
# throw warning if type is not known
if sensor['type'] not in defs.sensortypes:
print("### Warning: sensor", sensor['name'], "is of unknown/custom type.")
selectionUtils.selectObjects([newsensor], clear=False, active=0)
return newsensor
def createSensor(sensor, reference, origin=mathutils.Matrix()):
"""This function creates a new sensor specified by its parameters.
The sensor dictionary has to contain these keys:
*name*: name of the new sensor
*type*: type specifier of the sensor
*shape*: a shape specifier for the sensor
*props*: custom properties to be written to the sensor object
Args:
sensor(dict): phobos representation of the new sensor
reference(bpy_types.Object): object to add a parent relationship to
origin(mathutils.Matrix, optional): new sensors origin (Default value = mathutils.Matrix())
Returns:
: The newly created sensor object
"""
layers = defs.layerTypes['sensor']
bUtils.toggleLayer(layers, value=True)
# create sensor object
if sensor['shape'].startswith('resource'):
newsensor = bUtils.createPrimitive(
sensor['name'],
'box',
[1, 1, 1],
layers,
plocation=origin.to_translation(),
protation=origin.to_euler(),
pmaterial=sensor['material'],
phobostype='sensor',
)
# use resource name provided as: "resource:whatever_name"
resource_obj = ioUtils.getResource(['sensor'] + sensor['shape'].split('://')[1].split('_'))
if resource_obj:
log("Assigned resource mesh and materials to new sensor object.", 'DEBUG')
newsensor.data = resource_obj.data
newsensor.scale = (sensor['size'],) * 3
else:
log("Could not use resource mesh for sensor. Default cube used instead.", 'WARNING')
else:
newsensor = bUtils.createPrimitive(
sensor['name'],
sensor['shape'],
sensor['size'],
layers,
plocation=origin.to_translation(),
protation=origin.to_euler(),
pmaterial=sensor['material'],
phobostype='sensor',
)
# assign the parent if available
if reference is not None:
eUtils.parentObjectsTo(newsensor, reference)
# TODO we need to deal with other types of parameters for sensors
# TODO cameraRotLock() use or dispose?
# contact, force and torque sensors (or unknown sensors)
# else:
# newsensor = bUtils.createPrimitive(
# sensor['name'], 'ico', 0.05, layers, 'phobos_sensor',
# origin.to_translation(), protation=origin.to_euler())
# if sensor['name'] == 'Joint6DOF':
# # TODO delete me? handle this
# #newsensor['sensor/nodes'] = nUtils.getObjectName(reference)
# pass
# elif 'Node' in sensor['type']:
# newsensor['sensor/nodes'] = sorted([nUtils.getObjectName(ref) for ref in reference])
# elif 'Joint' in sensor['type'] or 'Motor' in sensor['type']:
# newsensor['sensor/joints'] = sorted([nUtils.getObjectName(ref) for ref in reference])
# elif sensor['type'] in ['Joint6DOF']:
# for obj in context.selected_objects:
# if obj.phobostype == 'link':
# sensor['name'] = "sensor_joint6dof_" + nUtils.getObjectName(obj, phobostype="joint")
# sensors.createSensor(sensor, obj, obj.matrix_world)
# elif 'Node' in sensor['type']:
# sensors.createSensor(sensor, [obj for obj in context.selected_objects if obj.phobostype == 'collision'],
# mathutils.Matrix.Translation(context.scene.cursor_location))
# elif 'Motor' in sensor['type'] or 'Joint' in sensor['type']:
# sensors.createSensor(sensor, [obj for obj in context.selected_objects if obj.phobostype == 'link'],
# mathutils.Matrix.Translation(context.scene.cursor_location))
# set sensor properties
newsensor.phobostype = 'sensor'
newsensor.name = sensor['name']
newsensor['sensor/name'] = sensor['name']
newsensor['sensor/type'] = sensor['type']
# write the custom properties to the sensor
eUtils.addAnnotation(newsensor, sensor['props'], namespace='sensor')
# throw warning if type is not known
# TODO we need to link this error to the sensor type specifications
if sensor['type'] not in [key.lower() for key in defs.def_settings['sensors']]:
log(
"Sensor " + sensor['name'] + " is of unknown/custom type: " + sensor['type'] + ".",
'WARNING',
)
# select the new sensor
sUtils.selectObjects([newsensor], clear=True, active=0)
return newsensor
def createLink(link):
"""Creates the blender representation of a given link and its parent joint.
The link is added to the link layer.
These entries in the dictionary are mandatory:
*name*: name for the link
The specified dictionary may contain these entries:
*matrix*: world matrix for the new link transformation
*scale*: scale for the new link (single float)
*visual*: list of visual dictionaries
*collision*: list of collision dictionaries
*inertial*: inertial dictionary (an inertial object will be created on the fly)
Furthermore any generic properties, prepended by a `$` will be added as custom properties to the
link. E.g. $test/etc would be put to link/test/etc. However, these properties are extracted
only in the first layer of hierarchy.
Args:
link(dict): The link you want to create a representation of.
Returns:
: bpy_types.Object -- the newly created blender link object.
"""
log("Creating link object '{}'...".format(link['name']), 'DEBUG', prefix='\n')
# create armature/bone
bUtils.toggleLayer(defs.layerTypes['link'], True)
bpy.ops.object.select_all(action='DESELECT')
bpy.ops.object.armature_add(layers=bUtils.defLayers([defs.layerTypes['link']]))
newlink = bpy.context.active_object
# Move bone when adding at selected objects location
if 'matrix' in link:
newlink.matrix_world = link['matrix']
# give it a proper name
newlink.phobostype = 'link'
if link['name'] in bpy.data.objects.keys():
log('Object with name of new link already exists: ' + link['name'], 'WARNING')
nUtils.safelyName(newlink, link['name'])
# set the size of the link
visuals, collisions = getGeometricElements(link)
if visuals or collisions:
scale = max(
(geometrymodel.getLargestDimension(e['geometry']) for e in visuals + collisions)
)
else:
scale = 0.2
# use scaling factor provided by user
if 'scale' in link:
scale *= link['scale']
newlink.scale = (scale, scale, scale)
bpy.ops.object.transform_apply(scale=True)
# add custom properties
for prop in link:
if prop.startswith('$'):
for tag in link[prop]:
newlink['link/' + prop[1:] + '/' + tag] = link[prop][tag]
# create inertial
if 'inertial' in link:
inertia.createInertial(link['inertial'], newlink)
# create geometric elements
log(
"Creating visual and collision objects for link '{0}':\n{1}".format(
link['name'], ' \n'.join([elem['name'] for elem in visuals + collisions])
),
'DEBUG',
)
for vis in visuals:
geometrymodel.createGeometry(vis, 'visual', newlink)
for col in collisions:
geometrymodel.createGeometry(col, 'collision', newlink)
return newlink
def createMotor(motor,
parentobj,
origin=mathutils.Matrix(),
addcontrollers=False):
"""This function creates a new motor specified by its parameters.
If *addcontrollers* is set, a controller object will be created from the controller definition
which is specified in the motor dictionary (key *controller*).
Args:
motor(dict): phobos representation of the new motor.
parentobj(bpy_types.Object): object to parent new motor to
origin(mathutils.Matrix, optional): new motors origin (Default value = mathutils.Matrix())
addcontrollers(bool, optional): whether to add the defined controller as object (Default value = False)
Returns:
bpy.types.Object: new motor object or a list of the new motor_object and the new controller object
"""
bUtils.toggleLayer('motor', value=True)
primitive_name = ''
# create name if not given by motor dict
if not 'name' in motor or len(motor['name']) == 0:
motor['name'] = parentobj.name
primitive_name = "motor_" + motor['name']
else:
primitive_name = motor['name']
primitive_name = ''
# create name if not given by motor dict
if not 'name' in motor or len(motor['name']) == 0:
motor['name'] = parentobj.name
primitive_name = "motor_" + motor['name']
else:
primitive_name = motor['name']
# create motor object
if motor['shape'].startswith('resource'):
newmotor = bUtils.createPrimitive(
primitive_name,
'box',
[1, 1, 1],
[],
plocation=origin.to_translation(),
protation=origin.to_euler(),
pmaterial=motor['material'],
phobostype='motor',
)
# use resource name provided as: "resource:whatever_name"
resource_obj = ioUtils.getResource(
['motor'] + motor['shape'].split('://')[1].split('_'))
if resource_obj:
log("Assigned resource mesh and materials to new motor object.",
'DEBUG')
newmotor.data = resource_obj.data
newmotor.scale = (motor['size'], ) * 3
else:
log(
"Could not use resource mesh for motor. Default cube used instead.",
'WARNING')
else:
newmotor = bUtils.createPrimitive(
primitive_name,
motor['shape'],
motor['size'],
[],
plocation=origin.to_translation(),
protation=origin.to_euler(),
pmaterial=motor['material'],
phobostype='motor',
)
# assign the parent if available
if parentobj is not None:
sUtils.selectObjects([newmotor, parentobj], clear=True, active=1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
# set motor properties
newmotor.phobostype = 'motor'
# should not be nessaccary: newmotor.name = motor['name']
defname = motor['defname']
# write the custom properties to the motor
eUtils.addAnnotation(newmotor,
motor['props'],
namespace='motor',
ignore=['defname'])
# fix motor name since it can differe from object name
newmotor['motor/name'] = motor['name']
if 'controller' in defs.definitions['motors'][defname] and addcontrollers:
import phobos.model.controllers as controllermodel
motorcontroller = defs.definitions['motors'][defname]['controller']
controllerdefs = ioUtils.getDictFromYamlDefs(
'controller', motorcontroller, newmotor.name + '_controller')
newcontroller = controllermodel.createController(
controllerdefs,
newmotor,
origin=newmotor.matrix_world,
annotations='all')
else:
newcontroller = None
# select the new motor
sUtils.selectObjects(
[newmotor] if not newcontroller else [newmotor, newcontroller],
clear=True,
active=0)
return newmotor if not newcontroller else [newmotor, newcontroller]
def createJoint(joint, linkobj=None, links=None):
"""Adds joint data to a link object.
If the linkobj is not specified, it is derived from the **child** entry in the joint (object is
searched in the current scene). This only works if the search for the child yields a single
object. Alternatively, it is possible to provide the model dictionary of links. In this case,
the link object is searched in the dictionary (make sure the **object** keys of the dictionary
are set properly).
These entries are mandatory for the dictionary:
| **name**: name of the joint
These entries are optional:
| **axis**: tuple which specifies the axis of the editbone
| **limits**: limits of the joint movement
| **lower**: lower limit (defaults to 0.)
| **upper**: upper limit (defaults to 0.)
| **effort**: maximum effort for the joint
| **velocity**: maximum velocity for the joint
Furthermore any generic properties, prepended by a ``$`` will be added as custom properties to
the joint. E.g. ``$test/etc`` would be put to joint/test/etc. However, these properties are
extracted only in the first layer of hierarchy.
Args:
joint(dict): dictionary containing the joint definition
linkobj(bpy.types.Object, optional): link object receiving joint (Default value = None)
links(dict, optional): model dictionary of links (Default value = None)
Returns:
None: None
"""
# try deriving link object from joint['child']
if not linkobj:
# link dictionary provided -> search for child link object
if (
links
and 'child' in joint
and joint['child'] in links
and 'object' in links[joint['child']]
):
linkobj = links[joint['child']]['object']
# search for child link in scene
else:
linkobj = sUtils.getObjectByName(joint['child'])
if isinstance(linkobj, list):
log(
"Could not identify object to define joint '{0}'.".format(joint['name']),
'ERROR',
)
return
# make sure the proper joint name is kept
if joint['name'] != linkobj.name:
linkobj['joint/name'] = joint['name']
# select the link object
bUtils.toggleLayer(list(linkobj.layers).index(True), True)
sUtils.selectObjects([linkobj], clear=True, active=0)
# set axis
if 'axis' in joint:
if mathutils.Vector(tuple(joint['axis'])).length == 0.:
log('Axis of joint {0} is of zero length: '.format(joint['name']), 'ERROR')
else:
bpy.ops.object.mode_set(mode='EDIT')
editbone = linkobj.data.edit_bones[0]
length = editbone.length
axis = mathutils.Vector(tuple(joint['axis']))
editbone.tail = editbone.head + axis.normalized() * length
# add constraints to the joint
if 'limits' in joint:
for param in ['effort', 'velocity']:
if param in joint['limits']:
linkobj['joint/max' + param] = joint['limits'][param]
else:
log(
"Joint limits incomplete for joint {}. Missing {}.".format(
joint['name'], param
),
'ERROR',
)
if all(elem in joint['limits'] for elem in ['lower', 'upper']):
lower = joint['limits']['lower']
upper = joint['limits']['upper']
else:
log("Joint limits upper/lower is missing! Defaulted to [-1e-5, 1e-5].", 'WARNING')
lower = -1e-5
upper = 1e-5
else:
log("Joint limits upper/lower is missing! Defaulted both to [-1e-5, 1e-5].", 'WARNING')
lower = -1e-5
upper = 1e-5
setJointConstraints(linkobj, joint['type'], lower, upper)
# add generic properties
for prop in joint:
if prop.startswith('$'):
for tag in joint[prop]:
linkobj['joint/' + prop[1:] + '/' + tag] = joint[prop][tag]
log("Assigned joint information to {}.".format(linkobj.name), 'DEBUG')
def buildModelFromDictionary(model):
"""Creates the Blender representation of the imported model, using a model dictionary.
Args:
model(dict): model representation of the imported model
Returns:
"""
log("Creating Blender model...", 'INFO', prefix='\n' + '-' * 25 + '\n')
log(" Initializing materials... ({} total)".format(len(model['materials'])), 'INFO')
for mat in model['materials']:
matmodel.createMaterial(model['materials'][mat], logging=True, adjust=True)
# ['name'], tuple(mat['color'][0:3]), (1, 1, 1), mat['color'][-1])
newobjects = []
log(" Creating links... ({} total)".format(len(model['links'])), 'INFO')
for lnk in model['links']:
link = model['links'][lnk]
model['links'][lnk]['object'] = linkmodel.createLink(link)
newobjects.append(model['links'][lnk]['object'])
newobjects.extend(model['links'][lnk]['object'].children)
log("Setting parent-child relationships", 'INFO', prefix='\n')
bUtils.toggleLayer('link', True)
for lnk in model['links']:
parent = model['links'][lnk]
log("Children for link " + parent['name'] + ":\n" + '\n'.join(parent['children']), 'DEBUG')
for chi in parent['children']:
child = model['links'][chi]
child['object'].matrix_world = parent['object'].matrix_world
eUtils.parentObjectsTo(child['object'], parent['object'])
# set transformations
log("Transforming links... ({} total)".format(len(model['links'])), 'INFO', prefix='\n')
for lnk in model['links']:
if 'parent' not in model['links'][lnk]:
root = model['links'][lnk]
break
linkmodel.setLinkTransformations(model, root)
log("Creating joints... ({} total)".format(len(model['joints'])), 'INFO', prefix='\n')
for j in model['joints']:
joint = model['joints'][j]
jointmodel.createJoint(joint, links=model['links'])
log("Assigning model name: {}".format(model['name']), 'INFO')
rootlink = sUtils.getRoot(bpy.data.objects[root['object'].name])
if 'name' not in model:
log("Model name not specified in URDF. Make sure to define it thereafter.", 'WARNING')
else:
rootlink['model/name'] = model['name']
rootlink.location = (0, 0, 0)
# TODO make sure this works
log("Creating sensors...", 'INFO')
if 'sensors' in model and model['sensors']:
for sen in model['sensors']:
sensormodel.createSensor(model['sensors'][sen], model['sensors'][sen]['parent'])
else:
log(" No sensors in model.", 'INFO')
# TODO make sure this works
log("Creating motors...", 'INFO')
if 'motors' in model and model['motors']:
for motor in model['motors']:
eUtils.setProperties(
model['joints'][model['motors'][motor]['joint']],
model['motors'][motor],
category='motor',
)
else:
log(" No motors in model.", 'INFO')
# TODO make sure this works
log("Creating groups...", 'INFO')
if 'groups' in model and model['groups']:
for group in model['groups']:
createGroup(model['groups'][group])
else:
log(" No kinematic groups in model.", 'INFO')
# TODO make sure this works
log("Creating chains...", 'INFO')
if 'chains' in model and model['chains']:
for ch in model['chains']:
createChain(model['chains'][ch])
else:
log(" No kinematic chains in model.", 'INFO')
# TODO make sure this works
log("Creating lights...", 'INFO')
if 'lights' in model and model['lights']:
for light in model['lights']:
lightmodel.createLight(model['lights'][light])
else:
log(" No lights in model.", 'INFO')
# display new objects after import
sUtils.selectObjects(newobjects, clear=True, active=0)
eUtils.sortObjectsToLayers(newobjects)
for obj in newobjects:
bUtils.setObjectLayersActive(obj, extendlayers=True)
bpy.ops.object.mode_set(mode='OBJECT')
bpy.ops.view3d.view_selected()
# update the scene
bUtils.update()
def createJoint(joint, linkobj=None, links=None):
"""Adds joint data to a link object.
If the linkobj is not specified, it is derived from the **child** entry in the joint (object is
searched in the current scene). This only works if the search for the child yields a single
object. Alternatively, it is possible to provide the model dictionary of links. In this case,
the link object is searched in the dictionary (make sure the **object** keys of the dictionary
are set properly).
These entries are mandatory for the dictionary:
| **name**: name of the joint
These entries are optional:
| **axis**: tuple which specifies the axis of the editbone
| **limits**: limits of the joint movement
| **lower**: lower limit (defaults to 0.)
| **upper**: upper limit (defaults to 0.)
| **effort**: maximum effort for the joint
| **velocity**: maximum velocity for the joint
Furthermore any generic properties, prepended by a ``$`` will be added as custom properties to
the joint. E.g. ``$test/etc`` would be put to joint/test/etc. However, these properties are
extracted only in the first layer of hierarchy.
Args:
joint(dict): dictionary containing the joint definition
linkobj(bpy.types.Object, optional): link object receiving joint (Default value = None)
links(dict, optional): model dictionary of links (Default value = None)
Returns:
None: None
"""
# try deriving link object from joint['child']
if not linkobj:
# link dictionary provided -> search for child link object
if (links and 'child' in joint and joint['child'] in links
and 'object' in links[joint['child']]):
linkobj = links[joint['child']]['object']
# search for child link in scene
else:
linkobj = sUtils.getObjectByName(joint['child'])
if isinstance(linkobj, list):
log(
"Could not identify object to define joint '{0}'.".format(
joint['name']),
'ERROR',
)
return
# make sure the proper joint name is kept
if joint['name'] != linkobj.name:
linkobj['joint/name'] = joint['name']
# select the link object
bUtils.toggleLayer(list(linkobj.layers).index(True), True)
sUtils.selectObjects([linkobj], clear=True, active=0)
# set axis
if 'axis' in joint:
if mathutils.Vector(tuple(joint['axis'])).length == 0.:
log('Axis of joint {0} is of zero length: '.format(joint['name']),
'ERROR')
else:
bpy.ops.object.mode_set(mode='EDIT')
editbone = linkobj.data.edit_bones[0]
length = editbone.length
axis = mathutils.Vector(tuple(joint['axis']))
editbone.tail = editbone.head + axis.normalized() * length
# add constraints to the joint
if 'limits' in joint:
for param in ['effort', 'velocity']:
if param in joint['limits']:
linkobj['joint/max' + param] = joint['limits'][param]
else:
log(
"Joint limits incomplete for joint {}. Missing {}.".format(
joint['name'], param),
'ERROR',
)
if all(elem in joint['limits'] for elem in ['lower', 'upper']):
lower = joint['limits']['lower']
upper = joint['limits']['upper']
else:
log(
"Joint limits upper/lower is missing! Defaulted to [-1e-5, 1e-5].",
'WARNING')
lower = -1e-5
upper = 1e-5
else:
log(
"Joint limits upper/lower is missing! Defaulted both to [-1e-5, 1e-5].",
'WARNING')
lower = -1e-5
upper = 1e-5
setJointConstraints(linkobj, joint['type'], lower, upper)
# add generic properties
for prop in joint:
if prop.startswith('$'):
for tag in joint[prop]:
linkobj['joint/' + prop[1:] + '/' + tag] = joint[prop][tag]
log("Assigned joint information to {}.".format(linkobj.name), 'DEBUG')
def createController(controller,
reference,
origin=mathutils.Matrix(),
annotations=None):
"""This function creates a new controller specified by its parameters.
If an annotation category or the keyword 'all' is specified, the respective annotations for the
controller will be added as objects.
Args:
controller(dict): phobos representation of the new controller
reference(bpy_types.Object): object to add a parent relationship to
origin(mathutils.Matrix, optional): new controllers origin (Default value = mathutils.Matrix())
annotations(list(str, optional): list of annotation keys or 'all' to add to as annotation
objects (Default value = None)
Returns:
: bpy.types.Object -- new created controller object
"""
bUtils.toggleLayer('controller', value=True)
# create controller object
if controller['shape'].startswith('resource'):
newcontroller = bUtils.createPrimitive(
controller['name'],
'box',
[1, 1, 1],
[],
plocation=origin.to_translation(),
protation=origin.to_euler(),
pmaterial=controller['material'],
phobostype='controller',
)
# use resource name provided as: "resource:whatever_name"
resource_obj = ioUtils.getResource(
['controller'] + controller['shape'].split('://')[1].split('_'))
if resource_obj:
log(
"Assigned resource mesh and materials to new controller object.",
'DEBUG')
newcontroller.data = resource_obj.data
newcontroller.scale = (controller['size'], ) * 3
else:
log(
"Could not use resource mesh for controller. Default cube used instead.",
'WARNING')
else:
newcontroller = bUtils.createPrimitive(
controller['name'],
controller['shape'],
controller['size'],
layers,
plocation=origin.to_translation(),
protation=origin.to_euler(),
pmaterial=controller['material'],
phobostype='controller',
)
newcontroller.name = controller['name']
newcontroller['controller/type'] = controller['type']
# write the custom properties to the controller
eUtils.addAnnotation(newcontroller,
controller['props'],
namespace='controller')
if controller['annotations'] and annotations:
if annotations == 'all':
keys = controller['annotations'].keys()
elif isinstance(annotations, list):
keys = [
key for key in annotations if key in controller['annotations']
]
else:
keys = []
for key in keys:
eUtils.addAnnotationObject(newcontroller,
controller['annotations'][key],
namespace='controller/' + key)
# assign the parent if available
if reference is not None:
sUtils.selectObjects([newcontroller, reference], clear=True, active=1)
if reference.phobostype == 'link':
bpy.ops.object.parent_set(type='BONE_RELATIVE')
else:
bpy.ops.object.parent_set(type='OBJECT')
return newcontroller
def buildModelFromDictionary(model):
"""Creates the Blender representation of the imported model, using a model dictionary.
Args:
model:
Returns:
"""
# DOCU add some more docstring
log("Creating Blender model...", 'INFO')
log("Creating links...", 'INFO')
for l in model['links']:
link = model['links'][l]
model['links'][l]['object'] = linkmodel.createLink(link)
log("Setting parent-child relationships", 'INFO')
bUtils.toggleLayer(defs.layerTypes['link'], True)
for l in model['links']:
parent = model['links'][l]
for c in parent['children']:
child = model['links'][c]
child['object'].matrix_world = parent['object'].matrix_world
sUtils.selectObjects([child['object'], parent['object']], True, 1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
log("Creating joints...", 'INFO')
for j in model['joints']:
joint = model['joints'][j]
jointmodel.createJoint(joint)
log('...finished.', 'INFO')
# set transformations
log("Placing links...", 'INFO')
for l in model['links']:
if 'parent' not in model['links'][l]:
root = model['links'][l]
break
linkmodel.placeChildLinks(model, root)
log("Assigning model name...", 'INFO')
try:
rootlink = sUtils.getRoot(bpy.data.objects[root['name']])
rootlink['modelname'] = model['name']
rootlink.location = (0, 0, 0)
except (KeyError, NameError):
log("Could not assign model name to root link.", "ERROR")
try:
log("Creating sensors...", 'INFO')
for s in model['sensors']:
sensormodel.createSensor(model['sensors'][s])
except KeyError:
log("No sensors in model " + model['name'], 'INFO')
try:
log("Creating motors...", 'INFO')
for m in model['motors']:
eUtils.addDictionaryToObj(
model['motors'][m],
model['joints'][model['motors'][m]['joint']],
category='motor')
except KeyError:
log("No motors in model " + model['name'], 'INFO')
try:
log("Creating controllers...", 'INFO')
for c in model['controllers']:
controllermodel.createController(model['controllers'][c])
except KeyError:
log("No controllers in model " + model['name'], 'INFO')
try:
log("Creating groups...", 'INFO')
for g in model['groups']:
createGroup(model['groups'][g])
except KeyError:
log("No kinematic groups in model " + model['name'], 'INFO')
try:
log("Creating chains...", 'INFO')
for ch in model['chains']:
createChain(model['chains'][ch])
except KeyError:
log("No kinematic chains in model " + model['name'], 'INFO')
try:
log("Creating lights...", 'INFO')
for l in model['lights']:
lightmodel.createLight(model['lights'][l])
except KeyError:
log("No lights in model " + model['name'], 'INFO')
# display all objects after import
for obj in bpy.data.objects:
bUtils.setObjectLayersActive(obj)
bpy.ops.object.mode_set(mode='OBJECT')
bpy.ops.object.select_all(action='SELECT')
bpy.ops.view3d.view_selected()
# update transformations
bUtils.update()
def createMotor(motor, parentobj, origin=mathutils.Matrix(), addcontrollers=False):
"""This function creates a new motor specified by its parameters.
If *addcontrollers* is set, a controller object will be created from the controller definition
which is specified in the motor dictionary (key *controller*).
Args:
motor(dict): phobos representation of the new motor.
parentobj(bpy_types.Object): object to parent new motor to
origin(mathutils.Matrix, optional): new motors origin (Default value = mathutils.Matrix())
addcontrollers(bool, optional): whether to add the defined controller as object (Default value = False)
Returns:
bpy.types.Object: new motor object or a list of the new motor_object and the new controller object
"""
layers = defs.layerTypes['motor']
bUtils.toggleLayer(layers, value=True)
# create motor object
if motor['shape'].startswith('resource'):
newmotor = bUtils.createPrimitive(
motor['name'],
'box',
[1, 1, 1],
layers,
plocation=origin.to_translation(),
protation=origin.to_euler(),
pmaterial=motor['material'],
phobostype='motor',
)
# use resource name provided as: "resource:whatever_name"
resource_obj = ioUtils.getResource(['motor'] + motor['shape'].split('://')[1].split('_'))
if resource_obj:
log("Assigned resource mesh and materials to new motor object.", 'DEBUG')
newmotor.data = resource_obj.data
newmotor.scale = (motor['size'],) * 3
else:
log("Could not use resource mesh for motor. Default cube used instead.", 'WARNING')
else:
newmotor = bUtils.createPrimitive(
motor['name'],
motor['shape'],
motor['size'],
layers,
plocation=origin.to_translation(),
protation=origin.to_euler(),
pmaterial=motor['material'],
phobostype='motor',
)
# assign the parent if available
if parentobj is not None:
sUtils.selectObjects([newmotor, parentobj], clear=True, active=1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
# set motor properties
newmotor.phobostype = 'motor'
newmotor.name = motor['name']
defname = motor['defname']
# write the custom properties to the motor
eUtils.addAnnotation(newmotor, motor['props'], namespace='motor', ignore=['defname'])
if 'controller' in defs.definitions['motors'][defname] and addcontrollers:
import phobos.model.controllers as controllermodel
motorcontroller = defs.definitions['motors'][defname]['controller']
controllerdefs = ioUtils.getDictFromYamlDefs(
'controller', motorcontroller, newmotor.name + '_controller'
)
newcontroller = controllermodel.createController(
controllerdefs, newmotor, origin=newmotor.matrix_world, annotations='all'
)
else:
newcontroller = None
# select the new motor
sUtils.selectObjects(
[newmotor] if not newcontroller else [newmotor, newcontroller], clear=True, active=0
)
return newmotor if not newcontroller else [newmotor, newcontroller]
def createSensor(sensor, reference, origin=mathutils.Matrix()):
"""This function creates a new sensor specified by its parameters.
:param sensor: The phobos representation of the new sensor.
:type sensor: dict
:param reference: This is an object to add a parent relationship to.
:type reference: bpy_types.Object
:param origin: The new sensors origin.
:type origin: mathutils.Matrix
:return: The newly created sensor object
"""
blenderUtils.toggleLayer(defs.layerTypes['sensor'], value=True)
# create sensor object
if 'Camera' in sensor['type']:
bpy.context.scene.layers[defs.layerTypes['sensor']] = True
bpy.ops.object.add(type='CAMERA', location=origin.to_translation(),
rotation=origin.to_euler(),
layers=blenderUtils.defLayers([defs.layerTypes['sensor']]))
newsensor = bpy.context.active_object
if reference is not None:
selectionUtils.selectObjects([newsensor, bpy.data.objects[reference]], clear=True, active=1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
elif sensor['type'] in ['RaySensor', 'RotatingRaySensor', 'ScanningSonar', 'MultiLevelLaserRangeFinder']:
# TODO: create a proper ray sensor scanning layer disc here
newsensor = blenderUtils.createPrimitive(sensor['name'], 'disc', (0.5, 36),
defs.layerTypes['sensor'], 'phobos_laserscanner',
origin.to_translation(), protation=origin.to_euler())
if reference is not None and reference != []:
if type(reference) == str:
key = reference
else:
key = reference[0]
selectionUtils.selectObjects([newsensor, bpy.data.objects[key]], clear=True, active=1)
bpy.ops.object.parent_set(type='BONE_RELATIVE')
else: # contact, force and torque sensors (or unknown sensors)
newsensor = blenderUtils.createPrimitive(sensor['name'], 'sphere', 0.05,
defs.layerTypes['sensor'], 'phobos_sensor',
origin.to_translation(), protation=origin.to_euler())
if 'Node' in sensor['type']:
newsensor['sensor/nodes'] = sorted(reference)
elif 'Joint' in sensor['type'] or 'Motor' in sensor['type']:
newsensor['sensor/joints'] = sorted(reference)
# set sensor properties
newsensor.phobostype = 'sensor'
newsensor.name = sensor['name']
newsensor['sensor/type'] = sensor['type']
#for prop in ['link', 'joint', 'links', 'joints', 'motors']:
# if prop in sensor:
# newsensor['sensor/'+prop] = sensor[prop]
# add custom properties
#for prop in sensor:
# if prop.startswith('$'):
# for tag in sensor[prop]:
# newsensor[prop[1:]+'/'+tag] = sensor[prop][tag]
# throw warning if type is not known
if sensor['type'] not in defs.sensortypes:
print("### Warning: sensor", sensor['name'], "is of unknown/custom type.")
selectionUtils.selectObjects([newsensor], clear=False, active=0)
return newsensor