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